pinhole_camera_model.cpp
Go to the documentation of this file.
00001 #include "image_geometry/pinhole_camera_model.h"
00002 #include <sensor_msgs/distortion_models.h>
00003 #include <boost/make_shared.hpp>
00004 
00005 namespace image_geometry {
00006 
00007 enum DistortionState { NONE, CALIBRATED, UNKNOWN };
00008 
00009 struct PinholeCameraModel::Cache
00010 {
00011   DistortionState distortion_state;
00012 
00013   cv::Mat_<double> K_binned, P_binned; // Binning applied, but not cropping
00014   
00015   mutable bool full_maps_dirty;
00016   mutable cv::Mat full_map1, full_map2;
00017 
00018   mutable bool reduced_maps_dirty;
00019   mutable cv::Mat reduced_map1, reduced_map2;
00020   
00021   mutable bool rectified_roi_dirty;
00022   mutable cv::Rect rectified_roi;
00023 
00024   Cache()
00025     : full_maps_dirty(true),
00026       reduced_maps_dirty(true),
00027       rectified_roi_dirty(true)
00028   {
00029   }
00030 };
00031 
00032 PinholeCameraModel::PinholeCameraModel()
00033 {
00034 }
00035 
00036 PinholeCameraModel& PinholeCameraModel::operator=(const PinholeCameraModel& other)
00037 {
00038   if (other.initialized())
00039     this->fromCameraInfo(other.cameraInfo());
00040   return *this;
00041 }
00042 
00043 PinholeCameraModel::PinholeCameraModel(const PinholeCameraModel& other)
00044 {
00045   if (other.initialized())
00046     fromCameraInfo(other.cam_info_);
00047 }
00048 
00049 // For uint32_t, string, bool...
00050 template<typename T>
00051 bool update(const T& new_val, T& my_val)
00052 {
00053   if (my_val == new_val)
00054     return false;
00055   my_val = new_val;
00056   return true;
00057 }
00058 
00059 // For boost::array, std::vector
00060 template<typename MatT>
00061 bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat, int rows, int cols)
00062 {
00063   if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
00064     return false;
00065   my_mat = new_mat;
00066   // D may be empty if camera is uncalibrated or distortion model is non-standard
00067   cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
00068   return true;
00069 }
00070 
00071 template<typename MatT, typename MatU>
00072 bool updateMat(const MatT& new_mat, MatT& my_mat, MatU& cv_mat)
00073 {
00074   if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
00075     return false;
00076   my_mat = new_mat;
00077   // D may be empty if camera is uncalibrated or distortion model is non-standard
00078   cv_mat = MatU(&my_mat[0]);
00079   return true;
00080 }
00081 
00082 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
00083 {
00084   // Create our repository of cached data (rectification maps, etc.)
00085   if (!cache_)
00086     cache_ = boost::make_shared<Cache>();
00087   
00088   // Binning = 0 is considered the same as binning = 1 (no binning).
00089   uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
00090   uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
00091 
00092   // ROI all zeros is considered the same as full resolution.
00093   sensor_msgs::RegionOfInterest roi = msg.roi;
00094   if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
00095     roi.width  = msg.width;
00096     roi.height = msg.height;
00097   }
00098 
00099   // Update time stamp (and frame_id if that changes for some reason)
00100   cam_info_.header = msg.header;
00101   
00102   // Update any parameters that have changed. The full rectification maps are
00103   // invalidated by any change in the calibration parameters OR binning.
00104   bool &full_dirty = cache_->full_maps_dirty;
00105   full_dirty |= update(msg.height, cam_info_.height);
00106   full_dirty |= update(msg.width,  cam_info_.width);
00107   full_dirty |= update(msg.distortion_model, cam_info_.distortion_model);
00108   full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size());
00109   full_dirty |= updateMat(msg.K, cam_info_.K, K_full_);
00110   full_dirty |= updateMat(msg.R, cam_info_.R, R_);
00111   full_dirty |= updateMat(msg.P, cam_info_.P, P_full_);
00112   full_dirty |= update(binning_x, cam_info_.binning_x);
00113   full_dirty |= update(binning_y, cam_info_.binning_y);
00114 
00115   // The reduced rectification maps are invalidated by any of the above or a
00116   // change in ROI.
00117   bool &reduced_dirty = cache_->reduced_maps_dirty;
00118   reduced_dirty  = full_dirty;
00119   reduced_dirty |= update(roi.x_offset,   cam_info_.roi.x_offset);
00120   reduced_dirty |= update(roi.y_offset,   cam_info_.roi.y_offset);
00121   reduced_dirty |= update(roi.height,     cam_info_.roi.height);
00122   reduced_dirty |= update(roi.width,      cam_info_.roi.width);
00123   reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify);
00124   // As is the rectified ROI
00125   cache_->rectified_roi_dirty = reduced_dirty;
00126 
00127   // Figure out how to handle the distortion
00128   if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
00129       cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
00130     // If any distortion coefficient is non-zero, then need to apply the distortion
00131     cache_->distortion_state = NONE;
00132     for (size_t i = 0; i < cam_info_.D.size(); ++i)
00133     {
00134       if (cam_info_.D[i] != 0)
00135       {
00136         cache_->distortion_state = CALIBRATED;
00137         break;
00138       }
00139     }
00140   }
00141   else
00142     cache_->distortion_state = UNKNOWN;
00143 
00144   // If necessary, create new K_ and P_ adjusted for binning and ROI
00146   bool adjust_binning = (binning_x > 1) || (binning_y > 1);
00147   bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
00148 
00149   if (!adjust_binning && !adjust_roi) {
00150     K_ = K_full_;
00151     P_ = P_full_;
00152   }
00153   else {
00154     K_ = K_full_;
00155     P_ = P_full_;
00156 
00157     // ROI is in full image coordinates, so change it first
00158     if (adjust_roi) {
00159       // Move principal point by the offset
00161       K_(0,2) -= roi.x_offset;
00162       K_(1,2) -= roi.y_offset;
00163       P_(0,2) -= roi.x_offset;
00164       P_(1,2) -= roi.y_offset;
00165     }
00166 
00167     if (binning_x > 1) {
00168       double scale_x = 1.0 / binning_x;
00169       K_(0,0) *= scale_x;
00170       K_(0,2) *= scale_x;
00171       P_(0,0) *= scale_x;
00172       P_(0,2) *= scale_x;
00173       P_(0,3) *= scale_x;
00174     }
00175     if (binning_y > 1) {
00176       double scale_y = 1.0 / binning_y;
00177       K_(1,1) *= scale_y;
00178       K_(1,2) *= scale_y;
00179       P_(1,1) *= scale_y;
00180       P_(1,2) *= scale_y;
00181       P_(1,3) *= scale_y;
00182     }
00183   }
00184 
00185   return reduced_dirty;
00186 }
00187 
00188 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg)
00189 {
00190   return fromCameraInfo(*msg);
00191 }
00192 
00193 cv::Size PinholeCameraModel::fullResolution() const
00194 {
00195   assert( initialized() );
00196   return cv::Size(cam_info_.width, cam_info_.height);
00197 }
00198 
00199 cv::Size PinholeCameraModel::reducedResolution() const
00200 {
00201   assert( initialized() );
00202 
00203   cv::Rect roi = rectifiedRoi();
00204   return cv::Size(roi.width / binningX(), roi.height / binningY());
00205 }
00206 
00207 cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const
00208 {
00209   cv::Rect roi = rectifiedRoi();
00210   return cv::Point2d(uv_reduced.x * binningX() + roi.x,
00211                      uv_reduced.y * binningY() + roi.y);
00212 }
00213 
00214 cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const
00215 {
00216   cv::Rect roi = rectifiedRoi();
00217   return cv::Rect(roi_reduced.x * binningX() + roi.x,
00218                   roi_reduced.y * binningY() + roi.y,
00219                   roi_reduced.width  * binningX(),
00220                   roi_reduced.height * binningY());
00221 }
00222 
00223 cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const
00224 {
00225   cv::Rect roi = rectifiedRoi();
00226   return cv::Point2d((uv_full.x - roi.x) / binningX(),
00227                      (uv_full.y - roi.y) / binningY());
00228 }
00229 
00230 cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const
00231 {
00232   cv::Rect roi = rectifiedRoi();
00233   return cv::Rect((roi_full.x - roi.x) / binningX(),
00234                   (roi_full.y - roi.y) / binningY(),
00235                   roi_full.width  / binningX(),
00236                   roi_full.height / binningY());
00237 }
00238 
00239 cv::Rect PinholeCameraModel::rawRoi() const
00240 {
00241   assert( initialized() );
00242 
00243   return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
00244                   cam_info_.roi.width, cam_info_.roi.height);
00245 }
00246 
00247 cv::Rect PinholeCameraModel::rectifiedRoi() const
00248 {
00249   assert( initialized() );
00250   
00251   if (cache_->rectified_roi_dirty)
00252   {
00253     if (!cam_info_.roi.do_rectify)
00254       cache_->rectified_roi = rawRoi();
00255     else
00256       cache_->rectified_roi = rectifyRoi(rawRoi());
00257     cache_->rectified_roi_dirty = false;
00258   }
00259   return cache_->rectified_roi;
00260 }
00261 
00262 cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const
00263 {
00264   assert( initialized() );
00265   assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane
00266 
00267   // [U V W]^T = P * [X Y Z 1]^T
00268   // u = U/W
00269   // v = V/W
00270   cv::Point2d uv_rect;
00271   uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx();
00272   uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy();
00273   return uv_rect;
00274 }
00275 
00276 cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const
00277 {
00278   assert( initialized() );
00279 
00280   cv::Point3d ray;
00281   ray.x = (uv_rect.x - cx() - Tx()) / fx();
00282   ray.y = (uv_rect.y - cy() - Ty()) / fy();
00283   ray.z = 1.0;
00284   return ray;
00285 }
00286 
00287 void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const
00288 {
00289   assert( initialized() );
00290 
00291   switch (cache_->distortion_state) {
00292     case NONE:
00293       raw.copyTo(rectified);
00294       break;
00295     case CALIBRATED:
00296       initRectificationMaps();
00297       if (raw.depth() == CV_32F || raw.depth() == CV_64F)
00298       {
00299         cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
00300       }
00301       else {
00302         cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
00303       }
00304       break;
00305     default:
00306       assert(cache_->distortion_state == UNKNOWN);
00307       throw Exception("Cannot call rectifyImage when distortion is unknown.");
00308   }
00309 }
00310 
00311 void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const
00312 {
00313   assert( initialized() );
00314 
00315   throw Exception("PinholeCameraModel::unrectifyImage is unimplemented.");
00317   // Similar to rectifyImage, but need to build separate set of inverse maps (raw->rectified)...
00318   // - Build src_pt Mat with all the raw pixel coordinates (or do it one row at a time)
00319   // - Do cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_)
00320   // - Use convertMaps() to convert dst_pt to fast fixed-point maps
00321   // - cv::remap(rectified, raw, ...)
00322   // Need interpolation argument. Same caching behavior?
00323 }
00324 
00325 cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
00326 {
00327   assert( initialized() );
00328 
00329   if (cache_->distortion_state == NONE)
00330     return uv_raw;
00331   if (cache_->distortion_state == UNKNOWN)
00332     throw Exception("Cannot call rectifyPoint when distortion is unknown.");
00333   assert(cache_->distortion_state == CALIBRATED);
00334 
00336   cv::Point2f raw32 = uv_raw, rect32;
00337   const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
00338   cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);
00339   cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);
00340   return rect32;
00341 }
00342 
00343 cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
00344 {
00345   assert( initialized() );
00346 
00347   if (cache_->distortion_state == NONE)
00348     return uv_rect;
00349   if (cache_->distortion_state == UNKNOWN)
00350     throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
00351   assert(cache_->distortion_state == CALIBRATED);
00352 
00353   // Convert to a ray
00354   cv::Point3d ray = projectPixelTo3dRay(uv_rect);
00355 
00356   // Project the ray on the image
00357   cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
00358   cv::Rodrigues(R_.t(), r_vec);
00359   std::vector<cv::Point2d> image_point;
00360   cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);
00361 
00362   return image_point[0];
00363 }
00364 
00365 cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const
00366 {
00367   assert( initialized() );
00368 
00370   
00371   // For now, just unrectify the four corners and take the bounding box.
00372   cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
00373   cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
00374   cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
00375                                                  roi_raw.y + roi_raw.height));
00376   cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height));
00377 
00378   cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
00379                    std::ceil (std::min(rect_tl.y, rect_tr.y)));
00380   cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)),
00381                    std::floor(std::max(rect_bl.y, rect_br.y)));
00382 
00383   return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
00384 }
00385 
00386 cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const
00387 {
00388   assert( initialized() );
00389 
00391   
00392   // For now, just unrectify the four corners and take the bounding box.
00393   cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y));
00394   cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y));
00395   cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width,
00396                                                   roi_rect.y + roi_rect.height));
00397   cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height));
00398 
00399   cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)),
00400                    std::floor(std::min(raw_tl.y, raw_tr.y)));
00401   cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)),
00402                    std::ceil (std::max(raw_bl.y, raw_br.y)));
00403 
00404   return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
00405 }
00406 
00407 void PinholeCameraModel::initRectificationMaps() const
00408 {
00411   
00412   if (cache_->full_maps_dirty) {
00413     // Create the full-size map at the binned resolution
00415     cv::Size binned_resolution = fullResolution();
00416     binned_resolution.width  /= binningX();
00417     binned_resolution.height /= binningY();
00418 
00419     cv::Matx33d K_binned;
00420     cv::Matx34d P_binned;
00421     if (binningX() == 1 && binningY() == 1) {
00422       K_binned = K_full_;
00423       P_binned = P_full_;
00424     }
00425     else {
00426       K_binned = K_full_;
00427       P_binned = P_full_;
00428       if (binningX() > 1) {
00429         double scale_x = 1.0 / binningX();
00430         K_binned(0,0) *= scale_x;
00431         K_binned(0,2) *= scale_x;
00432         P_binned(0,0) *= scale_x;
00433         P_binned(0,2) *= scale_x;
00434         P_binned(0,3) *= scale_x;
00435       }
00436       if (binningY() > 1) {
00437         double scale_y = 1.0 / binningY();
00438         K_binned(1,1) *= scale_y;
00439         K_binned(1,2) *= scale_y;
00440         P_binned(1,1) *= scale_y;
00441         P_binned(1,2) *= scale_y;
00442         P_binned(1,3) *= scale_y;
00443       }
00444     }
00445     
00446     // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
00447     cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
00448                                 CV_16SC2, cache_->full_map1, cache_->full_map2);
00449     cache_->full_maps_dirty = false;
00450   }
00451 
00452   if (cache_->reduced_maps_dirty) {
00454     cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
00455                  cam_info_.roi.width, cam_info_.roi.height);
00456     if (roi.x != 0 || roi.y != 0 ||
00457         roi.height != (int)cam_info_.height ||
00458         roi.width  != (int)cam_info_.width) {
00459 
00460       // map1 contains integer (x,y) offsets, which we adjust by the ROI offset
00461       // map2 contains LUT index for subpixel interpolation, which we can leave as-is
00462       roi.x /= binningX();
00463       roi.y /= binningY();
00464       roi.width  /= binningX();
00465       roi.height /= binningY();
00466       cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
00467       cache_->reduced_map2 = cache_->full_map2(roi);
00468     }
00469     else {
00470       // Otherwise we're rectifying the full image
00471       cache_->reduced_map1 = cache_->full_map1;
00472       cache_->reduced_map2 = cache_->full_map2;
00473     }
00474     cache_->reduced_maps_dirty = false;
00475   }
00476 }
00477 
00478 } //namespace image_geometry


image_geometry
Author(s): Patrick Mihelich
autogenerated on Thu Jun 6 2019 21:23:35