diff --git a/surface/include/pcl/surface/impl/texture_mapping.hpp b/surface/include/pcl/surface/impl/texture_mapping.hpp index eb5424395a6..6d8dfff91f1 100644 --- a/surface/include/pcl/surface/impl/texture_mapping.hpp +++ b/surface/include/pcl/surface/impl/texture_mapping.hpp @@ -1042,12 +1042,29 @@ pcl::TextureMapping::getPointUVCoordinates(const pcl::PointXYZ &pt, co // compute image center and dimension double sizeX = cam.width; double sizeY = cam.height; - double cx = sizeX / 2.0; - double cy = sizeY / 2.0; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = sizeX / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = sizeY / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h > 0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; // project point on camera's image plane - UV_coordinates.x = static_cast ((cam.focal_length * (pt.x / pt.z) + cx) / sizeX); //horizontal - UV_coordinates.y = 1.0f - static_cast ((cam.focal_length * (pt.y / pt.z) + cy) / sizeY); //vertical + UV_coordinates.x = static_cast ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal + UV_coordinates.y = 1.0f - static_cast ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical // point is visible! if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0) diff --git a/surface/include/pcl/surface/texture_mapping.h b/surface/include/pcl/surface/texture_mapping.h index e706326a264..026dd9d1152 100644 --- a/surface/include/pcl/surface/texture_mapping.h +++ b/surface/include/pcl/surface/texture_mapping.h @@ -50,12 +50,26 @@ namespace pcl namespace texture_mapping { - /** \brief Structure to store camera pose and focal length. */ + /** \brief Structure to store camera pose and focal length. + * + * One can assign a value to focal_length, to be used along + * both camera axes or, optionally, axis-specific values + * (focal_length_w and focal_length_h). Optionally, one can + * also specify center-of-focus using parameters + * center_w and center_h. If the center-of-focus is not + * specified, it will be set to the geometric center of + * the camera, as defined by the width and height parameters. + */ struct Camera { - Camera () : pose (), focal_length (), height (), width (), texture_file () {} + Camera () : pose (), focal_length (), focal_length_w (-1), focal_length_h (-1), + center_w (-1), center_h (-1), height (), width (), texture_file () {} Eigen::Affine3f pose; double focal_length; + double focal_length_w; // optional + double focal_length_h; // optinoal + double center_w; // optional + double center_h; // optional double height; double width; std::string texture_file; @@ -187,11 +201,25 @@ namespace pcl // compute image center and dimension double sizeX = cam.width; double sizeY = cam.height; - double cx = (sizeX) / 2.0; - double cy = (sizeY) / 2.0; - - double focal_x = cam.focal_length; - double focal_y = cam.focal_length; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = (sizeX) / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = (sizeY) / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h>0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; // project point on image frame UV_coordinates[0] = static_cast ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal