camera_utilities.cpp
Go to the documentation of this file.
1 
34 #include <algorithm>
35 
36 #include <sensor_msgs/distortion_models.h>
37 
38 #include <multisense_ros/camera_utilities.h>
39 
40 namespace multisense_ros {
41 
42 namespace {
43 
44 struct ScaleT
45 {
46  double x_scale = 1.0;
47  double y_scale = 1.0;
48 };
49 
50 ScaleT compute_scale(const crl::multisense::image::Config &config,
51  const crl::multisense::system::DeviceInfo& device_info)
52 {
53  const auto imagerHeight = device_info.imagerHeight;
54 
55  const double x_scale = 1.0 / ((static_cast<double>(device_info.imagerWidth) /
56  static_cast<double>(config.width())));
57 
58  const double y_scale = 1.0 / ((static_cast<double>(imagerHeight) /
59  static_cast<double>(config.height())));
60 
61  return ScaleT{x_scale,
62  y_scale};
63 }
64 
65 }// namespace
66 
67 void ycbcrToBgr(const crl::multisense::image::Header &luma,
68  const crl::multisense::image::Header &chroma,
69  uint8_t *output)
70 {
71  const size_t rgb_stride = luma.width * 3;
72 
73  for(uint32_t y=0; y< luma.height; ++y)
74  {
75  const size_t row_offset = y * rgb_stride;
76 
77  for(uint32_t x=0; x< luma.width; ++x)
78  {
79  memcpy(output + row_offset + (3 * x), ycbcrToBgr<uint8_t>(luma, chroma, x, y).data(), 3);
80  }
81  }
82 }
83 
84 Eigen::Matrix4d makeQ(const crl::multisense::image::Config& config,
85  const crl::multisense::image::Calibration& calibration,
86  const crl::multisense::system::DeviceInfo& device_info)
87 {
88  Eigen::Matrix4d q_matrix = Eigen::Matrix4d::Zero();
89 
90  const auto scale = compute_scale(config, device_info);
91 
92  //
93  // Compute the Q matrix here, as image_geometery::StereoCameraModel does
94  // not allow for non-square pixels.
95  //
96  // FyTx 0 0 -FyCxTx
97  // 0 FxTx 0 -FxCyTx
98  // 0 0 0 FxFyTx
99  // 0 0 -Fy Fy(Cx - Cx')
100  //
101  const auto fx = calibration.left.P[0][0] * scale.x_scale;
102  const auto cx = calibration.left.P[0][2] * scale.x_scale;
103  const auto fy = calibration.left.P[1][1] * scale.y_scale;
104  const auto cy = calibration.left.P[1][2] * scale.y_scale;
105  const auto tx = calibration.right.P[0][3] / calibration.right.P[0][0];
106  const auto cx_prime = calibration.right.P[0][2] * scale.x_scale;
107 
108  q_matrix(0,0) = fy * tx;
109  q_matrix(1,1) = fx * tx;
110  q_matrix(0,3) = -fy * cx * tx;
111  q_matrix(1,3) = -fx * cy * tx;
112  q_matrix(2,3) = fx * fy * tx;
113  q_matrix(3,2) = -fy;
114  q_matrix(3,3) = fy * (cx - cx_prime);
115 
116  return q_matrix;
117 }
118 
119 sensor_msgs::CameraInfo makeCameraInfo(const crl::multisense::image::Config& config,
120  const crl::multisense::image::Calibration::Data& calibration,
121  const ScaleT &scale)
122 {
123  sensor_msgs::CameraInfo camera_info;
124 
125  camera_info.width = config.width();
126  camera_info.height = config.height();
127 
128  camera_info.P[0] = calibration.P[0][0] * scale.x_scale;
129  camera_info.P[1] = calibration.P[0][1];
130  camera_info.P[2] = calibration.P[0][2] * scale.x_scale;
131  camera_info.P[3] = calibration.P[0][3] * scale.x_scale;
132  camera_info.P[4] = calibration.P[1][0];
133  camera_info.P[5] = calibration.P[1][1] * scale.y_scale;
134  camera_info.P[6] = calibration.P[1][2] * scale.y_scale;
135  camera_info.P[7] = calibration.P[1][3];
136  camera_info.P[8] = calibration.P[2][0];
137  camera_info.P[9] = calibration.P[2][1];
138  camera_info.P[10] = calibration.P[2][2];
139  camera_info.P[11] = calibration.P[2][3];
140 
141  camera_info.K[0] = calibration.M[0][0] * scale.x_scale;
142  camera_info.K[1] = calibration.M[0][1];
143  camera_info.K[2] = calibration.M[0][2] * scale.x_scale;
144  camera_info.K[3] = calibration.M[1][0];
145  camera_info.K[4] = calibration.M[1][1] * scale.y_scale;
146  camera_info.K[5] = calibration.M[1][2] * scale.y_scale;
147  camera_info.K[6] = calibration.M[2][0];
148  camera_info.K[7] = calibration.M[2][1];
149  camera_info.K[8] = calibration.M[2][2];
150 
151  camera_info.R[0] = calibration.R[0][0];
152  camera_info.R[1] = calibration.R[0][1];
153  camera_info.R[2] = calibration.R[0][2];
154  camera_info.R[3] = calibration.R[1][0];
155  camera_info.R[4] = calibration.R[1][1];
156  camera_info.R[5] = calibration.R[1][2];
157  camera_info.R[6] = calibration.R[2][0];
158  camera_info.R[7] = calibration.R[2][1];
159  camera_info.R[8] = calibration.R[2][2];
160 
161  //
162  // Distortion coefficients follow OpenCV's convention.
163  // k1, k2, p1, p2, k3, k4, k5, k6
164 
165  camera_info.D.resize(8);
166  for (size_t i=0 ; i < 8 ; ++i)
167  {
168  camera_info.D[i] = calibration.D[i];
169  }
170 
171  //
172  // MultiSense cameras support both the full 8 parameter rational_polynomial
173  // model and the simplified 5 parameter plum_bob model. If the last 3
174  // parameters of the distortion model are 0 then the camera is using
175  // the simplified plumb_bob model
176 
177  if (calibration.D[7] == 0.0 && calibration.D[6] == 0.0 && calibration.D[5] == 0.0)
178  {
179  camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
180  }
181  else
182  {
183  camera_info.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
184  }
185 
186  return camera_info;
187 }
188 
189 RectificationRemapT makeRectificationRemap(const crl::multisense::image::Config& config,
190  const crl::multisense::image::Calibration::Data& calibration,
191  const crl::multisense::system::DeviceInfo& device_info)
192 {
193  RectificationRemapT remap;
194 
195  const auto scale = compute_scale(config, device_info);
196 
197  const cv::Matx33d K(calibration.M[0][0] * scale.x_scale,
198  calibration.M[0][1],
199  calibration.M[0][2] * scale.x_scale,
200  calibration.M[1][0],
201  calibration.M[1][1] * scale.y_scale,
202  calibration.M[1][2] * scale.y_scale,
203  calibration.M[2][0],
204  calibration.M[2][1],
205  calibration.M[2][2]);
206 
207  const cv::Matx33d R(calibration.R[0][0],
208  calibration.R[0][1],
209  calibration.R[0][2],
210  calibration.R[1][0],
211  calibration.R[1][1],
212  calibration.R[1][2],
213  calibration.R[2][0],
214  calibration.R[2][1],
215  calibration.R[2][2]);
216 
217  const cv::Matx34d P(calibration.P[0][0] * scale.x_scale,
218  calibration.P[0][1],
219  calibration.P[0][2] * scale.x_scale,
220  calibration.P[0][3] * scale.x_scale,
221  calibration.P[1][0],
222  calibration.P[1][1] * scale.y_scale,
223  calibration.P[1][2] * scale.y_scale,
224  calibration.P[1][3],
225  calibration.P[2][0],
226  calibration.P[2][1],
227  calibration.P[2][2],
228  calibration.P[2][3]);
229 
230  const bool plumbob = calibration.D[7] == 0.0 && calibration.D[6] == 0.0 && calibration.D[5] == 0.0;
231 
232  cv::Mat D(plumbob ? 5 : 8, 1, CV_64FC1);
233  for (int i = 0; i < D.rows ; ++i)
234  {
235  D.at<double>(i) = calibration.D[i];
236  }
237 
238  cv::initUndistortRectifyMap(K, D, R, P, cv::Size(config.width(), config.height()), CV_32FC1, remap.map1, remap.map2);
239 
240  return remap;
241 }
242 
243 StereoCalibrationManager::StereoCalibrationManager(const crl::multisense::image::Config& config,
244  const crl::multisense::image::Calibration& calibration,
245  const crl::multisense::system::DeviceInfo& device_info):
246  config_(config),
247  calibration_(calibration),
248  device_info_(device_info),
249  q_matrix_(makeQ(config_, calibration_, device_info_)),
250  left_camera_info_(makeCameraInfo(config_, calibration_.left, compute_scale(config_, device_info_))),
251  right_camera_info_(makeCameraInfo(config_, calibration_.right, compute_scale(config_, device_info_))),
252  aux_camera_info_(makeCameraInfo(config_, calibration_.aux, config_.cameraProfile() == crl::multisense::Full_Res_Aux_Cam ?
253  ScaleT{1., 1.} : compute_scale(config_, device_info_))),
254  left_remap_(std::make_shared<RectificationRemapT>(makeRectificationRemap(config_, calibration_.left, device_info_))),
255  right_remap_(std::make_shared<RectificationRemapT>(makeRectificationRemap(config_, calibration_.right, device_info_)))
256 {
257 }
258 
259 void StereoCalibrationManager::updateConfig(const crl::multisense::image::Config& config)
260 {
261  //
262  // Only update the calibration if the resolution changed.
263 
264  if (config_.width() == config.width() && config_.height() == config.height() && config_.cameraProfile() == config.cameraProfile())
265  {
266  std::lock_guard<std::mutex> lock(mutex_);
267  config_ = config;
268  return;
269  }
270 
271  auto q_matrix = makeQ(config, calibration_, device_info_);
272  auto left_camera_info = makeCameraInfo(config, calibration_.left, compute_scale(config, device_info_));
273  auto right_camera_info = makeCameraInfo(config, calibration_.right, compute_scale(config, device_info_));
274 
275  const ScaleT aux_scale = config.cameraProfile() == crl::multisense::Full_Res_Aux_Cam ?
276  ScaleT{1., 1.} : compute_scale(config, device_info_);
277 
278  auto aux_camera_info = makeCameraInfo(config, calibration_.aux, aux_scale);
279  auto left_remap = std::make_shared<RectificationRemapT>(makeRectificationRemap(config, calibration_.left, device_info_));
280  auto right_remap = std::make_shared<RectificationRemapT>(makeRectificationRemap(config, calibration_.right, device_info_));
281 
282  std::lock_guard<std::mutex> lock(mutex_);
283 
284  config_ = config;
285  q_matrix_ = std::move(q_matrix);
286  left_camera_info_ = std::move(left_camera_info);
287  right_camera_info_ = std::move(right_camera_info);
288  aux_camera_info_ = std::move(aux_camera_info);
289  left_remap_ = left_remap;
290  right_remap_ = right_remap;
291 }
292 
293 crl::multisense::image::Config StereoCalibrationManager::config() const
294 {
295  std::lock_guard<std::mutex> lock(mutex_);
296 
297  return config_;
298 }
299 
300 Eigen::Matrix4d StereoCalibrationManager::Q() const
301 {
302  std::lock_guard<std::mutex> lock(mutex_);
303 
304  return q_matrix_;
305 }
306 
307 double StereoCalibrationManager::T() const
308 {
309  std::lock_guard<std::mutex> lock(mutex_);
310 
311  //
312  // The right camera projection matrix is of the form:
313  //
314  // [fx, 0, cx, t * fx]
315  // [ 0, fy, cy, 0 ]
316  // [ 0, 0, 1, 0 ]
317  //
318  // divide the t * fx term by fx to get t
319 
320  return right_camera_info_.P[3] / right_camera_info_.P[0];
321 }
322 
323 Eigen::Vector3d StereoCalibrationManager::aux_T() const
324 {
325  std::lock_guard<std::mutex> lock(mutex_);
326 
327  //
328  // The aux camera projection matrix is of the form:
329  //
330  // [fx, 0, cx, tx * fx]
331  // [ 0, fy, cy, ty * fy]
332  // [ 0, 0, 1, tz ]
333  //
334  // divide the t * fx term by fx to get t
335 
336  return Eigen::Vector3d{aux_camera_info_.P[3] / aux_camera_info_.P[0],
337  aux_camera_info_.P[7] / aux_camera_info_.P[5],
338  aux_camera_info_.P[11]};
339 }
340 
341 Eigen::Vector3f StereoCalibrationManager::reproject(size_t u, size_t v, double d) const
342 {
343  std::lock_guard<std::mutex> lock(mutex_);
344 
345  return reproject(u, v, d, left_camera_info_, right_camera_info_);
346 }
347 
348 Eigen::Vector3f StereoCalibrationManager::reproject(size_t u,
349  size_t v,
350  double d,
351  const sensor_msgs::CameraInfo &left_camera_info,
352  const sensor_msgs::CameraInfo &right_camera_info)
353 {
354  if (d == 0.0)
355  {
356  return Eigen::Vector3f{std::numeric_limits<float>::max(),
357  std::numeric_limits<float>::max(),
358  std::numeric_limits<float>::max()};
359  }
360 
361 
362  const double &fx = left_camera_info.P[0];
363  const double &fy = left_camera_info.P[5];
364  const double &cx = left_camera_info.P[2];
365  const double &cy = left_camera_info.P[6];
366  const double &cx_right = right_camera_info.P[2];
367  const double tx = right_camera_info.P[3] / right_camera_info.P[0];
368 
369  const double xB = ((fy * tx) * u) + (-fy * cx * tx);
370  const double yB = ((fx * tx) * v) + (-fx * cy * tx);
371  const double zB = (fx * fy * tx);
372  const double invB = 1. / (-fy * d) + (fy * (cx - cx_right));
373 
374  return Eigen::Vector3f{static_cast<float>(xB * invB), static_cast<float>(yB * invB), static_cast<float>(zB * invB)};
375 }
376 
377 bool StereoCalibrationManager::isValidReprojectedPoint(const Eigen::Vector3f& pt, const float squared_max_range)
378 {
379  return pt[2] > 0.0f && std::isfinite(pt[2]) && pt.squaredNorm() < squared_max_range;
380 }
381 
382 Eigen::Vector2f StereoCalibrationManager::rectifiedAuxProject(const Eigen::Vector3f &left_rectified_point) const
383 {
384  std::lock_guard<std::mutex> lock(mutex_);
385 
386  return rectifiedAuxProject(left_rectified_point, aux_camera_info_);
387 }
388 
389 Eigen::Vector2f StereoCalibrationManager::rectifiedAuxProject(const Eigen::Vector3f &left_rectified_point,
390  const sensor_msgs::CameraInfo &aux_camera_info)
391 {
392  const double &fx = aux_camera_info.P[0];
393  const double &fy = aux_camera_info.P[5];
394  const double &cx = aux_camera_info.P[2];
395  const double &cy = aux_camera_info.P[6];
396  const double &fxtx = aux_camera_info.P[3];
397  const double &fyty = aux_camera_info.P[7];
398  const double &tz = aux_camera_info.P[11];
399 
400  //
401  // Project the left_rectified_point into the aux image using the auxP matrix
402  //
403  const double uB = (fx * left_rectified_point(0) + (cx * left_rectified_point(2)) + fxtx);
404  const double vB = (fy * left_rectified_point(1) + (cy * left_rectified_point(2)) + fyty);
405  const double invB = 1.0 / (left_rectified_point(2) + tz);
406 
407  return Eigen::Vector2f{static_cast<float>(uB * invB), static_cast<float>(vB * invB)};
408 }
409 
410 OperatingResolutionT StereoCalibrationManager::operatingStereoResolution() const
411 {
412  std::lock_guard<std::mutex> lock(mutex_);
413 
414  return OperatingResolutionT{config_.width(), config_.height()};
415 }
416 
417 OperatingResolutionT StereoCalibrationManager::operatingAuxResolution() const
418 {
419  std::lock_guard<std::mutex> lock(mutex_);
420 
421  if (config_.cameraProfile() == crl::multisense::Full_Res_Aux_Cam)
422  {
423  return OperatingResolutionT{S30_AUX_CAM_WIDTH, S30_AUX_CAM_HEIGHT};
424  }
425 
426  const auto scale = compute_scale(config_, device_info_);
427 
428  return OperatingResolutionT{config_.width(), static_cast<size_t>(S30_AUX_CAM_HEIGHT * scale.y_scale)};
429 
430 }
431 
432 bool StereoCalibrationManager::validRight() const
433 {
434  return std::isfinite(T());
435 }
436 
437 bool StereoCalibrationManager::validAux() const
438 {
439  const Eigen::Vector3d auxT = aux_T();
440  return std::isfinite(auxT(0)) && std::isfinite(auxT(1)) && std::isfinite(auxT(2)) ;
441 }
442 
443 sensor_msgs::CameraInfo StereoCalibrationManager::leftCameraInfo(const std::string& frame_id,
444  const ros::Time& stamp) const
445 {
446  std::lock_guard<std::mutex> lock(mutex_);
447 
448  auto camera_info = left_camera_info_;
449  camera_info.header.frame_id = frame_id;
450  camera_info.header.stamp = stamp;
451 
452  return camera_info;
453 }
454 
455 sensor_msgs::CameraInfo StereoCalibrationManager::rightCameraInfo(const std::string& frame_id,
456  const ros::Time& stamp) const
457 {
458  std::lock_guard<std::mutex> lock(mutex_);
459 
460  auto camera_info = right_camera_info_;
461  camera_info.header.frame_id = frame_id;
462  camera_info.header.stamp = stamp;
463 
464  return camera_info;
465 }
466 sensor_msgs::CameraInfo StereoCalibrationManager::auxCameraInfo(const std::string& frame_id,
467  const ros::Time& stamp,
468  const OperatingResolutionT &resolution) const
469 {
470  return auxCameraInfo(frame_id, stamp, resolution.width, resolution.height);
471 }
472 
473 sensor_msgs::CameraInfo StereoCalibrationManager::auxCameraInfo(const std::string& frame_id,
474  const ros::Time& stamp,
475  size_t width,
476  size_t height) const
477 {
478  std::lock_guard<std::mutex> lock(mutex_);
479 
480  auto camera_info = aux_camera_info_;
481  camera_info.header.frame_id = frame_id;
482  camera_info.header.stamp = stamp;
483  camera_info.width = width;
484  camera_info.height = height;
485 
486  return camera_info;
487 }
488 
489 std::shared_ptr<RectificationRemapT> StereoCalibrationManager::leftRemap() const
490 {
491  std::lock_guard<std::mutex> lock(mutex_);
492 
493  return left_remap_;
494 }
495 
496 std::shared_ptr<RectificationRemapT> StereoCalibrationManager::rightRemap() const
497 {
498  std::lock_guard<std::mutex> lock(mutex_);
499 
500  return right_remap_;
501 }
502 
503 bool clipPoint(const BorderClip& borderClipType,
504  double borderClipValue,
505  size_t height,
506  size_t width,
507  size_t u,
508  size_t v)
509 {
510  switch (borderClipType)
511  {
512  case BorderClip::NONE:
513  {
514  return false;
515  }
516  case BorderClip::RECTANGULAR:
517  {
518  return !( u >= borderClipValue && u <= width - borderClipValue &&
519  v >= borderClipValue && v <= height - borderClipValue);
520  }
521  case BorderClip::CIRCULAR:
522  {
523  const double halfWidth = static_cast<double>(width)/2.0;
524  const double halfHeight = static_cast<double>(height)/2.0;
525 
526  const double radius = sqrt( halfWidth * halfWidth + halfHeight * halfHeight ) - borderClipValue;
527 
528  return !(Eigen::Vector2d{halfWidth - u, halfHeight - v}.norm() < radius);
529  }
530  default:
531  {
532  ROS_WARN("Camera: Unknown border clip type.");
533  break;
534  }
535  }
536 
537  return true;
538 }
539 
540 cv::Vec3b interpolateColor(const Eigen::Vector2f &pixel, const cv::Mat &image)
541 {
542  const float width = image.cols;
543  const float height = image.rows;
544 
545  const float &u = pixel(0);
546  const float &v = pixel(1);
547 
548  //
549  // Implement a basic bileinar interpolation scheme
550  // https://en.wikipedia.org/wiki/Bilinear_interpolation
551  //
552  const size_t min_u = static_cast<size_t>(std::min(std::max(std::floor(u), 0.f), width - 1.f));
553  const size_t max_u = static_cast<size_t>(std::min(std::max(std::floor(u) + 1, 0.f), width - 1.f));
554  const size_t min_v = static_cast<size_t>(std::min(std::max(std::floor(v), 0.f), height - 1.f));
555  const size_t max_v = static_cast<size_t>(std::min(std::max(std::floor(v) + 1, 0.f), height - 1.f));
556 
557  const cv::Vec3d element00 = image.at<cv::Vec3b>(width * min_v + min_u);
558  const cv::Vec3d element01 = image.at<cv::Vec3b>(width * min_v + max_u);
559  const cv::Vec3d element10 = image.at<cv::Vec3b>(width * max_v + min_u);
560  const cv::Vec3d element11 = image.at<cv::Vec3b>(width * max_v + max_u);
561 
562  const size_t delta_u = max_u - min_u;
563  const size_t delta_v = max_v - min_v;
564 
565  const double u_ratio = delta_u == 0 ? 1. : (static_cast<double>(max_u) - u) / static_cast<double>(delta_u);
566  const double v_ratio = delta_v == 0 ? 1. : (static_cast<double>(max_v) - v) / static_cast<double>(delta_v);
567 
568  const cv::Vec3b f_xy0 = element00 * u_ratio + element01 * (1. - u_ratio);
569  const cv::Vec3b f_xy1 = element10 * u_ratio + element11 * (1. - u_ratio);
570 
571  return (f_xy0 * v_ratio + f_xy1 * (1. - v_ratio));
572 }
573 
574 }// namespace
multisense_ros::OperatingResolutionT
Definition: camera_utilities.h:56
multisense_ros::StereoCalibrationManager::aux_camera_info_
sensor_msgs::CameraInfo aux_camera_info_
Definition: camera_utilities.h:266
multisense_ros::StereoCalibrationManager::q_matrix_
Eigen::Matrix4d q_matrix_
Definition: camera_utilities.h:262
sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL
const std::string RATIONAL_POLYNOMIAL
multisense_ros::interpolateColor
cv::Vec3b interpolateColor(const Eigen::Vector2f &pixel, const cv::Mat &image)
Given a floating point pixel location, determine its average color using all neighboring pixels.
Definition: camera_utilities.cpp:540
multisense_ros::StereoCalibrationManager::reproject
Eigen::Vector3f reproject(size_t u, size_t v, double d) const
Reproject disparity values into 3D.
Definition: camera_utilities.cpp:341
multisense_ros::BorderClip::RECTANGULAR
@ RECTANGULAR
y_scale
double y_scale
Definition: camera_utilities.cpp:47
crl::multisense::image::Header::height
uint32_t height
distortion_models.h
x_scale
double x_scale
Definition: camera_utilities.cpp:46
multisense_ros::StereoCalibrationManager::config_
crl::multisense::image::Config config_
Definition: camera_utilities.h:250
crl::multisense::image::Calibration::Data
multisense_ros::StereoCalibrationManager::config
crl::multisense::image::Config config() const
Definition: camera_utilities.cpp:293
multisense_ros::S30_AUX_CAM_WIDTH
static constexpr size_t S30_AUX_CAM_WIDTH
Definition: camera_utilities.h:53
crl::multisense::image::Config::cameraProfile
CameraProfile cameraProfile() const
crl::multisense::image::Calibration::Data::P
float P[3][4]
crl::multisense::Full_Res_Aux_Cam
static CRL_CONSTEXPR CameraProfile Full_Res_Aux_Cam
crl::multisense::image::Header::width
uint32_t width
multisense_ros::StereoCalibrationManager::calibration_
const crl::multisense::image::Calibration calibration_
Definition: camera_utilities.h:251
crl::multisense::image::Calibration::aux
Data aux
crl::multisense::image::Calibration::Data::M
float M[3][3]
crl::multisense::image::Calibration::left
Data left
multisense_ros::StereoCalibrationManager::validAux
bool validAux() const
Determine if the Aux calibration is valid.
Definition: camera_utilities.cpp:437
crl::multisense::image::Config::width
uint32_t width() const
multisense_ros::RectificationRemapT
Definition: camera_utilities.h:64
data
data
multisense_ros::StereoCalibrationManager::T
double T() const
Translation which transforms points from the rectified left camera frame into the recified right came...
Definition: camera_utilities.cpp:307
f
f
crl::multisense::system::DeviceInfo::imagerWidth
uint32_t imagerWidth
multisense_ros::BorderClip::CIRCULAR
@ CIRCULAR
multisense_ros::StereoCalibrationManager::right_camera_info_
sensor_msgs::CameraInfo right_camera_info_
Definition: camera_utilities.h:265
multisense_ros::clipPoint
bool clipPoint(const BorderClip &borderClipType, double borderClipValue, size_t height, size_t width, size_t u, size_t v)
Determine if a pixel should be clipped given the provided parameters.
Definition: camera_utilities.cpp:503
multisense_ros::StereoCalibrationManager::device_info_
const crl::multisense::system::DeviceInfo & device_info_
Definition: camera_utilities.h:252
crl
crl::multisense::image::Config
multisense_ros::StereoCalibrationManager::validRight
bool validRight() const
Determine if the Right calibration is valid.
Definition: camera_utilities.cpp:432
crl::multisense::image::Config::height
uint32_t height() const
crl::multisense::image::Calibration::Data::D
float D[8]
multisense_ros
Definition: camera.h:58
crl::multisense::system::DeviceInfo
multisense_ros::StereoCalibrationManager::Q
Eigen::Matrix4d Q() const
Q stereo reprojection matrix see: https://docs.opencv.org/4.3.0/d9/d0c/group__calib3d....
Definition: camera_utilities.cpp:300
multisense_ros::StereoCalibrationManager::rightCameraInfo
sensor_msgs::CameraInfo rightCameraInfo(const std::string &frame_id, const ros::Time &stamp) const
Definition: camera_utilities.cpp:455
crl::multisense::image::Calibration
d
d
multisense_ros::S30_AUX_CAM_HEIGHT
static constexpr size_t S30_AUX_CAM_HEIGHT
Definition: camera_utilities.h:54
ROS_WARN
#define ROS_WARN(...)
multisense_ros::BorderClip::NONE
@ NONE
multisense_ros::StereoCalibrationManager::StereoCalibrationManager
EIGEN_MAKE_ALIGNED_OPERATOR_NEW StereoCalibrationManager(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration &calibration, const crl::multisense::system::DeviceInfo &device_info)
Definition: camera_utilities.cpp:243
multisense_ros::BorderClip
BorderClip
Definition: camera_utilities.h:62
multisense_ros::StereoCalibrationManager::leftRemap
std::shared_ptr< RectificationRemapT > leftRemap() const
Definition: camera_utilities.cpp:489
multisense_ros::OperatingResolutionT::width
size_t width
Definition: camera_utilities.h:58
remap
ROSCPP_DECL std::string remap(const std::string &name)
multisense_ros::StereoCalibrationManager::operatingStereoResolution
OperatingResolutionT operatingStereoResolution() const
Get the current main stereo pair operating resolution. This resolution applies for both the mono and ...
Definition: camera_utilities.cpp:410
multisense_ros::ycbcrToBgr
constexpr Eigen::Matrix< T, 3, 1 > ycbcrToBgr(const crl::multisense::image::Header &luma, const crl::multisense::image::Header &chroma, size_t u, size_t v)
Definition: camera_utilities.h:104
multisense_ros::StereoCalibrationManager::left_camera_info_
sensor_msgs::CameraInfo left_camera_info_
Definition: camera_utilities.h:264
multisense_ros::StereoCalibrationManager::updateConfig
void updateConfig(const crl::multisense::image::Config &config)
Definition: camera_utilities.cpp:259
multisense_ros::StereoCalibrationManager::rightRemap
std::shared_ptr< RectificationRemapT > rightRemap() const
Definition: camera_utilities.cpp:496
multisense_ros::StereoCalibrationManager::operatingAuxResolution
OperatingResolutionT operatingAuxResolution() const
Get the current aux camera operating resolution. This resolution applies for just the mono topics....
Definition: camera_utilities.cpp:417
multisense_ros::StereoCalibrationManager::auxCameraInfo
sensor_msgs::CameraInfo auxCameraInfo(const std::string &frame_id, const ros::Time &stamp, const OperatingResolutionT &resolution) const
Definition: camera_utilities.cpp:466
camera_utilities.h
multisense_ros::StereoCalibrationManager::leftCameraInfo
sensor_msgs::CameraInfo leftCameraInfo(const std::string &frame_id, const ros::Time &stamp) const
Definition: camera_utilities.cpp:443
multisense_ros::OperatingResolutionT::height
size_t height
Definition: camera_utilities.h:59
ros::Time
multisense_ros::makeCameraInfo
sensor_msgs::CameraInfo makeCameraInfo(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration::Data &calibration, const crl::multisense::system::DeviceInfo &device_info, bool scale_calibration)
crl::multisense::image::Calibration::Data::R
float R[3][3]
multisense_ros::makeRectificationRemap
RectificationRemapT makeRectificationRemap(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration::Data &calibration, const crl::multisense::system::DeviceInfo &device_info)
Definition: camera_utilities.cpp:189
multisense_ros::StereoCalibrationManager::left_remap_
std::shared_ptr< RectificationRemapT > left_remap_
Definition: camera_utilities.h:268
multisense_ros::StereoCalibrationManager::right_remap_
std::shared_ptr< RectificationRemapT > right_remap_
Definition: camera_utilities.h:269
multisense_ros::StereoCalibrationManager::aux_T
Eigen::Vector3d aux_T() const
Translation which transforms points from the rectified left camera frame into the rectified aux camer...
Definition: camera_utilities.cpp:323
multisense_ros::StereoCalibrationManager::rectifiedAuxProject
Eigen::Vector2f rectifiedAuxProject(const Eigen::Vector3f &left_rectified_point) const
Project points corresponding to disparity measurements in the left rectified image frame into the aux...
Definition: camera_utilities.cpp:382
multisense_ros::StereoCalibrationManager::mutex_
std::mutex mutex_
Definition: camera_utilities.h:257
multisense_ros::makeQ
Eigen::Matrix4d makeQ(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration &calibration, const crl::multisense::system::DeviceInfo &device_info)
Definition: camera_utilities.cpp:84
sensor_msgs::distortion_models::PLUMB_BOB
const std::string PLUMB_BOB
crl::multisense::image::Calibration::right
Data right
crl::multisense::system::DeviceInfo::imagerHeight
uint32_t imagerHeight
multisense_ros::StereoCalibrationManager::isValidReprojectedPoint
static bool isValidReprojectedPoint(const Eigen::Vector3f &pt, const float squared_max_range)
Check if a reprojected point falls within a valid range.
Definition: camera_utilities.cpp:377
crl::multisense::image::Header

multisense_ros
Author(s):
autogenerated on Thu Feb 20 2025 03:51:03