pinhole_camera_model.cpp
Go to the documentation of this file.
3 #include <opencv2/calib3d/calib3d.hpp>
4 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
5 #include <boost/make_shared.hpp>
6 #endif
7 
8 namespace image_geometry {
9 
12 
14 {
17 
18  cv::Mat_<double> K_binned, P_binned; // Binning applied, but not cropping
19 
20  mutable bool full_maps_dirty;
21  mutable cv::Mat full_map1, full_map2;
22 
23  mutable bool reduced_maps_dirty;
24  mutable cv::Mat reduced_map1, reduced_map2;
25 
28 
31 
32  mutable bool rectified_roi_dirty;
33  mutable cv::Rect rectified_roi;
34 
38  full_maps_dirty(true),
39  reduced_maps_dirty(true),
43  {
44  }
45 };
46 
48 {
49 }
50 
52 {
53  if (other.initialized())
54  this->fromCameraInfo(other.cameraInfo());
55  return *this;
56 }
57 
59 {
60  if (other.initialized())
62 }
63 
64 // For uint32_t, string, bool...
65 template<typename T>
66 bool update(const T& new_val, T& my_val)
67 {
68  if (my_val == new_val)
69  return false;
70  my_val = new_val;
71  return true;
72 }
73 
74 // For std::vector
75 template<typename MatT>
76 bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat, int rows, int cols)
77 {
78  if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
79  return false;
80  my_mat = new_mat;
81  // D may be empty if camera is uncalibrated or distortion model is non-standard
82  cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
83  return true;
84 }
85 
86 template<typename MatT, typename MatU>
87 bool updateMat(const MatT& new_mat, MatT& my_mat, MatU& cv_mat)
88 {
89  if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
90  return false;
91  my_mat = new_mat;
92  // D may be empty if camera is uncalibrated or distortion model is non-standard
93  cv_mat = MatU(&my_mat[0]);
94  return true;
95 }
96 
97 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
98 {
99  // Create our repository of cached data (rectification maps, etc.)
100  if (!cache_)
101 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
102  cache_ = boost::make_shared<Cache>();
103 #else
104  cache_ = std::make_shared<Cache>();
105 #endif
106 
107  // Binning = 0 is considered the same as binning = 1 (no binning).
108  uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
109  uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
110 
111  // ROI all zeros is considered the same as full resolution.
112  sensor_msgs::RegionOfInterest roi = msg.roi;
113  if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
114  roi.width = msg.width;
115  roi.height = msg.height;
116  }
117 
118  // Update time stamp (and frame_id if that changes for some reason)
119  cam_info_.header = msg.header;
120 
121  // Update any parameters that have changed. The full rectification maps are
122  // invalidated by any change in the calibration parameters OR binning.
123  bool full_dirty = false;
124  full_dirty |= update(msg.height, cam_info_.height);
125  full_dirty |= update(msg.width, cam_info_.width);
126  full_dirty |= update(msg.distortion_model, cam_info_.distortion_model);
127  full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size());
128  full_dirty |= updateMat(msg.K, cam_info_.K, K_full_);
129  full_dirty |= updateMat(msg.R, cam_info_.R, R_);
130  full_dirty |= updateMat(msg.P, cam_info_.P, P_full_);
131  full_dirty |= update(binning_x, cam_info_.binning_x);
132  full_dirty |= update(binning_y, cam_info_.binning_y);
133  cache_->full_maps_dirty |= full_dirty;
134  cache_->unrectify_full_maps_dirty |= full_dirty;
135 
136  // The reduced rectification maps are invalidated by any of the above or a
137  // change in ROI.
138  bool reduced_dirty = full_dirty;
139  reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset);
140  reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset);
141  reduced_dirty |= update(roi.height, cam_info_.roi.height);
142  reduced_dirty |= update(roi.width, cam_info_.roi.width);
143  reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify);
144  cache_->reduced_maps_dirty |= reduced_dirty;
145  cache_->reduced_maps_dirty |= cache_->full_maps_dirty;
146  cache_->unrectify_reduced_maps_dirty |= reduced_dirty;
147  cache_->unrectify_reduced_maps_dirty |= cache_->unrectify_full_maps_dirty;
148 
149  // As is the rectified ROI
150  cache_->rectified_roi_dirty |= reduced_dirty;
151 
152  // Figure out how to handle the distortion
153  if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
156  // If any distortion coefficient is non-zero, then need to apply the distortion
157  cache_->distortion_state = NONE;
158  for (size_t i = 0; i < cam_info_.D.size(); ++i)
159  {
160  if (cam_info_.D[i] != 0)
161  {
162  cache_->distortion_state = CALIBRATED;
163  break;
164  }
165  }
166  }
167  else
168  cache_->distortion_state = UNKNOWN;
169 
170  // Get the distortion model, if supported
171  if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
173  cache_->distortion_model = PLUMB_BOB_OR_RATIONAL_POLYNOMIAL;
174  }
175  else if(cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
176  cache_->distortion_model = EQUIDISTANT;
177  }
178  else
179  cache_->distortion_model = UNKNOWN_MODEL;
180 
181  // If necessary, create new K_ and P_ adjusted for binning and ROI
183  bool adjust_binning = (binning_x > 1) || (binning_y > 1);
184  bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
185 
186  if (!adjust_binning && !adjust_roi) {
187  K_ = K_full_;
188  P_ = P_full_;
189  }
190  else {
191  K_ = K_full_;
192  P_ = P_full_;
193 
194  // ROI is in full image coordinates, so change it first
195  if (adjust_roi) {
196  // Move principal point by the offset
198  K_(0,2) -= roi.x_offset;
199  K_(1,2) -= roi.y_offset;
200  P_(0,2) -= roi.x_offset;
201  P_(1,2) -= roi.y_offset;
202  }
203 
204  if (binning_x > 1) {
205  double scale_x = 1.0 / binning_x;
206  K_(0,0) *= scale_x;
207  K_(0,2) *= scale_x;
208  P_(0,0) *= scale_x;
209  P_(0,2) *= scale_x;
210  P_(0,3) *= scale_x;
211  }
212  if (binning_y > 1) {
213  double scale_y = 1.0 / binning_y;
214  K_(1,1) *= scale_y;
215  K_(1,2) *= scale_y;
216  P_(1,1) *= scale_y;
217  P_(1,2) *= scale_y;
218  P_(1,3) *= scale_y;
219  }
220  }
221 
222  return reduced_dirty;
223 }
224 
225 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg)
226 {
227  return fromCameraInfo(*msg);
228 }
229 
231 {
232  assert( initialized() );
233  return cv::Size(cam_info_.width, cam_info_.height);
234 }
235 
237 {
238  assert( initialized() );
239 
240  cv::Rect roi = rectifiedRoi();
241  return cv::Size(roi.width / binningX(), roi.height / binningY());
242 }
243 
244 cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const
245 {
246  cv::Rect roi = rectifiedRoi();
247  return cv::Point2d(uv_reduced.x * binningX() + roi.x,
248  uv_reduced.y * binningY() + roi.y);
249 }
250 
251 cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const
252 {
253  cv::Rect roi = rectifiedRoi();
254  return cv::Rect(roi_reduced.x * binningX() + roi.x,
255  roi_reduced.y * binningY() + roi.y,
256  roi_reduced.width * binningX(),
257  roi_reduced.height * binningY());
258 }
259 
260 cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const
261 {
262  cv::Rect roi = rectifiedRoi();
263  return cv::Point2d((uv_full.x - roi.x) / binningX(),
264  (uv_full.y - roi.y) / binningY());
265 }
266 
267 cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const
268 {
269  cv::Rect roi = rectifiedRoi();
270  return cv::Rect((roi_full.x - roi.x) / binningX(),
271  (roi_full.y - roi.y) / binningY(),
272  roi_full.width / binningX(),
273  roi_full.height / binningY());
274 }
275 
277 {
278  assert( initialized() );
279 
280  return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
281  cam_info_.roi.width, cam_info_.roi.height);
282 }
283 
285 {
286  assert( initialized() );
287 
288  if (cache_->rectified_roi_dirty)
289  {
290  if (!cam_info_.roi.do_rectify)
291  cache_->rectified_roi = rawRoi();
292  else
293  cache_->rectified_roi = rectifyRoi(rawRoi());
294  cache_->rectified_roi_dirty = false;
295  }
296  return cache_->rectified_roi;
297 }
298 
299 cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const
300 {
301  assert( initialized() );
302  assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane
303 
304  // [U V W]^T = P * [X Y Z 1]^T
305  // u = U/W
306  // v = V/W
307  cv::Point2d uv_rect;
308  uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx();
309  uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy();
310  return uv_rect;
311 }
312 
313 cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const
314 {
315  return projectPixelTo3dRay(uv_rect, P_);
316 }
317 
318 cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const
319 {
320  assert( initialized() );
321 
322  const double& fx = P(0,0);
323  const double& fy = P(1,1);
324  const double& cx = P(0,2);
325  const double& cy = P(1,2);
326  const double& Tx = P(0,3);
327  const double& Ty = P(1,3);
328 
329  cv::Point3d ray;
330  ray.x = (uv_rect.x - cx - Tx) / fx;
331  ray.y = (uv_rect.y - cy - Ty) / fy;
332  ray.z = 1.0;
333  return ray;
334 }
335 
336 void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const
337 {
338  assert( initialized() );
339 
340  switch (cache_->distortion_state) {
341  case NONE:
342  raw.copyTo(rectified);
343  break;
344  case CALIBRATED:
346  if (raw.depth() == CV_32F || raw.depth() == CV_64F)
347  {
348  cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
349  }
350  else {
351  cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
352  }
353  break;
354  default:
355  assert(cache_->distortion_state == UNKNOWN);
356  throw Exception("Cannot call rectifyImage when distortion is unknown.");
357  }
358 }
359 
360 void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const
361 {
362  assert( initialized() );
363 
364  switch (cache_->distortion_state) {
365  case NONE:
366  rectified.copyTo(raw);
367  break;
368  case CALIBRATED:
370  if (rectified.depth() == CV_32F || rectified.depth() == CV_64F)
371  {
372  cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
373  }
374  else {
375  cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation);
376  }
377  break;
378  default:
379  assert(cache_->distortion_state == UNKNOWN);
380  throw Exception("Cannot call rectifyImage when distortion is unknown.");
381  }
382 }
383 
384 cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
385 {
386  return rectifyPoint(uv_raw, K_, P_);
387 }
388 
389 cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const
390 {
391  assert( initialized() );
392 
393  if (cache_->distortion_state == NONE)
394  return uv_raw;
395  if (cache_->distortion_state == UNKNOWN)
396  throw Exception("Cannot call rectifyPoint when distortion is unknown.");
397  assert(cache_->distortion_state == CALIBRATED);
398 
400  cv::Point2f raw32 = uv_raw, rect32;
401  const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
402  cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);
403 
404  switch (cache_->distortion_model) {
406  cv::undistortPoints(src_pt, dst_pt, K, D_, R_, P);
407  break;
408  case EQUIDISTANT:
409  cv::fisheye::undistortPoints(src_pt, dst_pt, K, D_, R_, P);
410  break;
411  default:
412  assert(cache_->distortion_model == UNKNOWN_MODEL);
413  throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
414  }
415  return rect32;
416 }
417 
418 cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
419 {
420  return unrectifyPoint(uv_rect, K_, P_);
421 }
422 
423 cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const
424 {
425  assert( initialized() );
426 
427  if (cache_->distortion_state == NONE)
428  return uv_rect;
429  if (cache_->distortion_state == UNKNOWN)
430  throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
431  assert(cache_->distortion_state == CALIBRATED);
432 
433  // Convert to a ray
434  cv::Point3d ray = projectPixelTo3dRay(uv_rect, P);
435 
436  // Project the ray on the image
437  cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
438  cv::Rodrigues(R_.t(), r_vec);
439  std::vector<cv::Point2d> image_point;
440 
441  switch (cache_->distortion_model) {
443  cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K, D_, image_point);
444  break;
445  case EQUIDISTANT:
446  cv::fisheye::projectPoints(std::vector<cv::Point3d>(1, ray), image_point, r_vec, t_vec, K, D_);
447  break;
448  default:
449  assert(cache_->distortion_model == UNKNOWN_MODEL);
450  throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
451  }
452 
453  return image_point[0];
454 }
455 
456 cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const
457 {
458  assert( initialized() );
459 
461 
462  // For now, just unrectify the four corners and take the bounding box.
463  // Since ROI is specified in unbinned coordinates (see REP-104), this has to use K_full_ and P_full_.
464  cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y), K_full_, P_full_);
465  cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y), K_full_, P_full_);
466  cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
467  roi_raw.y + roi_raw.height), K_full_, P_full_);
468  cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height), K_full_, P_full_);
469 
470  cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
471  std::ceil (std::min(rect_tl.y, rect_tr.y)));
472  cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)),
473  std::floor(std::max(rect_bl.y, rect_br.y)));
474 
475  return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
476 }
477 
478 cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const
479 {
480  assert( initialized() );
481 
483 
484  // For now, just unrectify the four corners and take the bounding box.
485  cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y));
486  cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y));
487  cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width,
488  roi_rect.y + roi_rect.height));
489  cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height));
490 
491  cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)),
492  std::floor(std::min(raw_tl.y, raw_tr.y)));
493  cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)),
494  std::ceil (std::max(raw_bl.y, raw_br.y)));
495 
496  return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
497 }
498 
500 {
503 
504  if (cache_->full_maps_dirty) {
505  // Create the full-size map at the binned resolution
507  cv::Size binned_resolution = fullResolution();
508  binned_resolution.width /= binningX();
509  binned_resolution.height /= binningY();
510 
511  cv::Matx33d K_binned;
512  cv::Matx34d P_binned;
513  if (binningX() == 1 && binningY() == 1) {
514  K_binned = K_full_;
515  P_binned = P_full_;
516  }
517  else {
518  K_binned = K_full_;
519  P_binned = P_full_;
520  if (binningX() > 1) {
521  double scale_x = 1.0 / binningX();
522  K_binned(0,0) *= scale_x;
523  K_binned(0,2) *= scale_x;
524  P_binned(0,0) *= scale_x;
525  P_binned(0,2) *= scale_x;
526  P_binned(0,3) *= scale_x;
527  }
528  if (binningY() > 1) {
529  double scale_y = 1.0 / binningY();
530  K_binned(1,1) *= scale_y;
531  K_binned(1,2) *= scale_y;
532  P_binned(1,1) *= scale_y;
533  P_binned(1,2) *= scale_y;
534  P_binned(1,3) *= scale_y;
535  }
536  }
537 
538  switch (cache_->distortion_model) {
540  // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
541  cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
542  CV_16SC2, cache_->full_map1, cache_->full_map2);
543  break;
544  case EQUIDISTANT:
545  cv::fisheye::initUndistortRectifyMap(K_binned,D_, R_, P_binned, binned_resolution,
546  CV_16SC2, cache_->full_map1, cache_->full_map2);
547  break;
548  default:
549  assert(cache_->distortion_model == UNKNOWN_MODEL);
550  throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
551  }
552  cache_->full_maps_dirty = false;
553  }
554 
555  if (cache_->reduced_maps_dirty) {
557  cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
558  cam_info_.roi.width, cam_info_.roi.height);
559  if (roi.x != 0 || roi.y != 0 ||
560  (roi.height != 0 && roi.height != (int)cam_info_.height) ||
561  (roi.width != 0 && roi.width != (int)cam_info_.width)) {
562 
563  // map1 contains integer (x,y) offsets, which we adjust by the ROI offset
564  // map2 contains LUT index for subpixel interpolation, which we can leave as-is
565  roi.x /= binningX();
566  roi.y /= binningY();
567  roi.width /= binningX();
568  roi.height /= binningY();
569  cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
570  cache_->reduced_map2 = cache_->full_map2(roi);
571  }
572  else {
573  // Otherwise we're rectifying the full image
574  cache_->reduced_map1 = cache_->full_map1;
575  cache_->reduced_map2 = cache_->full_map2;
576  }
577  cache_->reduced_maps_dirty = false;
578  }
579 }
580 
582 {
585 
586  if (cache_->unrectify_full_maps_dirty) {
587  // Create the full-size map at the binned resolution
589  cv::Size binned_resolution = fullResolution();
590  binned_resolution.width /= binningX();
591  binned_resolution.height /= binningY();
592 
593  cv::Matx33d K_binned;
594  cv::Matx34d P_binned;
595  if (binningX() == 1 && binningY() == 1) {
596  K_binned = K_full_;
597  P_binned = P_full_;
598  }
599  else {
600  K_binned = K_full_;
601  P_binned = P_full_;
602  if (binningX() > 1) {
603  double scale_x = 1.0 / binningX();
604  K_binned(0,0) *= scale_x;
605  K_binned(0,2) *= scale_x;
606  P_binned(0,0) *= scale_x;
607  P_binned(0,2) *= scale_x;
608  P_binned(0,3) *= scale_x;
609  }
610  if (binningY() > 1) {
611  double scale_y = 1.0 / binningY();
612  K_binned(1,1) *= scale_y;
613  K_binned(1,2) *= scale_y;
614  P_binned(1,1) *= scale_y;
615  P_binned(1,2) *= scale_y;
616  P_binned(1,3) *= scale_y;
617  }
618  }
619 
620  cv::Mat float_map_x(binned_resolution.height, binned_resolution.width, CV_32FC1);
621  cv::Mat float_map_y(binned_resolution.height, binned_resolution.width, CV_32FC1);
622  for (size_t x = 0; x < binned_resolution.width; x++) {
623  for (size_t y = 0; y < binned_resolution.height; y++) {
624  cv::Point2f uv_raw(x, y), uv_rect;
625  uv_rect = rectifyPoint(uv_raw, K_binned, P_binned);
626  float_map_x.at<float>(y, x) = uv_rect.x;
627  float_map_y.at<float>(y, x) = uv_rect.y;
628  }
629  }
630  // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
631  convertMaps(float_map_x, float_map_y, cache_->unrectify_full_map1, cache_->unrectify_full_map2, CV_16SC2);
632  cache_->unrectify_full_maps_dirty = false;
633  }
634 
635  if (cache_->unrectify_reduced_maps_dirty) {
637  cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
638  cam_info_.roi.width, cam_info_.roi.height);
639  if (roi.x != 0 || roi.y != 0 ||
640  (roi.height != 0 && roi.height != (int)cam_info_.height) ||
641  (roi.width != 0 && roi.width != (int)cam_info_.width)) {
642 
643  // map1 contains integer (x,y) offsets, which we adjust by the ROI offset
644  // map2 contains LUT index for subpixel interpolation, which we can leave as-is
645  roi.x /= binningX();
646  roi.y /= binningY();
647  roi.width /= binningX();
648  roi.height /= binningY();
649  cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1(roi) - cv::Scalar(roi.x, roi.y);
650  cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2(roi);
651  }
652  else {
653  // Otherwise we're rectifying the full image
654  cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1;
655  cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2;
656  }
657  cache_->unrectify_reduced_maps_dirty = false;
658  }
659 }
660 
661 } //namespace image_geometry
sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL
const std::string RATIONAL_POLYNOMIAL
image_geometry::PinholeCameraModel::D_
cv::Mat_< double > D_
Definition: pinhole_camera_model.h:279
image_geometry::PLUMB_BOB_OR_RATIONAL_POLYNOMIAL
@ PLUMB_BOB_OR_RATIONAL_POLYNOMIAL
Definition: pinhole_camera_model.cpp:11
image_geometry::PinholeCameraModel::Cache::full_map1
cv::Mat full_map1
Definition: pinhole_camera_model.cpp:21
image_geometry::PinholeCameraModel::Ty
double Ty() const
Returns the y-translation term of the projection matrix.
Definition: pinhole_camera_model.h:327
image_geometry::PinholeCameraModel::binningX
uint32_t binningX() const
Returns the number of columns in each bin.
Definition: pinhole_camera_model.h:336
image_geometry::Exception
Definition: pinhole_camera_model.h:14
distortion_models.h
image_geometry::PinholeCameraModel::toReducedResolution
cv::Point2d toReducedResolution(const cv::Point2d &uv_full) const
Definition: pinhole_camera_model.cpp:260
image_geometry::UNKNOWN_MODEL
@ UNKNOWN_MODEL
Definition: pinhole_camera_model.cpp:11
sensor_msgs::distortion_models::EQUIDISTANT
const std::string EQUIDISTANT
image_geometry::PinholeCameraModel::cam_info_
sensor_msgs::CameraInfo cam_info_
Definition: pinhole_camera_model.h:278
image_geometry::PinholeCameraModel::rectifyImage
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const
Rectify a raw camera image.
Definition: pinhole_camera_model.cpp:336
image_geometry::NONE
@ NONE
Definition: pinhole_camera_model.cpp:10
image_geometry::PinholeCameraModel::rectifiedRoi
cv::Rect rectifiedRoi() const
The current rectified ROI, which best fits the raw ROI.
Definition: pinhole_camera_model.cpp:284
pinhole_camera_model.h
image_geometry::PinholeCameraModel::Cache::distortion_model
DistortionModel distortion_model
Definition: pinhole_camera_model.cpp:16
image_geometry::update
bool update(const T &new_val, T &my_val)
Definition: pinhole_camera_model.cpp:66
image_geometry::PinholeCameraModel::binningY
uint32_t binningY() const
Returns the number of rows in each bin.
Definition: pinhole_camera_model.h:337
image_geometry::PinholeCameraModel
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
Definition: pinhole_camera_model.h:24
image_geometry::PinholeCameraModel::Cache::reduced_map2
cv::Mat reduced_map2
Definition: pinhole_camera_model.cpp:24
image_geometry::PinholeCameraModel::projectPixelTo3dRay
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
Project a rectified pixel to a 3d ray.
Definition: pinhole_camera_model.cpp:313
image_geometry
Definition: pinhole_camera_model.h:12
image_geometry::PinholeCameraModel::project3dToPixel
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
Project a 3d point to rectified pixel coordinates.
Definition: pinhole_camera_model.cpp:299
image_geometry::PinholeCameraModel::Cache::distortion_state
DistortionState distortion_state
Definition: pinhole_camera_model.cpp:15
image_geometry::CALIBRATED
@ CALIBRATED
Definition: pinhole_camera_model.cpp:10
image_geometry::PinholeCameraModel::Cache::unrectify_reduced_map1
cv::Mat unrectify_reduced_map1
Definition: pinhole_camera_model.cpp:30
image_geometry::PinholeCameraModel::Cache::K_binned
cv::Mat_< double > K_binned
Definition: pinhole_camera_model.cpp:18
image_geometry::PinholeCameraModel::unrectifyRoi
cv::Rect unrectifyRoi(const cv::Rect &roi_rect) const
Compute the raw ROI best fitting a rectified ROI.
Definition: pinhole_camera_model.cpp:478
image_geometry::PinholeCameraModel::cameraInfo
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
Definition: pinhole_camera_model.h:314
image_geometry::PinholeCameraModel::Tx
double Tx() const
Returns the x-translation term of the projection matrix.
Definition: pinhole_camera_model.h:326
image_geometry::PinholeCameraModel::P_full_
cv::Matx34d P_full_
Definition: pinhole_camera_model.h:284
image_geometry::PinholeCameraModel::fromCameraInfo
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
Definition: pinhole_camera_model.cpp:97
image_geometry::PinholeCameraModel::fx
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
Definition: pinhole_camera_model.h:322
image_geometry::PinholeCameraModel::Cache::unrectify_reduced_maps_dirty
bool unrectify_reduced_maps_dirty
Definition: pinhole_camera_model.cpp:29
image_geometry::PinholeCameraModel::cache_
std::shared_ptr< Cache > cache_
Definition: pinhole_camera_model.h:287
image_geometry::PinholeCameraModel::Cache::P_binned
cv::Mat_< double > P_binned
Definition: pinhole_camera_model.cpp:18
image_geometry::PinholeCameraModel::Cache::reduced_map1
cv::Mat reduced_map1
Definition: pinhole_camera_model.cpp:24
image_geometry::PinholeCameraModel::toFullResolution
cv::Point2d toFullResolution(const cv::Point2d &uv_reduced) const
Definition: pinhole_camera_model.cpp:244
image_geometry::updateMat
bool updateMat(const MatT &new_mat, MatT &my_mat, cv::Mat_< double > &cv_mat, int rows, int cols)
Definition: pinhole_camera_model.cpp:76
image_geometry::PinholeCameraModel::Cache::unrectify_full_map1
cv::Mat unrectify_full_map1
Definition: pinhole_camera_model.cpp:27
image_geometry::PinholeCameraModel::Cache::full_maps_dirty
bool full_maps_dirty
Definition: pinhole_camera_model.cpp:20
image_geometry::PinholeCameraModel::Cache::full_map2
cv::Mat full_map2
Definition: pinhole_camera_model.cpp:21
image_geometry::DistortionState
DistortionState
Definition: pinhole_camera_model.cpp:10
image_geometry::PinholeCameraModel::Cache::rectified_roi
cv::Rect rectified_roi
Definition: pinhole_camera_model.cpp:33
image_geometry::PinholeCameraModel::Cache::unrectify_full_map2
cv::Mat unrectify_full_map2
Definition: pinhole_camera_model.cpp:27
image_geometry::UNKNOWN
@ UNKNOWN
Definition: pinhole_camera_model.cpp:10
image_geometry::PinholeCameraModel::cy
double cy() const
Returns the y coordinate of the optical center.
Definition: pinhole_camera_model.h:325
image_geometry::PinholeCameraModel::fullResolution
cv::Size fullResolution() const
The resolution at which the camera was calibrated.
Definition: pinhole_camera_model.cpp:230
image_geometry::PinholeCameraModel::Cache::reduced_maps_dirty
bool reduced_maps_dirty
Definition: pinhole_camera_model.cpp:23
image_geometry::DistortionModel
DistortionModel
Definition: pinhole_camera_model.cpp:11
image_geometry::PinholeCameraModel::R_
cv::Matx33d R_
Definition: pinhole_camera_model.h:280
image_geometry::PinholeCameraModel::initUnrectificationMaps
void initUnrectificationMaps() const
Definition: pinhole_camera_model.cpp:581
image_geometry::PinholeCameraModel::reducedResolution
cv::Size reducedResolution() const
The resolution of the current rectified image.
Definition: pinhole_camera_model.cpp:236
image_geometry::PinholeCameraModel::K_full_
cv::Matx33d K_full_
Definition: pinhole_camera_model.h:283
image_geometry::PinholeCameraModel::unrectifyImage
void unrectifyImage(const cv::Mat &rectified, cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Apply camera distortion to a rectified image.
Definition: pinhole_camera_model.cpp:360
image_geometry::PinholeCameraModel::unrectifyPoint
cv::Point2d unrectifyPoint(const cv::Point2d &uv_rect) const
Compute the raw image coordinates of a pixel in the rectified image.
Definition: pinhole_camera_model.cpp:418
image_geometry::PinholeCameraModel::Cache
Definition: pinhole_camera_model.cpp:13
image_geometry::PinholeCameraModel::K_
cv::Matx33d K_
Definition: pinhole_camera_model.h:281
image_geometry::PinholeCameraModel::rectifyPoint
cv::Point2d rectifyPoint(const cv::Point2d &uv_raw) const
Compute the rectified image coordinates of a pixel in the raw image.
Definition: pinhole_camera_model.cpp:384
image_geometry::PinholeCameraModel::fy
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
Definition: pinhole_camera_model.h:323
image_geometry::PinholeCameraModel::Cache::Cache
Cache()
Definition: pinhole_camera_model.cpp:35
image_geometry::PinholeCameraModel::cx
double cx() const
Returns the x coordinate of the optical center.
Definition: pinhole_camera_model.h:324
image_geometry::PinholeCameraModel::rectifyRoi
cv::Rect rectifyRoi(const cv::Rect &roi_raw) const
Compute the rectified ROI best fitting a raw ROI.
Definition: pinhole_camera_model.cpp:456
image_geometry::PinholeCameraModel::operator=
PinholeCameraModel & operator=(const PinholeCameraModel &other)
Definition: pinhole_camera_model.cpp:51
image_geometry::PinholeCameraModel::initRectificationMaps
void initRectificationMaps() const
Definition: pinhole_camera_model.cpp:499
sensor_msgs::distortion_models::PLUMB_BOB
const std::string PLUMB_BOB
image_geometry::PinholeCameraModel::PinholeCameraModel
PinholeCameraModel()
Definition: pinhole_camera_model.cpp:47
image_geometry::PinholeCameraModel::initialized
bool initialized() const
Returns true if the camera has been initialized.
Definition: pinhole_camera_model.h:275
image_geometry::PinholeCameraModel::Cache::unrectify_full_maps_dirty
bool unrectify_full_maps_dirty
Definition: pinhole_camera_model.cpp:26
image_geometry::PinholeCameraModel::rawRoi
cv::Rect rawRoi() const
The current raw ROI, as used for capture by the camera driver.
Definition: pinhole_camera_model.cpp:276
image_geometry::PinholeCameraModel::Cache::rectified_roi_dirty
bool rectified_roi_dirty
Definition: pinhole_camera_model.cpp:32
image_geometry::EQUIDISTANT
@ EQUIDISTANT
Definition: pinhole_camera_model.cpp:11
image_geometry::PinholeCameraModel::Cache::unrectify_reduced_map2
cv::Mat unrectify_reduced_map2
Definition: pinhole_camera_model.cpp:30
image_geometry::PinholeCameraModel::P_
cv::Matx34d P_
Definition: pinhole_camera_model.h:282


image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Aug 21 2024 02:47:03