first commit

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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+# ========================================================
+# 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()

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 <robot_sensor_msgs/CameraInfo.h>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <stdexcept>
+#include <string>
+#include <math.h>
+#include <robot/time.h>
+
+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_<double>& 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_<double> 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> cache_; // Holds cached data for internal use
+#else
+  std::shared_ptr<Cache> 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_<double>& 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

package.xml

@@ -0,0 +1,41 @@
+<package>
+    <name>robot_image_geometry</name>
+    <version>0.7.10</version>
+    <description>
+        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.
+    </description>
+    <author>Tully Foote</author>
+    <author>Eitan Marder-Eppstein</author>
+    <author>Wim Meeussen</author>
+    <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+    <license>BSD</license>
+
+    <url type="website">http://www.ros.org/wiki/robot_image_geometry</url>
+        
+    <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+    <build_depend>libconsole-bridge-dev</build_depend>
+    <run_depend>libconsole-bridge-dev</run_depend>
+
+    <!-- OpenCV -->
+    <build_depend>libopencv-dev</build_depend>
+    <run_depend>libopencv-dev</run_depend>
+
+    <!-- Robot message package -->
+    <build_depend>robot_sensor_msgs</build_depend>
+    <run_depend>robot_sensor_msgs</run_depend>
+
+    <!-- Optional documentation dependencies -->
+    <!-- <doc_depend>dvipng</doc_depend>
+    <doc_depend>texlive-latex-extra</doc_depend> -->
+
+    <!-- <export>
+        <rosdoc config="rosdoc.yaml"/>
+    </export> -->
+</package>
+

src/pinhole_camera_model.cpp

@@ -0,0 +1,661 @@
+#include <robot_image_geometry/pinhole_camera_model.h>
+#include <robot_sensor_msgs/distortion_models.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#ifdef BOOST_SHARED_PTR_HPP_INCLUDED
+#include <boost/make_shared.hpp>
+#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_<double> 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<typename T>
+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<typename MatT>
+bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat, int rows, int cols)
+{
+  if ((my_mat == new_mat) && (my_mat.size() == static_cast<size_t>(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_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
+  return true;
+}
+
+template<typename MatT, typename MatU>
+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<Cache>();
+#else
+    cache_ = std::make_shared<Cache>();
+#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<float>::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<float>::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_<double>::zeros(3, 1);
+  cv::Rodrigues(R_.t(), r_vec);
+  std::vector<cv::Point2d> image_point;
+
+  switch (cache_->distortion_model) {
+    case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
+      cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K, D_, image_point);
+      break;
+    case EQUIDISTANT:
+      cv::fisheye::projectPoints(std::vector<cv::Point3d>(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<size_t>(binned_resolution.width); x++) {
+      for (size_t y = 0; y < static_cast<size_t>(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<float>(y, x) = uv_rect.x;
+        float_map_y.at<float>(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