12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <Eigen/Dense>
23 #include <opencv2/core/eigen.hpp>
27 const cv::Mat& src_right, cv::Mat& out_left, cv::Mat& out_right,
28 int16_t* map_xl, int16_t* map_xr, uint16_t* map_yl, uint16_t* map_yr,
33 src_left.data != out_left.data && src_right.data != out_right.data,
34 "in-place rectify not supported");
38 "Error: setFromCamParams() must be called prior to rectify().");
48 const CvMat mapx_left = cvMat(nrows_out, ncols_out, CV_16SC2, map_xl);
49 const CvMat mapy_left = cvMat(nrows_out, ncols_out, CV_16UC1, map_yl);
50 const CvMat mapx_right = cvMat(nrows_out, ncols_out, CV_16SC2, map_xr);
51 const CvMat mapy_right = cvMat(nrows_out, ncols_out, CV_16UC1, map_yr);
53 const cv::Mat mapx1 = cv::cvarrToMat(&mapx_left);
54 const cv::Mat mapy1 = cv::cvarrToMat(&mapy_left);
55 const cv::Mat mapx2 = cv::cvarrToMat(&mapx_right);
56 const cv::Mat mapy2 = cv::cvarrToMat(&mapy_right);
59 src_left, out_left, mapx1, mapy1, interp_method, cv::BORDER_CONSTANT,
62 src_right, out_right, mapx2, mapy2, interp_method, cv::BORDER_CONSTANT,
68 void CStereoRectifyMap::internal_invalidate()
71 m_dat_mapx_left.clear();
72 m_dat_mapx_right.clear();
73 m_dat_mapy_left.clear();
74 m_dat_mapy_right.clear();
77 void CStereoRectifyMap::setAlpha(
double alpha)
81 this->internal_invalidate();
84 void CStereoRectifyMap::enableResizeOutput(
85 bool enable,
unsigned int target_width,
unsigned int target_height)
87 m_resize_output = enable;
88 m_resize_output_value.x = target_width;
89 m_resize_output_value.y = target_height;
91 this->internal_invalidate();
94 void CStereoRectifyMap::enableBothCentersCoincide(
bool enable)
96 m_enable_both_centers_coincide = enable;
98 this->internal_invalidate();
113 const uint32_t ncols = cam1.ncols;
114 const uint32_t nrows = cam1.nrows;
116 const cv::Size trg_size =
119 m_resize_output_value.x,
120 m_resize_output_value.y)
123 const uint32_t ncols_out =
124 m_resize_output ? m_resize_output_value.x : ncols;
125 const uint32_t nrows_out =
126 m_resize_output ? m_resize_output_value.y : nrows;
132 m_dat_mapx_left.resize(2 * nrows_out * ncols_out);
133 m_dat_mapy_left.resize(nrows_out * ncols_out);
135 m_dat_mapx_right.resize(2 * nrows_out * ncols_out);
136 m_dat_mapy_right.resize(nrows_out * ncols_out);
139 cvMat(nrows_out, ncols_out, CV_16SC2, &m_dat_mapx_left[0]);
141 cvMat(nrows_out, ncols_out, CV_16UC1, &m_dat_mapy_left[0]);
143 cvMat(nrows_out, ncols_out, CV_16SC2, &m_dat_mapx_right[0]);
145 cvMat(nrows_out, ncols_out, CV_16UC1, &m_dat_mapy_right[0]);
147 cv::Mat _mapx_left = cv::cvarrToMat(&mapx_left,
false);
148 cv::Mat _mapy_left = cv::cvarrToMat(&mapy_left,
false);
149 cv::Mat _mapx_right = cv::cvarrToMat(&mapx_right,
false);
150 cv::Mat _mapy_right = cv::cvarrToMat(&mapy_right,
false);
160 for (
unsigned int i = 0; i < 3; ++i)
161 for (
unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);
164 double rcTrans[3] = {hMatrix(0, 3), hMatrix(1, 3), hMatrix(2, 3)};
166 double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
167 for (
unsigned int i = 0; i < 3; ++i)
168 for (
unsigned int j = 0; j < 3; ++j)
170 ipl[i][j] = cam1.intrinsicParams(i, j);
174 for (
unsigned int i = 0; i < 5; ++i)
176 dpl[i] = cam1.dist[i];
177 dpr[i] = cam2.
dist[i];
180 const cv::Mat
R(3, 3, CV_64F, &m1);
181 const cv::Mat T(3, 1, CV_64F, &rcTrans);
183 const cv::Mat K1(3, 3, CV_64F, ipl);
184 const cv::Mat K2(3, 3, CV_64F, ipr);
185 const cv::Mat D1(1, 5, CV_64F, dpl);
186 const cv::Mat D2(1, 5, CV_64F, dpr);
188 double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
189 cv::Mat R1(3, 3, CV_64F, _R1);
190 cv::Mat R2(3, 3, CV_64F, _R2);
191 cv::Mat P1(3, 4, CV_64F, _P1);
192 cv::Mat P2(3, 4, CV_64F, _P2);
193 cv::Mat Q(4, 4, CV_64F, _Q);
195 const cv::Size img_size(ncols, nrows);
196 const cv::Size real_trg_size =
197 m_resize_output ? trg_size
202 K1, D1, K2, D2, img_size,
R, T, R1, R2, P1, P2, Q,
203 m_enable_both_centers_coincide ? cv::CALIB_ZERO_DISPARITY : 0, m_alpha,
208 cv::initUndistortRectifyMap(
209 K1, D1, R1, P1, real_trg_size, CV_16SC2, _mapx_left, _mapy_left);
210 cv::initUndistortRectifyMap(
211 K2, D2, R2, P2, real_trg_size, CV_16SC2, _mapx_right, _mapy_right);
214 for (
unsigned int i = 0; i < 3; ++i)
215 for (
unsigned int j = 0; j < 3; ++j)
217 m_rectified_image_params.leftCamera.intrinsicParams(i, j) =
219 m_rectified_image_params.rightCamera.intrinsicParams(i, j) =
223 m_rectified_image_params.leftCamera.dist.fill(0);
224 m_rectified_image_params.rightCamera.dist.fill(0);
227 m_rectified_image_params.leftCamera.ncols = real_trg_size.width;
228 m_rectified_image_params.leftCamera.nrows = real_trg_size.height;
230 m_rectified_image_params.rightCamera.ncols = real_trg_size.width;
231 m_rectified_image_params.rightCamera.nrows = real_trg_size.height;
234 m_rectified_image_params.leftCamera.focalLengthMeters =
235 params.leftCamera.focalLengthMeters;
236 m_rectified_image_params.rightCamera.focalLengthMeters =
237 params.rightCamera.focalLengthMeters;
243 cv::cv2eigen(R1, Re);
250 cv::cv2eigen(R2, Re);
256 m_rectified_image_params.rightCameraPose =
params.rightCameraPose;
264 void CStereoRectifyMap::rectify(
272 const uint32_t ncols = m_camera_params.leftCamera.ncols;
273 const uint32_t nrows = m_camera_params.leftCamera.nrows;
275 const CvSize trg_size =
277 ? cvSize(m_resize_output_value.x, m_resize_output_value.y)
278 : cvSize(ncols, nrows);
288 cv::Mat& out_left = out_left_image.
asCvMatRef();
289 cv::Mat& out_right = out_right_image.
asCvMatRef();
292 *
this, in_left, in_right, out_left, out_right,
293 const_cast<int16_t*
>(&m_dat_mapx_left[0]),
294 const_cast<int16_t*
>(&m_dat_mapx_right[0]),
295 const_cast<uint16_t*
>(&m_dat_mapy_left[0]),
296 const_cast<uint16_t*
>(&m_dat_mapy_right[0]),
297 static_cast<int>(m_interpolation_method));
303 void CStereoRectifyMap::rectify(
305 const bool use_internal_mem_cache)
const
311 if (use_internal_mem_cache)
344 return m_rectified_image_params;
351 return m_rectified_image_params.leftCamera;
359 return m_rectified_image_params.rightCamera;
362 void CStereoRectifyMap::setRectifyMaps(
363 const std::vector<int16_t>& left_x,
const std::vector<uint16_t>& left_y,
364 const std::vector<int16_t>& right_x,
const std::vector<uint16_t>& right_y)
366 m_dat_mapx_left.resize(left_x.size());
367 m_dat_mapy_left.resize(left_y.size());
368 m_dat_mapx_right.resize(right_x.size());
369 m_dat_mapy_right.resize(right_y.size());
371 std::copy(left_x.begin(), left_x.end(), m_dat_mapx_left.begin());
372 std::copy(left_y.begin(), left_y.end(), m_dat_mapy_left.begin());
373 std::copy(right_x.begin(), right_x.end(), m_dat_mapx_right.begin());
374 std::copy(right_y.begin(), right_y.end(), m_dat_mapy_right.begin());
377 void CStereoRectifyMap::setRectifyMapsFast(
378 std::vector<int16_t>& left_x, std::vector<uint16_t>& left_y,
379 std::vector<int16_t>& right_x, std::vector<uint16_t>& right_y)
381 left_x.swap(m_dat_mapx_left);
382 left_y.swap(m_dat_mapy_left);
383 right_x.swap(m_dat_mapx_right);
384 right_y.swap(m_dat_mapy_right);