Add equidistant distortion model (#358)

The equidistant distortion model is based on the following publication:

J. Kannala and S. Brandt (2006). A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 8, pp. 1335-1340

In OpenCV, this model is called the "fisheye" distortion model.

.travis.sh

@@ -22,7 +22,7 @@ function travis_time_end {
     set -x
 }
 
-apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg # for docker
+apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg build-essential # for docker
 DEBIAN_FRONTEND=noninteractive apt-get install -y tzdata # https://stackoverflow.com/questions/44331836/apt-get-install-tzdata-noninteractive
 
 travis_time_start setup.before_install
@@ -33,7 +33,7 @@ sudo sh -c 'echo "deb http://repositories.ros.org/ubuntu/testing $(lsb_release -
 wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
 sudo apt-get update -qq || echo Ignore error on apt-get update
 # Install ROS
-sudo apt-get install -qq -y python3-catkin-pkg python3-catkin-tools python3-rosdep python3-wstool ros-$ROS_DISTRO-catkin
+sudo apt-get install -qq -y python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon python3-rosdep python3-wstool ros-$ROS_DISTRO-catkin
 source /opt/ros/$ROS_DISTRO/setup.bash
 # Setup for rosdep
 sudo rosdep init

image_geometry/src/pinhole_camera_model.cpp

@@ -8,10 +8,12 @@
 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
 
@@ -25,7 +27,9 @@ struct PinholeCameraModel::Cache
   mutable cv::Rect rectified_roi;
 
   Cache()
-    : full_maps_dirty(true),
+    : distortion_state(UNKNOWN),
+      distortion_model(UNKNOWN_MODEL),
+      full_maps_dirty(true),
       reduced_maps_dirty(true),
       rectified_roi_dirty(true)
   {
@@ -133,7 +137,8 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
 
   // Figure out how to handle the distortion
   if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
-      cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
+      cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL ||
+      cam_info_.distortion_model == 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)
@@ -148,6 +153,17 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
   else
     cache_->distortion_state = UNKNOWN;
 
+  // Get the distortion model, if supported
+  if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
+      cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
+    cache_->distortion_model = PLUMB_BOB_OR_RATIONAL_POLYNOMIAL;
+  }
+  else if(cam_info_.distortion_model == 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);
@@ -343,7 +359,18 @@ cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
   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);
-  cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);
+
+  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;
 }
 
@@ -364,7 +391,18 @@ cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
   cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
   cv::Rodrigues(R_.t(), r_vec);
   std::vector<cv::Point2d> image_point;
-  cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, 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];
 }
@@ -449,10 +487,21 @@ void PinholeCameraModel::initRectificationMaps() const
         P_binned(1,3) *= scale_y;
       }
     }
-
-    // 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);
+
+    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;
   }
 

image_geometry/test/CMakeLists.txt

@@ -2,3 +2,6 @@ catkin_add_nosetests(directed.py)
 
 catkin_add_gtest(${PROJECT_NAME}-utest utest.cpp)
 target_link_libraries(${PROJECT_NAME}-utest ${PROJECT_NAME} ${OpenCV_LIBS})
+
+catkin_add_gtest(${PROJECT_NAME}-utest-equi utest_equi.cpp)
+target_link_libraries(${PROJECT_NAME}-utest-equi ${PROJECT_NAME} ${OpenCV_LIBS})