From e740c51f4a8659b669c45bc12398247b567f0772 Mon Sep 17 00:00:00 2001 From: Perrine Aguiar Date: Thu, 6 Dec 2018 16:09:42 +0100 Subject: [PATCH 1/5] image_geometry: add fisheye model support --- image_geometry/src/pinhole_camera_model.cpp | 98 +++++++++++++++++---- 1 file changed, 82 insertions(+), 16 deletions(-) diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index 69f6f2c9c..ee9088b0c 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/image_geometry/src/pinhole_camera_model.cpp @@ -4,6 +4,62 @@ #include #endif +namespace { +// Compute fisheye distorted coordinates from undistorted coordinates. +// The distortion model used by the Tango fisheye camera 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 the Tango 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 ComputeWarpMapsToRectifyFisheyeImage( + 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(iu, ju) = jd; + cv_warp_map_y->at(iu, ju) = id; + } + } +} +} // namespace + namespace image_geometry { enum DistortionState { NONE, CALIBRATED, UNKNOWN }; @@ -132,7 +188,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 == "fisheye") { // 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 +244,6 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg) P_(1,3) *= scale_y; } } - return reduced_dirty; } @@ -253,7 +309,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,12 +356,16 @@ void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, in 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); + if (cam_info_.distortion_model == "fisheye") { + cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); + } else { + 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: @@ -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,16 @@ 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 == "fisheye") { + cache_->full_map1.create(cam_info_.height, cam_info_.width, CV_32FC1); + cache_->full_map2.create(cam_info_.height, cam_info_.width, CV_32FC1); + ComputeWarpMapsToRectifyFisheyeImage(cam_info_, &(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; } From 6f6547c5ca8869ed1a320569804f386deaa099af Mon Sep 17 00:00:00 2001 From: Perrine Aguiar Date: Thu, 6 Dec 2018 16:23:37 +0100 Subject: [PATCH 2/5] image_geometry: add const for fisheye model name --- .../include/image_geometry/pinhole_camera_model.h | 3 ++- image_geometry/src/pinhole_camera_model.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/image_geometry/include/image_geometry/pinhole_camera_model.h b/image_geometry/include/image_geometry/pinhole_camera_model.h index d5dfdb65d..ed2f0aa11 100644 --- a/image_geometry/include/image_geometry/pinhole_camera_model.h +++ b/image_geometry/include/image_geometry/pinhole_camera_model.h @@ -23,6 +23,7 @@ class Exception : public std::runtime_error class PinholeCameraModel { public: + static const std::string DISTORTION_MODEL_FISHEYE = "fisheye" PinholeCameraModel(); @@ -214,7 +215,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. * diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index ee9088b0c..d7ea9b24f 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/image_geometry/src/pinhole_camera_model.cpp @@ -189,7 +189,7 @@ 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 == "fisheye") { + cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE) { // 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) @@ -356,7 +356,7 @@ void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, in break; case CALIBRATED: initRectificationMaps(); - if (cam_info_.distortion_model == "fisheye") { + if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE) { cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); } else { if (raw.depth() == CV_32F || raw.depth() == CV_64F) @@ -509,7 +509,7 @@ void PinholeCameraModel::initRectificationMaps() const } } - if (cam_info_.distortion_model == "fisheye") { + if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE) { cache_->full_map1.create(cam_info_.height, cam_info_.width, CV_32FC1); cache_->full_map2.create(cam_info_.height, cam_info_.width, CV_32FC1); ComputeWarpMapsToRectifyFisheyeImage(cam_info_, &(cache_->full_map1), &(cache_->full_map2)); From 333a9785d5ab7c39f0b9d8366d262fbad7c96d9a Mon Sep 17 00:00:00 2001 From: Perrine Aguiar Date: Fri, 7 Dec 2018 10:32:43 +0100 Subject: [PATCH 3/5] image_geometry: use static string and update doc --- .../image_geometry/pinhole_camera_model.h | 2 +- image_geometry/src/pinhole_camera_model.cpp | 32 +++++++++---------- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/image_geometry/include/image_geometry/pinhole_camera_model.h b/image_geometry/include/image_geometry/pinhole_camera_model.h index ed2f0aa11..d85cbf827 100644 --- a/image_geometry/include/image_geometry/pinhole_camera_model.h +++ b/image_geometry/include/image_geometry/pinhole_camera_model.h @@ -23,7 +23,7 @@ class Exception : public std::runtime_error class PinholeCameraModel { public: - static const std::string DISTORTION_MODEL_FISHEYE = "fisheye" + static const std::string DISTORTION_MODEL_FISHEYE; PinholeCameraModel(); diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index d7ea9b24f..6fe9f906f 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/image_geometry/src/pinhole_camera_model.cpp @@ -6,10 +6,10 @@ namespace { // Compute fisheye distorted coordinates from undistorted coordinates. -// The distortion model used by the Tango fisheye camera is called FOV and is -// described in 'Straight lines have to be straight' by Frederic Devernay and +// 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( +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); @@ -23,13 +23,13 @@ void ApplyFovModel( *yd = yu * rd_div_ru; } } -// Compute the warp maps to undistort the Tango fisheye image using the FOV +// 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 ComputeWarpMapsToRectifyFisheyeImage( +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]; @@ -50,7 +50,7 @@ void ComputeWarpMapsToRectifyFisheyeImage( 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); + 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(iu, ju) = jd; @@ -62,6 +62,8 @@ void ComputeWarpMapsToRectifyFisheyeImage( namespace image_geometry { +const std::string PinholeCameraModel::DISTORTION_MODEL_FISHEYE = "fisheye"; + enum DistortionState { NONE, CALIBRATED, UNKNOWN }; struct PinholeCameraModel::Cache @@ -356,16 +358,12 @@ void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, in break; case CALIBRATED: initRectificationMaps(); - if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE) { - cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); - } else { - 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); - } + 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::quiet_NaN()); + } + else { + cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation); } break; default: @@ -512,7 +510,7 @@ void PinholeCameraModel::initRectificationMaps() const if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE) { cache_->full_map1.create(cam_info_.height, cam_info_.width, CV_32FC1); cache_->full_map2.create(cam_info_.height, cam_info_.width, CV_32FC1); - ComputeWarpMapsToRectifyFisheyeImage(cam_info_, &(cache_->full_map1), &(cache_->full_map2)); + initFisheyeUndistortRectifyMap(cam_info_, &(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, From 5171563aec4e9b274e764577fde9d71fc2eaa7d2 Mon Sep 17 00:00:00 2001 From: Perrine Aguiar Date: Mon, 17 Dec 2018 14:24:44 +0100 Subject: [PATCH 4/5] image_geometry: remove change in header file --- image_geometry/include/image_geometry/pinhole_camera_model.h | 1 - 1 file changed, 1 deletion(-) diff --git a/image_geometry/include/image_geometry/pinhole_camera_model.h b/image_geometry/include/image_geometry/pinhole_camera_model.h index d85cbf827..cdfc2fe0f 100644 --- a/image_geometry/include/image_geometry/pinhole_camera_model.h +++ b/image_geometry/include/image_geometry/pinhole_camera_model.h @@ -23,7 +23,6 @@ class Exception : public std::runtime_error class PinholeCameraModel { public: - static const std::string DISTORTION_MODEL_FISHEYE; PinholeCameraModel(); From 5fa39c53e1248106645acbbe1c143f2368366489 Mon Sep 17 00:00:00 2001 From: Perrine Aguiar Date: Mon, 17 Dec 2018 14:25:52 +0100 Subject: [PATCH 5/5] image_geometry: add support for opencv fisheye model --- image_geometry/src/pinhole_camera_model.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index 6fe9f906f..51c64a4e3 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/image_geometry/src/pinhole_camera_model.cpp @@ -62,7 +62,8 @@ void initFisheyeUndistortRectifyMap( namespace image_geometry { -const std::string PinholeCameraModel::DISTORTION_MODEL_FISHEYE = "fisheye"; +const std::string DISTORTION_MODEL_FISHEYE_FOV = "fisheye_fov"; +const std::string DISTORTION_MODEL_FISHEYE_CV = "fisheye_cv"; enum DistortionState { NONE, CALIBRATED, UNKNOWN }; @@ -191,7 +192,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 == DISTORTION_MODEL_FISHEYE) { + 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) @@ -507,10 +509,13 @@ void PinholeCameraModel::initRectificationMaps() const } } - if (cam_info_.distortion_model == DISTORTION_MODEL_FISHEYE) { + 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,