From a37cabd18e9bf28e1e385ed7ce40298b0d4b43d7 Mon Sep 17 00:00:00 2001 From: duongtd Date: Wed, 24 Jun 2026 17:45:43 +0700 Subject: [PATCH] first commit --- CMakeLists.txt | 200 ++++++ .../pinhole_camera_model.h | 365 ++++++++++ package.xml | 41 ++ src/pinhole_camera_model.cpp | 661 ++++++++++++++++++ 4 files changed, 1267 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 include/robot_image_geometry/pinhole_camera_model.h create mode 100644 package.xml create mode 100644 src/pinhole_camera_model.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..802cb78 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,200 @@ +cmake_minimum_required(VERSION 3.0.2) +project(robot_image_geometry VERSION 1.0.0 LANGUAGES CXX) + +# ======================================================== +# Detect build mode +# ======================================================== +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building robot_image_geometry with Catkin") +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building robot_image_geometry with Standalone CMake") +endif() + +# ======================================================== +# C++ Standard +# ======================================================== +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +add_compile_options( + -Wall + -Wextra + -Wpedantic +) + +# ======================================================== +# Common dependencies +# ======================================================== +find_package(OpenCV REQUIRED) + +# ======================================================== +# Standalone mode +# ======================================================== +if(NOT BUILDING_WITH_CATKIN) + + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + set(PACKAGES_DIR + robot_sensor_msgs + ) + + include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ) + +# ======================================================== +# Catkin mode +# ======================================================== +else() + + find_package(catkin REQUIRED COMPONENTS + robot_sensor_msgs + ) + + catkin_package( + INCLUDE_DIRS + include + + LIBRARIES + ${PROJECT_NAME} + + CATKIN_DEPENDS + robot_sensor_msgs + + DEPENDS + OpenCV + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ) + +endif() + +# ======================================================== +# Library +# ======================================================== +add_library(${PROJECT_NAME} SHARED + + src/pinhole_camera_model.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +# ======================================================== +# Link +# ======================================================== +if(BUILDING_WITH_CATKIN) + + add_dependencies( + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} + ) + + target_link_libraries(${PROJECT_NAME} + + PUBLIC + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + ) + +else() + + target_link_libraries(${PROJECT_NAME} + + PUBLIC + ${PACKAGES_DIR} + ${OpenCV_LIBRARIES} + ) + + set_target_properties(${PROJECT_NAME} + PROPERTIES + + LIBRARY_OUTPUT_DIRECTORY + ${CMAKE_BINARY_DIR} + + BUILD_RPATH + "${CMAKE_BINARY_DIR}" + + INSTALL_RPATH + "${CMAKE_INSTALL_PREFIX}/lib" + ) + +endif() + +# ======================================================== +# Install +# ======================================================== +if(BUILDING_WITH_CATKIN) + + install( + TARGETS ${PROJECT_NAME} + + ARCHIVE DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + + LIBRARY DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + + RUNTIME DESTINATION + ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) + +else() + + install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + install( + EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) + +endif() + +# ======================================================== +# Tests +# ======================================================== +if(BUILDING_WITH_CATKIN) + + if(CATKIN_ENABLE_TESTING AND EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test) + add_subdirectory(test) + endif() + +endif() \ No newline at end of file diff --git a/include/robot_image_geometry/pinhole_camera_model.h b/include/robot_image_geometry/pinhole_camera_model.h new file mode 100644 index 0000000..d3e983f --- /dev/null +++ b/include/robot_image_geometry/pinhole_camera_model.h @@ -0,0 +1,365 @@ +#ifndef IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H +#define IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H + +#include +#include +#include +#include +#include +#include +#include + +namespace image_geometry { + +class Exception : public std::runtime_error +{ +public: + Exception(const std::string& description) : std::runtime_error(description) {} +}; + +/** + * \brief Simplifies interpreting images geometrically using the parameters from + * robot_sensor_msgs/CameraInfo. + */ +class PinholeCameraModel +{ +public: + + PinholeCameraModel(); + + PinholeCameraModel(const PinholeCameraModel& other); + + PinholeCameraModel& operator=(const PinholeCameraModel& other); + + /** + * \brief Set the camera parameters from the robot_sensor_msgs/CameraInfo message. + */ + bool fromCameraInfo(const robot_sensor_msgs::CameraInfo& msg); + + /** + * \brief Set the camera parameters from the robot_sensor_msgs/CameraInfo message. + */ + bool fromCameraInfo(const robot_sensor_msgs::CameraInfoConstPtr& msg); + + /** + * \brief Get the name of the camera coordinate frame in tf. + */ + std::string tfFrame() const; + + /** + * \brief Get the time stamp associated with this camera model. + */ + robot::Time stamp() const; + + /** + * \brief The resolution at which the camera was calibrated. + * + * The maximum resolution at which the camera can be used with the current + * calibration; normally this is the same as the imager resolution. + */ + cv::Size fullResolution() const; + + /** + * \brief The resolution of the current rectified image. + * + * The size of the rectified image associated with the latest CameraInfo, as + * reduced by binning/ROI and affected by distortion. If binning and ROI are + * not in use, this is the same as fullResolution(). + */ + cv::Size reducedResolution() const; + + cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const; + + cv::Rect toFullResolution(const cv::Rect& roi_reduced) const; + + cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const; + + cv::Rect toReducedResolution(const cv::Rect& roi_full) const; + + /** + * \brief The current raw ROI, as used for capture by the camera driver. + */ + cv::Rect rawRoi() const; + + /** + * \brief The current rectified ROI, which best fits the raw ROI. + */ + cv::Rect rectifiedRoi() const; + + /** + * \brief Project a 3d point to rectified pixel coordinates. + * + * This is the inverse of projectPixelTo3dRay(). + * + * \param xyz 3d point in the camera coordinate frame + * \return (u,v) in rectified pixel coordinates + */ + cv::Point2d project3dToPixel(const cv::Point3d& xyz) const; + + /** + * \brief Project a rectified pixel to a 3d ray. + * + * Returns the unit vector in the camera coordinate frame in the direction of rectified + * pixel (u,v) in the image plane. This is the inverse of project3dToPixel(). + * + * In 1.4.x, the vector has z = 1.0. Previously, this function returned a unit vector. + * + * \param uv_rect Rectified pixel coordinates + * \return 3d ray passing through (u,v) + */ + cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const; + cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const; + + /** + * \brief Rectify a raw camera image. + */ + void rectifyImage(const cv::Mat& raw, cv::Mat& rectified, + int interpolation = cv::INTER_LINEAR) const; + + /** + * \brief Apply camera distortion to a rectified image. + */ + void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, + int interpolation = cv::INTER_LINEAR) const; + + /** + * \brief Compute the rectified image coordinates of a pixel in the raw image. + */ + cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const; + cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const; + + /** + * \brief Compute the raw image coordinates of a pixel in the rectified image. + */ + cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const; + cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const; + + /** + * \brief Compute the rectified ROI best fitting a raw ROI. + */ + cv::Rect rectifyRoi(const cv::Rect& roi_raw) const; + + /** + * \brief Compute the raw ROI best fitting a rectified ROI. + */ + cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const; + + /** + * \brief Returns the camera info message held internally + */ + const robot_sensor_msgs::CameraInfo& cameraInfo() const; + + /** + * \brief Returns the original camera matrix. + */ + const cv::Matx33d& intrinsicMatrix() const; + + /** + * \brief Returns the distortion coefficients. + */ + const cv::Mat_& distortionCoeffs() const; + + /** + * \brief Returns the rotation matrix. + */ + const cv::Matx33d& rotationMatrix() const; + + /** + * \brief Returns the projection matrix. + */ + const cv::Matx34d& projectionMatrix() const; + + /** + * \brief Returns the original camera matrix for full resolution. + */ + const cv::Matx33d& fullIntrinsicMatrix() const; + + /** + * \brief Returns the projection matrix for full resolution. + */ + const cv::Matx34d& fullProjectionMatrix() const; + + /** + * \brief Returns the focal length (pixels) in x direction of the rectified image. + */ + double fx() const; + + /** + * \brief Returns the focal length (pixels) in y direction of the rectified image. + */ + double fy() const; + + /** + * \brief Returns the x coordinate of the optical center. + */ + double cx() const; + + /** + * \brief Returns the y coordinate of the optical center. + */ + double cy() const; + + /** + * \brief Returns the x-translation term of the projection matrix. + */ + double Tx() const; + + /** + * \brief Returns the y-translation term of the projection matrix. + */ + double Ty() const; + + /** + * \brief Returns the horizontal field of view in radians. + */ + double fovX() const; + + /** + * \brief Returns the vertical field of view in radians. + */ + double fovY() const; + + /** + * \brief Returns the number of columns in each bin. + */ + uint32_t binningX() const; + + /** + * \brief Returns the number of rows in each bin. + */ + uint32_t binningY() const; + + /** + * \brief Compute delta u, given Z and delta X in Cartesian space. + * + * For given Z, this is the inverse of getDeltaX(). + * + * \param deltaX Delta X, in Cartesian space + * \param Z Z (depth), in Cartesian space + */ + double getDeltaU(double deltaX, double Z) const; + + /** + * \brief Compute delta v, given Z and delta Y in Cartesian space. + * + * For given Z, this is the inverse of getDeltaY(). + * + * \param deltaY Delta Y, in Cartesian space + * \param Z Z (depth), in Cartesian space + */ + double getDeltaV(double deltaY, double Z) const; + + /** + * \brief Compute delta X, given Z in Cartesian space and delta u in pixels. + * + * For given Z, this is the inverse of getDeltaU(). + * + * \param deltaU Delta u, in pixels + * \param Z Z (depth), in Cartesian space + */ + double getDeltaX(double deltaU, double Z) const; + + /** + * \brief Compute delta Y, given Z in Cartesian space and delta v in pixels. + * + * For given Z, this is the inverse of getDeltaV(). + * + * \param deltaV Delta v, in pixels + * \param Z Z (depth), in Cartesian space + */ + double getDeltaY(double deltaV, double Z) const; + + /** + * \brief Returns true if the camera has been initialized + */ + bool initialized() const { return (bool)cache_; } + +protected: + robot_sensor_msgs::CameraInfo cam_info_; + cv::Mat_ D_; // Unaffected by binning, ROI + cv::Matx33d R_; // Unaffected by binning, ROI + cv::Matx33d K_; // Describe current image (includes binning, ROI) + cv::Matx34d P_; // Describe current image (includes binning, ROI) + cv::Matx33d K_full_; // Describe full-res image, needed for full maps + cv::Matx34d P_full_; // Describe full-res image, needed for full maps + + // Use PIMPL here so we can change internals in patch updates if needed + struct Cache; +#ifdef BOOST_SHARED_PTR_HPP_INCLUDED + boost::shared_ptr cache_; // Holds cached data for internal use +#else + std::shared_ptr cache_; // Holds cached data for internal use +#endif + + void initRectificationMaps() const; + void initUnrectificationMaps() const; + + friend class StereoCameraModel; +}; + + +/* Trivial inline functions */ +inline std::string PinholeCameraModel::tfFrame() const +{ + assert( initialized() ); + return cam_info_.header.frame_id; +} + +inline robot::Time PinholeCameraModel::stamp() const +{ + assert( initialized() ); + return cam_info_.header.stamp; +} + +inline const robot_sensor_msgs::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; } +inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; } +inline const cv::Mat_& PinholeCameraModel::distortionCoeffs() const { return D_; } +inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; } +inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; } +inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; } +inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; } + +inline double PinholeCameraModel::fx() const { return P_(0,0); } +inline double PinholeCameraModel::fy() const { return P_(1,1); } +inline double PinholeCameraModel::cx() const { return P_(0,2); } +inline double PinholeCameraModel::cy() const { return P_(1,2); } +inline double PinholeCameraModel::Tx() const { return P_(0,3); } +inline double PinholeCameraModel::Ty() const { return P_(1,3); } + +inline double PinholeCameraModel::fovX() const { + return 2 * atan(rawRoi().width / (2 * fx())); +} +inline double PinholeCameraModel::fovY() const { + return 2 * atan(rawRoi().height / (2 * fy())); +} + +inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; } +inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; } + +inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const +{ + assert( initialized() ); + return fx() * deltaX / Z; +} + +inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const +{ + assert( initialized() ); + return fy() * deltaY / Z; +} + +inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const +{ + assert( initialized() ); + return Z * deltaU / fx(); +} + +inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const +{ + assert( initialized() ); + return Z * deltaV / fy(); +} + +} //namespace image_geometry + +#endif diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..97f95b4 --- /dev/null +++ b/package.xml @@ -0,0 +1,41 @@ + + robot_image_geometry + 0.7.10 + + robot_image_geometry is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. robot_image_geometry + maintains the relationship between coordinate frames in a tree + structure buffered in time, and lets the user transform points, + vectors, etc between any two coordinate frames at any desired + point in time. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/robot_image_geometry + + catkin + + libconsole-bridge-dev + libconsole-bridge-dev + + + libopencv-dev + libopencv-dev + + + robot_sensor_msgs + robot_sensor_msgs + + + + + + + diff --git a/src/pinhole_camera_model.cpp b/src/pinhole_camera_model.cpp new file mode 100644 index 0000000..a023728 --- /dev/null +++ b/src/pinhole_camera_model.cpp @@ -0,0 +1,661 @@ +#include +#include +#include +#ifdef BOOST_SHARED_PTR_HPP_INCLUDED +#include +#endif + +namespace image_geometry { + +enum DistortionState { NONE, CALIBRATED, UNKNOWN }; +enum DistortionModel { EQUIDISTANT, PLUMB_BOB_OR_RATIONAL_POLYNOMIAL, UNKNOWN_MODEL }; + +struct PinholeCameraModel::Cache +{ + DistortionState distortion_state; + DistortionModel distortion_model; + + cv::Mat_ K_binned, P_binned; // Binning applied, but not cropping + + mutable bool full_maps_dirty; + mutable cv::Mat full_map1, full_map2; + + mutable bool reduced_maps_dirty; + mutable cv::Mat reduced_map1, reduced_map2; + + mutable bool unrectify_full_maps_dirty; + mutable cv::Mat unrectify_full_map1, unrectify_full_map2; + + mutable bool unrectify_reduced_maps_dirty; + mutable cv::Mat unrectify_reduced_map1, unrectify_reduced_map2; + + mutable bool rectified_roi_dirty; + mutable cv::Rect rectified_roi; + + Cache() + : distortion_state(UNKNOWN), + distortion_model(UNKNOWN_MODEL), + full_maps_dirty(true), + reduced_maps_dirty(true), + unrectify_full_maps_dirty(true), + unrectify_reduced_maps_dirty(true), + rectified_roi_dirty(true) + { + } +}; + +PinholeCameraModel::PinholeCameraModel() +{ +} + +PinholeCameraModel& PinholeCameraModel::operator=(const PinholeCameraModel& other) +{ + if (other.initialized()) + this->fromCameraInfo(other.cameraInfo()); + return *this; +} + +PinholeCameraModel::PinholeCameraModel(const PinholeCameraModel& other) +{ + if (other.initialized()) + fromCameraInfo(other.cam_info_); +} + +// For uint32_t, string, bool... +template +bool update(const T& new_val, T& my_val) +{ + if (my_val == new_val) + return false; + my_val = new_val; + return true; +} + +// For std::vector +template +bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_& cv_mat, int rows, int cols) +{ + if ((my_mat == new_mat) && (my_mat.size() == static_cast(cv_mat.rows * cv_mat.cols))) + return false; + my_mat = new_mat; + // D may be empty if camera is uncalibrated or distortion model is non-standard + cv_mat = (my_mat.size() == 0) ? cv::Mat_() : cv::Mat_(rows, cols, &my_mat[0]); + return true; +} + +template +bool updateMat(const MatT& new_mat, MatT& my_mat, MatU& cv_mat) +{ + if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols)) + return false; + my_mat = new_mat; + // D may be empty if camera is uncalibrated or distortion model is non-standard + cv_mat = MatU(&my_mat[0]); + return true; +} + +bool PinholeCameraModel::fromCameraInfo(const robot_sensor_msgs::CameraInfo& msg) +{ + // Create our repository of cached data (rectification maps, etc.) + if (!cache_) +#ifdef BOOST_SHARED_PTR_HPP_INCLUDED + cache_ = boost::make_shared(); +#else + cache_ = std::make_shared(); +#endif + + // Binning = 0 is considered the same as binning = 1 (no binning). + uint32_t binning_x = msg.binning_x ? msg.binning_x : 1; + uint32_t binning_y = msg.binning_y ? msg.binning_y : 1; + + // ROI all zeros is considered the same as full resolution. + robot_sensor_msgs::RegionOfInterest roi = msg.roi; + if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) { + roi.width = msg.width; + roi.height = msg.height; + } + + // Update time stamp (and frame_id if that changes for some reason) + cam_info_.header = msg.header; + + // Update any parameters that have changed. The full rectification maps are + // invalidated by any change in the calibration parameters OR binning. + bool full_dirty = false; + full_dirty |= update(msg.height, cam_info_.height); + full_dirty |= update(msg.width, cam_info_.width); + full_dirty |= update(msg.distortion_model, cam_info_.distortion_model); + full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size()); + full_dirty |= updateMat(msg.K, cam_info_.K, K_full_); + full_dirty |= updateMat(msg.R, cam_info_.R, R_); + full_dirty |= updateMat(msg.P, cam_info_.P, P_full_); + full_dirty |= update(binning_x, cam_info_.binning_x); + full_dirty |= update(binning_y, cam_info_.binning_y); + cache_->full_maps_dirty |= full_dirty; + cache_->unrectify_full_maps_dirty |= full_dirty; + + // The reduced rectification maps are invalidated by any of the above or a + // change in ROI. + bool reduced_dirty = full_dirty; + reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset); + reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset); + reduced_dirty |= update(roi.height, cam_info_.roi.height); + reduced_dirty |= update(roi.width, cam_info_.roi.width); + reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify); + cache_->reduced_maps_dirty |= reduced_dirty; + cache_->reduced_maps_dirty |= cache_->full_maps_dirty; + cache_->unrectify_reduced_maps_dirty |= reduced_dirty; + cache_->unrectify_reduced_maps_dirty |= cache_->unrectify_full_maps_dirty; + + // As is the rectified ROI + cache_->rectified_roi_dirty |= reduced_dirty; + + // Figure out how to handle the distortion + if (cam_info_.distortion_model == robot_sensor_msgs::distortion_models::PLUMB_BOB || + cam_info_.distortion_model == robot_sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL || + cam_info_.distortion_model == robot_sensor_msgs::distortion_models::EQUIDISTANT) { + // If any distortion coefficient is non-zero, then need to apply the distortion + cache_->distortion_state = NONE; + for (size_t i = 0; i < cam_info_.D.size(); ++i) + { + if (cam_info_.D[i] != 0) + { + cache_->distortion_state = CALIBRATED; + break; + } + } + } + else + cache_->distortion_state = UNKNOWN; + + // Get the distortion model, if supported + if (cam_info_.distortion_model == robot_sensor_msgs::distortion_models::PLUMB_BOB || + cam_info_.distortion_model == robot_sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) { + cache_->distortion_model = PLUMB_BOB_OR_RATIONAL_POLYNOMIAL; + } + else if(cam_info_.distortion_model == robot_sensor_msgs::distortion_models::EQUIDISTANT) { + cache_->distortion_model = EQUIDISTANT; + } + else + cache_->distortion_model = UNKNOWN_MODEL; + + // If necessary, create new K_ and P_ adjusted for binning and ROI + /// @todo Calculate and use rectified ROI + bool adjust_binning = (binning_x > 1) || (binning_y > 1); + bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0); + + if (!adjust_binning && !adjust_roi) { + K_ = K_full_; + P_ = P_full_; + } + else { + K_ = K_full_; + P_ = P_full_; + + // ROI is in full image coordinates, so change it first + if (adjust_roi) { + // Move principal point by the offset + /// @todo Adjust P by rectified ROI instead + K_(0,2) -= roi.x_offset; + K_(1,2) -= roi.y_offset; + P_(0,2) -= roi.x_offset; + P_(1,2) -= roi.y_offset; + } + + if (binning_x > 1) { + double scale_x = 1.0 / binning_x; + K_(0,0) *= scale_x; + K_(0,2) *= scale_x; + P_(0,0) *= scale_x; + P_(0,2) *= scale_x; + P_(0,3) *= scale_x; + } + if (binning_y > 1) { + double scale_y = 1.0 / binning_y; + K_(1,1) *= scale_y; + K_(1,2) *= scale_y; + P_(1,1) *= scale_y; + P_(1,2) *= scale_y; + P_(1,3) *= scale_y; + } + } + + return reduced_dirty; +} + +bool PinholeCameraModel::fromCameraInfo(const robot_sensor_msgs::CameraInfoConstPtr& msg) +{ + return fromCameraInfo(*msg); +} + +cv::Size PinholeCameraModel::fullResolution() const +{ + assert( initialized() ); + return cv::Size(cam_info_.width, cam_info_.height); +} + +cv::Size PinholeCameraModel::reducedResolution() const +{ + assert( initialized() ); + + cv::Rect roi = rectifiedRoi(); + return cv::Size(roi.width / binningX(), roi.height / binningY()); +} + +cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Point2d(uv_reduced.x * binningX() + roi.x, + uv_reduced.y * binningY() + roi.y); +} + +cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Rect(roi_reduced.x * binningX() + roi.x, + roi_reduced.y * binningY() + roi.y, + roi_reduced.width * binningX(), + roi_reduced.height * binningY()); +} + +cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Point2d((uv_full.x - roi.x) / binningX(), + (uv_full.y - roi.y) / binningY()); +} + +cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const +{ + cv::Rect roi = rectifiedRoi(); + return cv::Rect((roi_full.x - roi.x) / binningX(), + (roi_full.y - roi.y) / binningY(), + roi_full.width / binningX(), + roi_full.height / binningY()); +} + +cv::Rect PinholeCameraModel::rawRoi() const +{ + assert( initialized() ); + + return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset, + cam_info_.roi.width, cam_info_.roi.height); +} + +cv::Rect PinholeCameraModel::rectifiedRoi() const +{ + assert( initialized() ); + + if (cache_->rectified_roi_dirty) + { + if (!cam_info_.roi.do_rectify) + cache_->rectified_roi = rawRoi(); + else + cache_->rectified_roi = rectifyRoi(rawRoi()); + cache_->rectified_roi_dirty = false; + } + return cache_->rectified_roi; +} + +cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const +{ + assert( initialized() ); + assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane + + // [U V W]^T = P * [X Y Z 1]^T + // u = U/W + // v = V/W + cv::Point2d uv_rect; + uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx(); + uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy(); + return uv_rect; +} + +cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const +{ + return projectPixelTo3dRay(uv_rect, P_); +} + +cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const +{ + assert( initialized() ); + + const double& fx = P(0,0); + const double& fy = P(1,1); + const double& cx = P(0,2); + const double& cy = P(1,2); + const double& Tx = P(0,3); + const double& Ty = P(1,3); + + cv::Point3d ray; + ray.x = (uv_rect.x - cx - Tx) / fx; + ray.y = (uv_rect.y - cy - Ty) / fy; + ray.z = 1.0; + return ray; +} + +void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const +{ + assert( initialized() ); + + switch (cache_->distortion_state) { + case NONE: + raw.copyTo(rectified); + break; + case CALIBRATED: + initRectificationMaps(); + if (raw.depth() == CV_32F || raw.depth() == CV_64F) + { + cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits::quiet_NaN()); + } + else { + cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation); + } + break; + default: + assert(cache_->distortion_state == UNKNOWN); + throw Exception("Cannot call rectifyImage when distortion is unknown."); + } +} + +void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const +{ + assert( initialized() ); + + switch (cache_->distortion_state) { + case NONE: + rectified.copyTo(raw); + break; + case CALIBRATED: + initUnrectificationMaps(); + if (rectified.depth() == CV_32F || rectified.depth() == CV_64F) + { + cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits::quiet_NaN()); + } + else { + cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation); + } + break; + default: + assert(cache_->distortion_state == UNKNOWN); + throw Exception("Cannot call rectifyImage when distortion is unknown."); + } +} + +cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const +{ + return rectifyPoint(uv_raw, K_, P_); +} + +cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const +{ + assert( initialized() ); + + if (cache_->distortion_state == NONE) + return uv_raw; + if (cache_->distortion_state == UNKNOWN) + throw Exception("Cannot call rectifyPoint when distortion is unknown."); + assert(cache_->distortion_state == CALIBRATED); + + /// @todo cv::undistortPoints requires the point data to be float, should allow double + cv::Point2f raw32 = uv_raw, rect32; + const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x); + cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x); + + switch (cache_->distortion_model) { + case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL: + cv::undistortPoints(src_pt, dst_pt, K, D_, R_, P); + break; + case EQUIDISTANT: + cv::fisheye::undistortPoints(src_pt, dst_pt, K, D_, R_, P); + break; + default: + assert(cache_->distortion_model == UNKNOWN_MODEL); + throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT."); + } + return rect32; +} + +cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const +{ + return unrectifyPoint(uv_rect, K_, P_); +} + +cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const +{ + assert( initialized() ); + + if (cache_->distortion_state == NONE) + return uv_rect; + if (cache_->distortion_state == UNKNOWN) + throw Exception("Cannot call unrectifyPoint when distortion is unknown."); + assert(cache_->distortion_state == CALIBRATED); + + // Convert to a ray + cv::Point3d ray = projectPixelTo3dRay(uv_rect, P); + + // Project the ray on the image + cv::Mat r_vec, t_vec = cv::Mat_::zeros(3, 1); + cv::Rodrigues(R_.t(), r_vec); + std::vector image_point; + + switch (cache_->distortion_model) { + case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL: + cv::projectPoints(std::vector(1, ray), r_vec, t_vec, K, D_, image_point); + break; + case EQUIDISTANT: + cv::fisheye::projectPoints(std::vector(1, ray), image_point, r_vec, t_vec, K, D_); + break; + default: + assert(cache_->distortion_model == UNKNOWN_MODEL); + throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT."); + } + + return image_point[0]; +} + +cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const +{ + assert( initialized() ); + + /// @todo Actually implement "best fit" as described by REP 104. + + // For now, just unrectify the four corners and take the bounding box. + // Since ROI is specified in unbinned coordinates (see REP-104), this has to use K_full_ and P_full_. + cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y), K_full_, P_full_); + cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y), K_full_, P_full_); + cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, + roi_raw.y + roi_raw.height), K_full_, P_full_); + cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height), K_full_, P_full_); + + cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)), + std::ceil (std::min(rect_tl.y, rect_tr.y))); + cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)), + std::floor(std::max(rect_bl.y, rect_br.y))); + + return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y); +} + +cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const +{ + assert( initialized() ); + + /// @todo Actually implement "best fit" as described by REP 104. + + // For now, just unrectify the four corners and take the bounding box. + cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y)); + cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y)); + cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, + roi_rect.y + roi_rect.height)); + cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height)); + + cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)), + std::floor(std::min(raw_tl.y, raw_tr.y))); + cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)), + std::ceil (std::max(raw_bl.y, raw_br.y))); + + return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y); +} + +void PinholeCameraModel::initRectificationMaps() const +{ + /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary. + /// Make sure we're handling that 100% correctly. + + if (cache_->full_maps_dirty) { + // Create the full-size map at the binned resolution + /// @todo Should binned resolution, K, P be part of public API? + cv::Size binned_resolution = fullResolution(); + binned_resolution.width /= binningX(); + binned_resolution.height /= binningY(); + + cv::Matx33d K_binned; + cv::Matx34d P_binned; + if (binningX() == 1 && binningY() == 1) { + K_binned = K_full_; + P_binned = P_full_; + } + else { + K_binned = K_full_; + P_binned = P_full_; + if (binningX() > 1) { + double scale_x = 1.0 / binningX(); + K_binned(0,0) *= scale_x; + K_binned(0,2) *= scale_x; + P_binned(0,0) *= scale_x; + P_binned(0,2) *= scale_x; + P_binned(0,3) *= scale_x; + } + if (binningY() > 1) { + double scale_y = 1.0 / binningY(); + K_binned(1,1) *= scale_y; + K_binned(1,2) *= scale_y; + P_binned(1,1) *= scale_y; + P_binned(1,2) *= scale_y; + P_binned(1,3) *= scale_y; + } + } + + switch (cache_->distortion_model) { + case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL: + // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap) + cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution, + CV_16SC2, cache_->full_map1, cache_->full_map2); + break; + case EQUIDISTANT: + cv::fisheye::initUndistortRectifyMap(K_binned,D_, R_, P_binned, binned_resolution, + CV_16SC2, cache_->full_map1, cache_->full_map2); + break; + default: + assert(cache_->distortion_model == UNKNOWN_MODEL); + throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT."); + } + cache_->full_maps_dirty = false; + } + + if (cache_->reduced_maps_dirty) { + /// @todo Use rectified ROI + cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset, + cam_info_.roi.width, cam_info_.roi.height); + if (roi.x != 0 || roi.y != 0 || + (roi.height != 0 && roi.height != (int)cam_info_.height) || + (roi.width != 0 && roi.width != (int)cam_info_.width)) { + + // map1 contains integer (x,y) offsets, which we adjust by the ROI offset + // map2 contains LUT index for subpixel interpolation, which we can leave as-is + roi.x /= binningX(); + roi.y /= binningY(); + roi.width /= binningX(); + roi.height /= binningY(); + cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y); + cache_->reduced_map2 = cache_->full_map2(roi); + } + else { + // Otherwise we're rectifying the full image + cache_->reduced_map1 = cache_->full_map1; + cache_->reduced_map2 = cache_->full_map2; + } + cache_->reduced_maps_dirty = false; + } +} + +void PinholeCameraModel::initUnrectificationMaps() const +{ + /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary. + /// Make sure we're handling that 100% correctly. + + if (cache_->unrectify_full_maps_dirty) { + // Create the full-size map at the binned resolution + /// @todo Should binned resolution, K, P be part of public API? + cv::Size binned_resolution = fullResolution(); + binned_resolution.width /= binningX(); + binned_resolution.height /= binningY(); + + cv::Matx33d K_binned; + cv::Matx34d P_binned; + if (binningX() == 1 && binningY() == 1) { + K_binned = K_full_; + P_binned = P_full_; + } + else { + K_binned = K_full_; + P_binned = P_full_; + if (binningX() > 1) { + double scale_x = 1.0 / binningX(); + K_binned(0,0) *= scale_x; + K_binned(0,2) *= scale_x; + P_binned(0,0) *= scale_x; + P_binned(0,2) *= scale_x; + P_binned(0,3) *= scale_x; + } + if (binningY() > 1) { + double scale_y = 1.0 / binningY(); + K_binned(1,1) *= scale_y; + K_binned(1,2) *= scale_y; + P_binned(1,1) *= scale_y; + P_binned(1,2) *= scale_y; + P_binned(1,3) *= scale_y; + } + } + + cv::Mat float_map_x(binned_resolution.height, binned_resolution.width, CV_32FC1); + cv::Mat float_map_y(binned_resolution.height, binned_resolution.width, CV_32FC1); + for (size_t x = 0; x < static_cast(binned_resolution.width); x++) { + for (size_t y = 0; y < static_cast(binned_resolution.height); y++) { + cv::Point2f uv_raw(x, y), uv_rect; + uv_rect = rectifyPoint(uv_raw, K_binned, P_binned); + float_map_x.at(y, x) = uv_rect.x; + float_map_y.at(y, x) = uv_rect.y; + } + } + // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap) + convertMaps(float_map_x, float_map_y, cache_->unrectify_full_map1, cache_->unrectify_full_map2, CV_16SC2); + cache_->unrectify_full_maps_dirty = false; + } + + if (cache_->unrectify_reduced_maps_dirty) { + /// @todo Use rectified ROI + cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset, + cam_info_.roi.width, cam_info_.roi.height); + if (roi.x != 0 || roi.y != 0 || + (roi.height != 0 && roi.height != (int)cam_info_.height) || + (roi.width != 0 && roi.width != (int)cam_info_.width)) { + + // map1 contains integer (x,y) offsets, which we adjust by the ROI offset + // map2 contains LUT index for subpixel interpolation, which we can leave as-is + roi.x /= binningX(); + roi.y /= binningY(); + roi.width /= binningX(); + roi.height /= binningY(); + cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1(roi) - cv::Scalar(roi.x, roi.y); + cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2(roi); + } + else { + // Otherwise we're rectifying the full image + cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1; + cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2; + } + cache_->unrectify_reduced_maps_dirty = false; + } +} + +} //namespace image_geometry