3 #include <opencv2/calib3d/calib3d.hpp>
4 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
5 #include <boost/make_shared.hpp>
66 bool update(
const T& new_val, T& my_val)
68 if (my_val == new_val)
75 template<
typename MatT>
76 bool updateMat(
const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat,
int rows,
int cols)
78 if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
82 cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
86 template<
typename MatT,
typename MatU>
87 bool updateMat(
const MatT& new_mat, MatT& my_mat, MatU& cv_mat)
89 if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
93 cv_mat = MatU(&my_mat[0]);
101 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED
102 cache_ = boost::make_shared<Cache>();
104 cache_ = std::make_shared<Cache>();
108 uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
109 uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
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;
123 bool full_dirty =
false;
126 full_dirty |=
update(msg.distortion_model,
cam_info_.distortion_model);
133 cache_->full_maps_dirty |= full_dirty;
134 cache_->unrectify_full_maps_dirty |= full_dirty;
138 bool reduced_dirty = full_dirty;
144 cache_->reduced_maps_dirty |= reduced_dirty;
146 cache_->unrectify_reduced_maps_dirty |= reduced_dirty;
147 cache_->unrectify_reduced_maps_dirty |=
cache_->unrectify_full_maps_dirty;
150 cache_->rectified_roi_dirty |= reduced_dirty;
158 for (
size_t i = 0; i <
cam_info_.D.size(); ++i)
183 bool adjust_binning = (binning_x > 1) || (binning_y > 1);
184 bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
186 if (!adjust_binning && !adjust_roi) {
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;
205 double scale_x = 1.0 / binning_x;
213 double scale_y = 1.0 / binning_y;
222 return reduced_dirty;
247 return cv::Point2d(uv_reduced.x *
binningX() + roi.x,
254 return cv::Rect(roi_reduced.x *
binningX() + roi.x,
263 return cv::Point2d((uv_full.x - roi.x) /
binningX(),
270 return cv::Rect((roi_full.x - roi.x) /
binningX(),
288 if (
cache_->rectified_roi_dirty)
294 cache_->rectified_roi_dirty =
false;
296 return cache_->rectified_roi;
302 assert(
P_(2, 3) == 0.0);
308 uv_rect.x = (
fx()*xyz.x +
Tx()) / xyz.z +
cx();
309 uv_rect.y = (
fy()*xyz.y +
Ty()) / xyz.z +
cy();
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);
330 ray.x = (uv_rect.x -
cx -
Tx) /
fx;
331 ray.y = (uv_rect.y -
cy -
Ty) /
fy;
340 switch (
cache_->distortion_state) {
342 raw.copyTo(rectified);
346 if (raw.depth() == CV_32F || raw.depth() == CV_64F)
348 cv::remap(raw, rectified,
cache_->reduced_map1,
cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
351 cv::remap(raw, rectified,
cache_->reduced_map1,
cache_->reduced_map2, interpolation);
356 throw Exception(
"Cannot call rectifyImage when distortion is unknown.");
364 switch (
cache_->distortion_state) {
366 rectified.copyTo(raw);
370 if (rectified.depth() == CV_32F || rectified.depth() == CV_64F)
372 cv::remap(rectified, raw,
cache_->unrectify_reduced_map1,
cache_->unrectify_reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
375 cv::remap(rectified, raw,
cache_->unrectify_reduced_map1,
cache_->unrectify_reduced_map2, interpolation);
380 throw Exception(
"Cannot call rectifyImage when distortion is unknown.");
396 throw Exception(
"Cannot call rectifyPoint when distortion is unknown.");
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);
404 switch (
cache_->distortion_model) {
406 cv::undistortPoints(src_pt, dst_pt, K,
D_,
R_, P);
409 cv::fisheye::undistortPoints(src_pt, dst_pt, K,
D_,
R_, P);
413 throw Exception(
"Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
430 throw Exception(
"Cannot call unrectifyPoint when distortion is unknown.");
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;
441 switch (
cache_->distortion_model) {
443 cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K,
D_, image_point);
446 cv::fisheye::projectPoints(std::vector<cv::Point3d>(1, ray), image_point, r_vec, t_vec, K,
D_);
450 throw Exception(
"Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
453 return image_point[0];
466 cv::Point2d rect_br =
rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
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)));
475 return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
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));
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)));
496 return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
504 if (
cache_->full_maps_dirty) {
508 binned_resolution.width /=
binningX();
509 binned_resolution.height /=
binningY();
511 cv::Matx33d K_binned;
512 cv::Matx34d P_binned;
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;
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;
538 switch (
cache_->distortion_model) {
541 cv::initUndistortRectifyMap(K_binned,
D_,
R_, P_binned, binned_resolution,
545 cv::fisheye::initUndistortRectifyMap(K_binned,
D_,
R_, P_binned, binned_resolution,
550 throw Exception(
"Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
552 cache_->full_maps_dirty =
false;
555 if (
cache_->reduced_maps_dirty) {
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)) {
569 cache_->reduced_map1 =
cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
577 cache_->reduced_maps_dirty =
false;
586 if (
cache_->unrectify_full_maps_dirty) {
590 binned_resolution.width /=
binningX();
591 binned_resolution.height /=
binningY();
593 cv::Matx33d K_binned;
594 cv::Matx34d P_binned;
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;
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;
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;
626 float_map_x.at<
float>(y, x) = uv_rect.x;
627 float_map_y.at<
float>(y, x) = uv_rect.y;
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;
635 if (
cache_->unrectify_reduced_maps_dirty) {
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)) {
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);
654 cache_->unrectify_reduced_map1 =
cache_->unrectify_full_map1;
655 cache_->unrectify_reduced_map2 =
cache_->unrectify_full_map2;
657 cache_->unrectify_reduced_maps_dirty =
false;