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;
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
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
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
00067 cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
00068 return true;
00069 }
00070
00071 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
00072 {
00073
00074 if (!cache_)
00075 cache_ = boost::make_shared<Cache>();
00076
00077
00078 uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
00079 uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
00080
00081
00082 sensor_msgs::RegionOfInterest roi = msg.roi;
00083 if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
00084 roi.width = msg.width;
00085 roi.height = msg.height;
00086 }
00087
00088
00089 cam_info_.header = msg.header;
00090
00091
00092
00093 bool &full_dirty = cache_->full_maps_dirty;
00094 full_dirty |= update(msg.height, cam_info_.height);
00095 full_dirty |= update(msg.width, cam_info_.width);
00096 full_dirty |= update(msg.distortion_model, cam_info_.distortion_model);
00097 full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size());
00098 full_dirty |= updateMat(msg.K, cam_info_.K, K_full_, 3, 3);
00099 full_dirty |= updateMat(msg.R, cam_info_.R, R_, 3, 3);
00100 full_dirty |= updateMat(msg.P, cam_info_.P, P_full_, 3, 4);
00101 full_dirty |= update(binning_x, cam_info_.binning_x);
00102 full_dirty |= update(binning_y, cam_info_.binning_y);
00103
00104
00105
00106 bool &reduced_dirty = cache_->reduced_maps_dirty;
00107 reduced_dirty = full_dirty;
00108 reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset);
00109 reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset);
00110 reduced_dirty |= update(roi.height, cam_info_.roi.height);
00111 reduced_dirty |= update(roi.width, cam_info_.roi.width);
00112 reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify);
00113
00114 cache_->rectified_roi_dirty = reduced_dirty;
00115
00116
00117 if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
00118 cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
00119 cache_->distortion_state = (cam_info_.D[0] == 0.0) ? NONE : CALIBRATED;
00120 }
00121 else
00122 cache_->distortion_state = UNKNOWN;
00123
00124
00126 bool adjust_binning = (binning_x > 1) || (binning_y > 1);
00127 bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
00128
00129 if (!adjust_binning && !adjust_roi) {
00130 K_ = K_full_;
00131 P_ = P_full_;
00132 }
00133 else {
00134 K_full_.copyTo(K_);
00135 P_full_.copyTo(P_);
00136
00137
00138 if (adjust_roi) {
00139
00141 K_(0,2) -= roi.x_offset;
00142 K_(1,2) -= roi.y_offset;
00143 P_(0,2) -= roi.x_offset;
00144 P_(1,2) -= roi.y_offset;
00145 }
00146
00147 if (binning_x > 1) {
00148 double scale_x = 1.0 / binning_x;
00149 K_(0,0) *= scale_x;
00150 K_(0,2) *= scale_x;
00151 P_(0,0) *= scale_x;
00152 P_(0,2) *= scale_x;
00153 P_(0,3) *= scale_x;
00154 }
00155 if (binning_y > 1) {
00156 double scale_y = 1.0 / binning_y;
00157 K_(1,1) *= scale_y;
00158 K_(1,2) *= scale_y;
00159 P_(1,1) *= scale_y;
00160 P_(1,2) *= scale_y;
00161 P_(1,3) *= scale_y;
00162 }
00163 }
00164
00165 return reduced_dirty;
00166 }
00167
00168 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg)
00169 {
00170 return fromCameraInfo(*msg);
00171 }
00172
00173 cv::Size PinholeCameraModel::fullResolution() const
00174 {
00175 assert( initialized() );
00176 return cv::Size(cam_info_.width, cam_info_.height);
00177 }
00178
00179 cv::Size PinholeCameraModel::reducedResolution() const
00180 {
00181 assert( initialized() );
00182
00183 cv::Rect roi = rectifiedRoi();
00184 return cv::Size(roi.width / binningX(), roi.height / binningY());
00185 }
00186
00187 cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const
00188 {
00189 cv::Rect roi = rectifiedRoi();
00190 return cv::Point2d(uv_reduced.x * binningX() + roi.x,
00191 uv_reduced.y * binningY() + roi.y);
00192 }
00193
00194 cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const
00195 {
00196 cv::Rect roi = rectifiedRoi();
00197 return cv::Rect(roi_reduced.x * binningX() + roi.x,
00198 roi_reduced.y * binningY() + roi.y,
00199 roi_reduced.width * binningX(),
00200 roi_reduced.height * binningY());
00201 }
00202
00203 cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const
00204 {
00205 cv::Rect roi = rectifiedRoi();
00206 return cv::Point2d((uv_full.x - roi.x) / binningX(),
00207 (uv_full.y - roi.y) / binningY());
00208 }
00209
00210 cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const
00211 {
00212 cv::Rect roi = rectifiedRoi();
00213 return cv::Rect((roi_full.x - roi.x) / binningX(),
00214 (roi_full.y - roi.y) / binningY(),
00215 roi_full.width / binningX(),
00216 roi_full.height / binningY());
00217 }
00218
00219 cv::Rect PinholeCameraModel::rawRoi() const
00220 {
00221 assert( initialized() );
00222
00223 return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
00224 cam_info_.roi.width, cam_info_.roi.height);
00225 }
00226
00227 cv::Rect PinholeCameraModel::rectifiedRoi() const
00228 {
00229 assert( initialized() );
00230
00231 if (cache_->rectified_roi_dirty)
00232 {
00233 if (!cam_info_.roi.do_rectify)
00234 cache_->rectified_roi = rawRoi();
00235 else
00236 cache_->rectified_roi = rectifyRoi(rawRoi());
00237 cache_->rectified_roi_dirty = false;
00238 }
00239 return cache_->rectified_roi;
00240 }
00241
00242 cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const
00243 {
00244 assert( initialized() );
00245 assert(P_(2, 3) == 0.0);
00246
00247
00248
00249
00250 cv::Point2d uv_rect;
00251 uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx();
00252 uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy();
00253 return uv_rect;
00254 }
00255
00256 cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const
00257 {
00258 assert( initialized() );
00259
00260 cv::Point3d ray;
00261 ray.x = (uv_rect.x - cx() - Tx()) / fx();
00262 ray.y = (uv_rect.y - cy() - Ty()) / fy();
00263 ray.z = 1.0;
00264 return ray;
00265 }
00266
00267 void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const
00268 {
00269 assert( initialized() );
00270
00271 switch (cache_->distortion_state) {
00272 case NONE:
00273 raw.copyTo(rectified);
00274 break;
00275 case CALIBRATED:
00276 initRectificationMaps();
00277 cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
00278 break;
00279 default:
00280 assert(cache_->distortion_state == UNKNOWN);
00281 throw Exception("Cannot call rectifyImage when distortion is unknown.");
00282 }
00283 }
00284
00285 void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const
00286 {
00287 assert( initialized() );
00288
00289 throw Exception("PinholeCameraModel::unrectifyImage is unimplemented.");
00291
00292
00293
00294
00295
00296
00297 }
00298
00299 cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
00300 {
00301 assert( initialized() );
00302
00303 if (cache_->distortion_state == NONE)
00304 return uv_raw;
00305 if (cache_->distortion_state == UNKNOWN)
00306 throw Exception("Cannot call rectifyPoint when distortion is unknown.");
00307 assert(cache_->distortion_state == CALIBRATED);
00308
00310 cv::Point2f raw32 = uv_raw, rect32;
00311 const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
00312 cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);
00313 cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);
00314 return rect32;
00315 }
00316
00317 cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
00318 {
00319 assert( initialized() );
00320
00321 if (cache_->distortion_state == NONE)
00322 return uv_rect;
00323 if (cache_->distortion_state == UNKNOWN)
00324 throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
00325 assert(cache_->distortion_state == CALIBRATED);
00326
00327
00328 cv::Point3d ray = projectPixelTo3dRay(uv_rect);
00329
00330
00331 cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
00332 cv::Rodrigues(R_.t(), r_vec);
00333 std::vector<cv::Point2d> image_point;
00334 cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);
00335
00336 return image_point[0];
00337 }
00338
00339 cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const
00340 {
00341 assert( initialized() );
00342
00344
00345
00346 cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
00347 cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
00348 cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
00349 roi_raw.y + roi_raw.height));
00350 cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height));
00351
00352 cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
00353 std::ceil (std::min(rect_tl.y, rect_tr.y)));
00354 cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)),
00355 std::floor(std::max(rect_bl.y, rect_br.y)));
00356
00357 return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
00358 }
00359
00360 cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const
00361 {
00362 assert( initialized() );
00363
00365
00366
00367 cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y));
00368 cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y));
00369 cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width,
00370 roi_rect.y + roi_rect.height));
00371 cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height));
00372
00373 cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)),
00374 std::floor(std::min(raw_tl.y, raw_tr.y)));
00375 cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)),
00376 std::ceil (std::max(raw_bl.y, raw_br.y)));
00377
00378 return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
00379 }
00380
00381 void PinholeCameraModel::initRectificationMaps() const
00382 {
00385
00386 if (cache_->full_maps_dirty) {
00387
00389 cv::Size binned_resolution = fullResolution();
00390 binned_resolution.width /= binningX();
00391 binned_resolution.height /= binningY();
00392
00393 cv::Mat_<double> K_binned, P_binned;
00394 if (binningX() == 1 && binningY() == 1) {
00395 K_binned = K_full_;
00396 P_binned = P_full_;
00397 }
00398 else {
00399 K_full_.copyTo(K_binned);
00400 P_full_.copyTo(P_binned);
00401 if (binningX() > 1) {
00402 double scale_x = 1.0 / binningX();
00403 K_binned(0,0) *= scale_x;
00404 K_binned(0,2) *= scale_x;
00405 P_binned(0,0) *= scale_x;
00406 P_binned(0,2) *= scale_x;
00407 P_binned(0,3) *= scale_x;
00408 }
00409 if (binningY() > 1) {
00410 double scale_y = 1.0 / binningY();
00411 K_binned(1,1) *= scale_y;
00412 K_binned(1,2) *= scale_y;
00413 P_binned(1,1) *= scale_y;
00414 P_binned(1,2) *= scale_y;
00415 P_binned(1,3) *= scale_y;
00416 }
00417 }
00418
00419
00420 cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
00421 CV_16SC2, cache_->full_map1, cache_->full_map2);
00422 cache_->full_maps_dirty = false;
00423 }
00424
00425 if (cache_->reduced_maps_dirty) {
00427 cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
00428 cam_info_.roi.width, cam_info_.roi.height);
00429 if (roi.x != 0 || roi.y != 0 ||
00430 roi.height != (int)cam_info_.height ||
00431 roi.width != (int)cam_info_.width) {
00432
00433
00434
00435 roi.x /= binningX();
00436 roi.y /= binningY();
00437 roi.width /= binningX();
00438 roi.height /= binningY();
00439 cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
00440 cache_->reduced_map2 = cache_->full_map2(roi);
00441 }
00442 else {
00443
00444 cache_->reduced_map1 = cache_->full_map1;
00445 cache_->reduced_map2 = cache_->full_map2;
00446 }
00447 cache_->reduced_maps_dirty = false;
00448 }
00449 }
00450
00451 }