Feature/add fisheye model support

image_geometry/include/image_geometry/pinhole_camera_model.h

@@ -214,7 +214,7 @@ class PinholeCameraModel
    * \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.
    *

image_geometry/src/pinhole_camera_model.cpp

@@ -4,8 +4,67 @@
 #include <boost/make_shared.hpp>
 #endif
 
+namespace {
+// Compute fisheye distorted coordinates from undistorted coordinates.
+// The distortion model used is called FOV and is described in
+// 'Straight lines have to be straight' by Frederic Devernay and
+// Olivier Faugeras. See https://hal.inria.fr/inria-00267247/document.
+void applyFovModel(
+    double xu, double yu, double w, double w_inverse, double two_tan_w_div_two,
+    double* xd, double* yd) {
+  double ru = sqrt(xu * xu + yu * yu);
+  const double epsilon = 1e-7;
+  if (w < epsilon || ru < epsilon) {
+    *xd = xu;
+    *yd = yu ;
+  } else {
+    double rd_div_ru = std::atan(ru * two_tan_w_div_two) * w_inverse / ru;
+    *xd = xu * rd_div_ru;
+    *yd = yu * rd_div_ru;
+  }
+}
+// Compute the warp maps to undistort a fisheye image using the FOV
+// model. See OpenCV documentation for more information on warp maps:
+// http://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html
+// @param fisheye_camera_info the fisheye camera intrinsics.
+// @param cv_warp_map_x the output map for the x direction.
+// @param cv_warp_map_y the output map for the y direction.
+void initFisheyeUndistortRectifyMap(
+    const sensor_msgs::CameraInfo& fisheye_camera_info,
+    cv::Mat* cv_warp_map_x, cv::Mat* cv_warp_map_y) {
+  const double fx = fisheye_camera_info.K[0];
+  const double fy = fisheye_camera_info.K[4];
+  const double cx = fisheye_camera_info.K[2];
+  const double cy = fisheye_camera_info.K[5];
+  const double w = fisheye_camera_info.D[0];
+  // Pre-computed variables for more efficiency.
+  const double fy_inverse = 1.0 / fy;
+  const double fx_inverse = 1.0 / fx;
+  const double w_inverse = 1 / w;
+  const double two_tan_w_div_two = 2.0 * std::tan(w * 0.5);
+  // Compute warp maps in x and y directions.
+  // OpenCV expects maps from dest to src, i.e. from undistorted to distorted
+  // pixel coordinates.
+  for(int iu = 0; iu < fisheye_camera_info.height; ++iu) {
+    for (int ju = 0; ju < fisheye_camera_info.width; ++ju) {
+      double xu = (ju - cx) * fx_inverse;
+      double yu = (iu - cy) * fy_inverse;
+      double xd, yd;
+      applyFovModel(xu, yu, w, w_inverse, two_tan_w_div_two, &xd, &yd);
+      double jd = cx + xd * fx;
+      double id = cy + yd * fy;
+      cv_warp_map_x->at<float>(iu, ju) = jd;
+      cv_warp_map_y->at<float>(iu, ju) = id;
+    }
+  }
+}
+} // namespace
+
 namespace image_geometry {
 
+const std::string DISTORTION_MODEL_FISHEYE_FOV = "fisheye_fov";
+const std::string DISTORTION_MODEL_FISHEYE_CV = "fisheye_cv";
+
 enum DistortionState { NONE, CALIBRATED, UNKNOWN };
 
 struct PinholeCameraModel::Cache
@@ -132,7 +191,9 @@ 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 == DISTORTION_MODEL_FISHEYE_FOV ||
+      cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE_CV) {
     // 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)
@@ -187,7 +248,6 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
       P_(1,3) *= scale_y;
     }
   }
-
   return reduced_dirty;
 }
 
@@ -253,7 +313,7 @@ cv::Rect PinholeCameraModel::rawRoi() const
 cv::Rect PinholeCameraModel::rectifiedRoi() const
 {
   assert( initialized() );
-  
+
   if (cache_->rectified_roi_dirty)
   {
     if (!cam_info_.roi.do_rectify)
@@ -300,7 +360,7 @@ void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, in
       break;
     case CALIBRATED:
       initRectificationMaps();
-      if (raw.depth() == CV_32F || raw.depth() == CV_64F)
+      if (raw.depth() == CV_32F || raw.depth() == CV_64F || raw.depth() == CV_32FC1)
       {
         cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
       }
@@ -373,7 +433,7 @@ 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.
   cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
   cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
@@ -394,7 +454,7 @@ 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));
@@ -414,7 +474,7 @@ 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?
@@ -448,10 +508,19 @@ 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);
+
+    if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE_FOV) {
+      cache_->full_map1.create(cam_info_.height, cam_info_.width, CV_32FC1);
+      cache_->full_map2.create(cam_info_.height, cam_info_.width, CV_32FC1);
+      initFisheyeUndistortRectifyMap(cam_info_, &(cache_->full_map1), &(cache_->full_map2));
+    } else if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE_CV) {
+      cv::fisheye::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
+                                           CV_16SC2, cache_->full_map1, cache_->full_map2);
+    } else {
+      // 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);
+    }
     cache_->full_maps_dirty = false;
   }