pinhole_camera_model.cpp
Go to the documentation of this file.
3 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
4 #include <boost/make_shared.hpp>
5 #endif
6 
7 namespace image_geometry {
8 
10 
12 {
14 
15  cv::Mat_<double> K_binned, P_binned; // Binning applied, but not cropping
16 
17  mutable bool full_maps_dirty;
18  mutable cv::Mat full_map1, full_map2;
19 
20  mutable bool reduced_maps_dirty;
21  mutable cv::Mat reduced_map1, reduced_map2;
22 
23  mutable bool rectified_roi_dirty;
24  mutable cv::Rect rectified_roi;
25 
27  : full_maps_dirty(true),
28  reduced_maps_dirty(true),
29  rectified_roi_dirty(true)
30  {
31  }
32 };
33 
35 {
36 }
37 
39 {
40  if (other.initialized())
41  this->fromCameraInfo(other.cameraInfo());
42  return *this;
43 }
44 
46 {
47  if (other.initialized())
49 }
50 
51 // For uint32_t, string, bool...
52 template<typename T>
53 bool update(const T& new_val, T& my_val)
54 {
55  if (my_val == new_val)
56  return false;
57  my_val = new_val;
58  return true;
59 }
60 
61 // For std::vector
62 template<typename MatT>
63 bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat, int rows, int cols)
64 {
65  if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
66  return false;
67  my_mat = new_mat;
68  // D may be empty if camera is uncalibrated or distortion model is non-standard
69  cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
70  return true;
71 }
72 
73 template<typename MatT, typename MatU>
74 bool updateMat(const MatT& new_mat, MatT& my_mat, MatU& cv_mat)
75 {
76  if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
77  return false;
78  my_mat = new_mat;
79  // D may be empty if camera is uncalibrated or distortion model is non-standard
80  cv_mat = MatU(&my_mat[0]);
81  return true;
82 }
83 
84 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
85 {
86  // Create our repository of cached data (rectification maps, etc.)
87  if (!cache_)
88 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
89  cache_ = boost::make_shared<Cache>();
90 #else
91  cache_ = std::make_shared<Cache>();
92 #endif
93 
94  // Binning = 0 is considered the same as binning = 1 (no binning).
95  uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
96  uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
97 
98  // ROI all zeros is considered the same as full resolution.
99  sensor_msgs::RegionOfInterest roi = msg.roi;
100  if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
101  roi.width = msg.width;
102  roi.height = msg.height;
103  }
104 
105  // Update time stamp (and frame_id if that changes for some reason)
106  cam_info_.header = msg.header;
107 
108  // Update any parameters that have changed. The full rectification maps are
109  // invalidated by any change in the calibration parameters OR binning.
110  bool &full_dirty = cache_->full_maps_dirty;
111  full_dirty |= update(msg.height, cam_info_.height);
112  full_dirty |= update(msg.width, cam_info_.width);
113  full_dirty |= update(msg.distortion_model, cam_info_.distortion_model);
114  full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size());
115  full_dirty |= updateMat(msg.K, cam_info_.K, K_full_);
116  full_dirty |= updateMat(msg.R, cam_info_.R, R_);
117  full_dirty |= updateMat(msg.P, cam_info_.P, P_full_);
118  full_dirty |= update(binning_x, cam_info_.binning_x);
119  full_dirty |= update(binning_y, cam_info_.binning_y);
120 
121  // The reduced rectification maps are invalidated by any of the above or a
122  // change in ROI.
123  bool &reduced_dirty = cache_->reduced_maps_dirty;
124  reduced_dirty = full_dirty;
125  reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset);
126  reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset);
127  reduced_dirty |= update(roi.height, cam_info_.roi.height);
128  reduced_dirty |= update(roi.width, cam_info_.roi.width);
129  reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify);
130  // As is the rectified ROI
131  cache_->rectified_roi_dirty = reduced_dirty;
132 
133  // Figure out how to handle the distortion
134  if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
136  // If any distortion coefficient is non-zero, then need to apply the distortion
137  cache_->distortion_state = NONE;
138  for (size_t i = 0; i < cam_info_.D.size(); ++i)
139  {
140  if (cam_info_.D[i] != 0)
141  {
142  cache_->distortion_state = CALIBRATED;
143  break;
144  }
145  }
146  }
147  else
148  cache_->distortion_state = UNKNOWN;
149 
150  // If necessary, create new K_ and P_ adjusted for binning and ROI
152  bool adjust_binning = (binning_x > 1) || (binning_y > 1);
153  bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
154 
155  if (!adjust_binning && !adjust_roi) {
156  K_ = K_full_;
157  P_ = P_full_;
158  }
159  else {
160  K_ = K_full_;
161  P_ = P_full_;
162 
163  // ROI is in full image coordinates, so change it first
164  if (adjust_roi) {
165  // Move principal point by the offset
167  K_(0,2) -= roi.x_offset;
168  K_(1,2) -= roi.y_offset;
169  P_(0,2) -= roi.x_offset;
170  P_(1,2) -= roi.y_offset;
171  }
172 
173  if (binning_x > 1) {
174  double scale_x = 1.0 / binning_x;
175  K_(0,0) *= scale_x;
176  K_(0,2) *= scale_x;
177  P_(0,0) *= scale_x;
178  P_(0,2) *= scale_x;
179  P_(0,3) *= scale_x;
180  }
181  if (binning_y > 1) {
182  double scale_y = 1.0 / binning_y;
183  K_(1,1) *= scale_y;
184  K_(1,2) *= scale_y;
185  P_(1,1) *= scale_y;
186  P_(1,2) *= scale_y;
187  P_(1,3) *= scale_y;
188  }
189  }
190 
191  return reduced_dirty;
192 }
193 
194 bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg)
195 {
196  return fromCameraInfo(*msg);
197 }
198 
200 {
201  assert( initialized() );
202  return cv::Size(cam_info_.width, cam_info_.height);
203 }
204 
206 {
207  assert( initialized() );
208 
209  cv::Rect roi = rectifiedRoi();
210  return cv::Size(roi.width / binningX(), roi.height / binningY());
211 }
212 
213 cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const
214 {
215  cv::Rect roi = rectifiedRoi();
216  return cv::Point2d(uv_reduced.x * binningX() + roi.x,
217  uv_reduced.y * binningY() + roi.y);
218 }
219 
220 cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const
221 {
222  cv::Rect roi = rectifiedRoi();
223  return cv::Rect(roi_reduced.x * binningX() + roi.x,
224  roi_reduced.y * binningY() + roi.y,
225  roi_reduced.width * binningX(),
226  roi_reduced.height * binningY());
227 }
228 
229 cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const
230 {
231  cv::Rect roi = rectifiedRoi();
232  return cv::Point2d((uv_full.x - roi.x) / binningX(),
233  (uv_full.y - roi.y) / binningY());
234 }
235 
236 cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const
237 {
238  cv::Rect roi = rectifiedRoi();
239  return cv::Rect((roi_full.x - roi.x) / binningX(),
240  (roi_full.y - roi.y) / binningY(),
241  roi_full.width / binningX(),
242  roi_full.height / binningY());
243 }
244 
246 {
247  assert( initialized() );
248 
249  return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
250  cam_info_.roi.width, cam_info_.roi.height);
251 }
252 
254 {
255  assert( initialized() );
256 
257  if (cache_->rectified_roi_dirty)
258  {
259  if (!cam_info_.roi.do_rectify)
260  cache_->rectified_roi = rawRoi();
261  else
262  cache_->rectified_roi = rectifyRoi(rawRoi());
263  cache_->rectified_roi_dirty = false;
264  }
265  return cache_->rectified_roi;
266 }
267 
268 cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const
269 {
270  assert( initialized() );
271  assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane
272 
273  // [U V W]^T = P * [X Y Z 1]^T
274  // u = U/W
275  // v = V/W
276  cv::Point2d uv_rect;
277  uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx();
278  uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy();
279  return uv_rect;
280 }
281 
282 cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const
283 {
284  assert( initialized() );
285 
286  cv::Point3d ray;
287  ray.x = (uv_rect.x - cx() - Tx()) / fx();
288  ray.y = (uv_rect.y - cy() - Ty()) / fy();
289  ray.z = 1.0;
290  return ray;
291 }
292 
293 void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const
294 {
295  assert( initialized() );
296 
297  switch (cache_->distortion_state) {
298  case NONE:
299  raw.copyTo(rectified);
300  break;
301  case CALIBRATED:
303  if (raw.depth() == CV_32F || raw.depth() == CV_64F)
304  {
305  cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
306  }
307  else {
308  cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
309  }
310  break;
311  default:
312  assert(cache_->distortion_state == UNKNOWN);
313  throw Exception("Cannot call rectifyImage when distortion is unknown.");
314  }
315 }
316 
317 void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const
318 {
319  assert( initialized() );
320 
321  throw Exception("PinholeCameraModel::unrectifyImage is unimplemented.");
323  // Similar to rectifyImage, but need to build separate set of inverse maps (raw->rectified)...
324  // - Build src_pt Mat with all the raw pixel coordinates (or do it one row at a time)
325  // - Do cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_)
326  // - Use convertMaps() to convert dst_pt to fast fixed-point maps
327  // - cv::remap(rectified, raw, ...)
328  // Need interpolation argument. Same caching behavior?
329 }
330 
331 cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
332 {
333  assert( initialized() );
334 
335  if (cache_->distortion_state == NONE)
336  return uv_raw;
337  if (cache_->distortion_state == UNKNOWN)
338  throw Exception("Cannot call rectifyPoint when distortion is unknown.");
339  assert(cache_->distortion_state == CALIBRATED);
340 
342  cv::Point2f raw32 = uv_raw, rect32;
343  const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
344  cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);
345  cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);
346  return rect32;
347 }
348 
349 cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
350 {
351  assert( initialized() );
352 
353  if (cache_->distortion_state == NONE)
354  return uv_rect;
355  if (cache_->distortion_state == UNKNOWN)
356  throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
357  assert(cache_->distortion_state == CALIBRATED);
358 
359  // Convert to a ray
360  cv::Point3d ray = projectPixelTo3dRay(uv_rect);
361 
362  // Project the ray on the image
363  cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
364  cv::Rodrigues(R_.t(), r_vec);
365  std::vector<cv::Point2d> image_point;
366  cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);
367 
368  return image_point[0];
369 }
370 
371 cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const
372 {
373  assert( initialized() );
374 
376 
377  // For now, just unrectify the four corners and take the bounding box.
378  cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
379  cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
380  cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
381  roi_raw.y + roi_raw.height));
382  cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height));
383 
384  cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
385  std::ceil (std::min(rect_tl.y, rect_tr.y)));
386  cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)),
387  std::floor(std::max(rect_bl.y, rect_br.y)));
388 
389  return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
390 }
391 
392 cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const
393 {
394  assert( initialized() );
395 
397 
398  // For now, just unrectify the four corners and take the bounding box.
399  cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y));
400  cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y));
401  cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width,
402  roi_rect.y + roi_rect.height));
403  cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height));
404 
405  cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)),
406  std::floor(std::min(raw_tl.y, raw_tr.y)));
407  cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)),
408  std::ceil (std::max(raw_bl.y, raw_br.y)));
409 
410  return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
411 }
412 
414 {
417 
418  if (cache_->full_maps_dirty) {
419  // Create the full-size map at the binned resolution
421  cv::Size binned_resolution = fullResolution();
422  binned_resolution.width /= binningX();
423  binned_resolution.height /= binningY();
424 
425  cv::Matx33d K_binned;
426  cv::Matx34d P_binned;
427  if (binningX() == 1 && binningY() == 1) {
428  K_binned = K_full_;
429  P_binned = P_full_;
430  }
431  else {
432  K_binned = K_full_;
433  P_binned = P_full_;
434  if (binningX() > 1) {
435  double scale_x = 1.0 / binningX();
436  K_binned(0,0) *= scale_x;
437  K_binned(0,2) *= scale_x;
438  P_binned(0,0) *= scale_x;
439  P_binned(0,2) *= scale_x;
440  P_binned(0,3) *= scale_x;
441  }
442  if (binningY() > 1) {
443  double scale_y = 1.0 / binningY();
444  K_binned(1,1) *= scale_y;
445  K_binned(1,2) *= scale_y;
446  P_binned(1,1) *= scale_y;
447  P_binned(1,2) *= scale_y;
448  P_binned(1,3) *= scale_y;
449  }
450  }
451 
452  // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
453  cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
454  CV_16SC2, cache_->full_map1, cache_->full_map2);
455  cache_->full_maps_dirty = false;
456  }
457 
458  if (cache_->reduced_maps_dirty) {
460  cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
461  cam_info_.roi.width, cam_info_.roi.height);
462  if (roi.x != 0 || roi.y != 0 ||
463  roi.height != (int)cam_info_.height ||
464  roi.width != (int)cam_info_.width) {
465 
466  // map1 contains integer (x,y) offsets, which we adjust by the ROI offset
467  // map2 contains LUT index for subpixel interpolation, which we can leave as-is
468  roi.x /= binningX();
469  roi.y /= binningY();
470  roi.width /= binningX();
471  roi.height /= binningY();
472  cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
473  cache_->reduced_map2 = cache_->full_map2(roi);
474  }
475  else {
476  // Otherwise we're rectifying the full image
477  cache_->reduced_map1 = cache_->full_map1;
478  cache_->reduced_map2 = cache_->full_map2;
479  }
480  cache_->reduced_maps_dirty = false;
481  }
482 }
483 
484 } //namespace image_geometry
cv::Rect rectifyRoi(const cv::Rect &roi_raw) const
Compute the rectified ROI best fitting a raw ROI.
cv::Size fullResolution() const
The resolution at which the camera was calibrated.
const std::string RATIONAL_POLYNOMIAL
bool initialized() const
Returns true if the camera has been initialized.
double Ty() const
Returns the y-translation term of the projection matrix.
void unrectifyImage(const cv::Mat &rectified, cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Apply camera distortion to a rectified image.
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
PinholeCameraModel & operator=(const PinholeCameraModel &other)
cv::Point2d toReducedResolution(const cv::Point2d &uv_full) const
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
Project a rectified pixel to a 3d ray.
cv::Point2d toFullResolution(const cv::Point2d &uv_reduced) const
bool update(const T &new_val, T &my_val)
cv::Size reducedResolution() const
The resolution of the current rectified image.
cv::Rect unrectifyRoi(const cv::Rect &roi_rect) const
Compute the raw ROI best fitting a rectified ROI.
double cx() const
Returns the x coordinate of the optical center.
double cy() const
Returns the y coordinate of the optical center.
uint32_t binningX() const
Returns the number of columns in each bin.
cv::Point2d unrectifyPoint(const cv::Point2d &uv_rect) const
Compute the raw image coordinates of a pixel in the rectified image.
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const
Rectify a raw camera image.
bool updateMat(const MatT &new_mat, MatT &my_mat, cv::Mat_< double > &cv_mat, int rows, int cols)
double Tx() const
Returns the x-translation term of the projection matrix.
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
cv::Rect rawRoi() const
The current raw ROI, as used for capture by the camera driver.
cv::Point2d rectifyPoint(const cv::Point2d &uv_raw) const
Compute the rectified image coordinates of a pixel in the raw image.
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
uint32_t binningY() const
Returns the number of rows in each bin.
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
Project a 3d point to rectified pixel coordinates.
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
cv::Rect rectifiedRoi() const
The current rectified ROI, which best fits the raw ROI.


image_geometry
Author(s): Patrick Mihelich
autogenerated on Thu Dec 12 2019 03:52:09