14 #include <mrpt/otherlibs/do_opencv_includes.h>
23 CStereoRectifyMap::CStereoRectifyMap()
25 m_resize_output(false),
26 m_enable_both_centers_coincide(false),
27 m_resize_output_value(0, 0),
49 bool enable,
unsigned int target_width,
unsigned int target_height)
71 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM >= 0x200
80 const cv::Size trg_size =
111 cv::Mat _mapx_left = cv::cvarrToMat(&mapx_left,
false);
112 cv::Mat _mapy_left = cv::cvarrToMat(&mapy_left,
false);
113 cv::Mat _mapx_right = cv::cvarrToMat(&mapx_right,
false);
114 cv::Mat _mapy_right = cv::cvarrToMat(&mapy_right,
false);
124 for (
unsigned int i = 0; i < 3; ++i)
125 for (
unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);
128 double rcTrans[3] = {hMatrix(0, 3), hMatrix(1, 3), hMatrix(2, 3)};
130 double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
131 for (
unsigned int i = 0; i < 3; ++i)
132 for (
unsigned int j = 0; j < 3; ++j)
134 ipl[i][j] = cam1.intrinsicParams(i, j);
138 for (
unsigned int i = 0; i < 5; ++i)
140 dpl[i] = cam1.dist[i];
141 dpr[i] = cam2.
dist[i];
144 const cv::Mat
R(3, 3, CV_64F, &m1);
145 const cv::Mat T(3, 1, CV_64F, &rcTrans);
147 const cv::Mat K1(3, 3, CV_64F, ipl);
148 const cv::Mat K2(3, 3, CV_64F, ipr);
149 const cv::Mat D1(1, 5, CV_64F, dpl);
150 const cv::Mat D2(1, 5, CV_64F, dpr);
152 double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
153 cv::Mat R1(3, 3, CV_64F, _R1);
154 cv::Mat R2(3, 3, CV_64F, _R2);
155 cv::Mat P1(3, 4, CV_64F, _P1);
156 cv::Mat P2(3, 4, CV_64F, _P2);
157 cv::Mat Q(4, 4, CV_64F, _Q);
159 const cv::Size img_size(ncols, nrows);
160 const cv::Size real_trg_size =
196 #if MRPT_OPENCV_VERSION_NUM < 0x210
199 K1, D1, K2, D2, img_size,
R, T, R1, R2, P1, P2, Q,
201 #elif MRPT_OPENCV_VERSION_NUM < 0x230
204 K1, D1, K2, D2, img_size,
R, T, R1, R2, P1, P2, Q,
m_alpha,
211 K1, D1, K2, D2, img_size,
R, T, R1, R2, P1, P2, Q,
218 cv::initUndistortRectifyMap(
219 K1, D1, R1, P1, real_trg_size, CV_16SC2, _mapx_left, _mapy_left);
220 cv::initUndistortRectifyMap(
221 K2, D2, R2, P2, real_trg_size, CV_16SC2, _mapx_right, _mapy_right);
224 for (
unsigned int i = 0; i < 3; ++i)
225 for (
unsigned int j = 0; j < 3; ++j)
245 params.leftCamera.focalLengthMeters;
247 params.rightCamera.focalLengthMeters;
251 const Eigen::Map<Eigen::Matrix3d> R1e(R1.ptr<
double>());
252 const Eigen::Map<Eigen::Matrix3d> R2e(R2.ptr<
double>());
275 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM >= 0x200
279 const CvSize trg_size =
282 : cvSize(ncols, nrows);
285 trg_size.width, trg_size.height, in_left_image.
isColor() ? 3 : 1,
288 trg_size.width, trg_size.height, in_left_image.
isColor() ? 3 : 1,
291 const IplImage* in_left = in_left_image.
getAs<IplImage>();
292 const IplImage* in_right = in_right_image.
getAs<IplImage>();
294 IplImage* out_left = out_left_image.
getAs<IplImage>();
295 IplImage* out_right = out_right_image.
getAs<IplImage>();
297 this->
rectify_IPL(in_left, in_right, out_left, out_right);
306 const bool use_internal_mem_cache)
const
310 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM >= 0x200
314 const CvSize trg_size =
317 : cvSize(ncols, nrows);
319 const IplImage* in_left = left_image.
getAs<IplImage>();
320 const IplImage* in_right = right_image.
getAs<IplImage>();
322 IplImage *out_left_image, *out_right_image;
323 if (use_internal_mem_cache)
326 trg_size.width, trg_size.height, left_image.
isColor() ? 3 : 1,
329 trg_size.width, trg_size.height, right_image.
isColor() ? 3 : 1,
338 cvCreateImage(trg_size, in_left->depth, in_left->nChannels);
340 cvCreateImage(trg_size, in_right->depth, in_right->nChannels);
343 this->
rectify_IPL(in_left, in_right, out_left_image, out_right_image);
345 if (use_internal_mem_cache)
350 trg_size.width, trg_size.height, left_image.
isColor() ? 3 : 1,
353 trg_size.width, trg_size.height, right_image.
isColor() ? 3 : 1,
356 cvCopy(out_left_image, left_image.
getAs<IplImage>());
357 cvCopy(out_right_image, right_image.
getAs<IplImage>());
378 const bool use_internal_mem_cache)
const
386 use_internal_mem_cache);
406 const void* srcImg_left,
const void* srcImg_right,
void* outImg_left,
407 void* outImg_right)
const
410 ASSERT_(srcImg_left != outImg_left && srcImg_right != outImg_right);
414 "Error: setFromCamParams() must be called prior to rectify().")
416 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM >= 0x200
425 const CvMat mapx_left = cvMat(
426 nrows_out, ncols_out, CV_16SC2,
428 const CvMat mapy_left = cvMat(
429 nrows_out, ncols_out, CV_16UC1,
431 const CvMat mapx_right = cvMat(
432 nrows_out, ncols_out, CV_16SC2,
434 const CvMat mapy_right = cvMat(
435 nrows_out, ncols_out, CV_16UC1,
438 const cv::Mat mapx1 = cv::cvarrToMat(&mapx_left);
439 const cv::Mat mapy1 = cv::cvarrToMat(&mapy_left);
440 const cv::Mat mapx2 = cv::cvarrToMat(&mapx_right);
441 const cv::Mat mapy2 = cv::cvarrToMat(&mapy_right);
443 const cv::Mat src1 = cv::cvarrToMat(srcImg_left);
444 const cv::Mat src2 = cv::cvarrToMat(srcImg_right);
445 cv::Mat dst1 = cv::cvarrToMat(outImg_left);
446 cv::Mat dst2 = cv::cvarrToMat(outImg_right);
450 cv::BORDER_CONSTANT, cvScalarAll(0));
453 cv::BORDER_CONSTANT, cvScalarAll(0));
482 const std::vector<int16_t>& left_x,
const std::vector<uint16_t>& left_y,
483 const std::vector<int16_t>& right_x,
const std::vector<uint16_t>& right_y)
497 std::vector<int16_t>& left_x, std::vector<uint16_t>& left_y,
498 std::vector<int16_t>& right_x, std::vector<uint16_t>& right_y)