MRPT  2.0.3
CStereoRectifyMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <Eigen/Dense>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::vision;
19 using namespace mrpt::img;
20 using namespace mrpt::math;
21 
22 #if MRPT_HAS_OPENCV
23 #include <opencv2/core/eigen.hpp>
24 
25 static void do_rectify(
26  const CStereoRectifyMap& me, const cv::Mat& src_left,
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,
29  int interp_method)
30 {
32  ASSERTMSG_(
33  src_left.data != out_left.data && src_right.data != out_right.data,
34  "in-place rectify not supported");
35 
36  if (!me.isSet())
38  "Error: setFromCamParams() must be called prior to rectify().");
39 
40  const uint32_t ncols = me.getCameraParams().leftCamera.ncols;
41  const uint32_t nrows = me.getCameraParams().leftCamera.nrows;
42 
43  const int ncols_out =
44  me.isEnabledResizeOutput() ? me.getResizeOutputSize().x : ncols;
45  const int nrows_out =
46  me.isEnabledResizeOutput() ? me.getResizeOutputSize().y : nrows;
47 
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);
52 
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);
57 
58  cv::remap(
59  src_left, out_left, mapx1, mapy1, interp_method, cv::BORDER_CONSTANT,
60  cvScalarAll(0));
61  cv::remap(
62  src_right, out_right, mapx2, mapy2, interp_method, cv::BORDER_CONSTANT,
63  cvScalarAll(0));
64  MRPT_END
65 }
66 #endif
67 
68 void CStereoRectifyMap::internal_invalidate()
69 {
70  // don't do a "strong clear" since memory is likely to be reasigned soon.
71  m_dat_mapx_left.clear();
72  m_dat_mapx_right.clear();
73  m_dat_mapy_left.clear();
74  m_dat_mapy_right.clear();
75 }
76 
77 void CStereoRectifyMap::setAlpha(double alpha)
78 {
79  m_alpha = alpha;
80 
81  this->internal_invalidate();
82 }
83 
84 void CStereoRectifyMap::enableResizeOutput(
85  bool enable, unsigned int target_width, unsigned int target_height)
86 {
87  m_resize_output = enable;
88  m_resize_output_value.x = target_width;
89  m_resize_output_value.y = target_height;
90 
91  this->internal_invalidate();
92 }
93 
94 void CStereoRectifyMap::enableBothCentersCoincide(bool enable)
95 {
96  m_enable_both_centers_coincide = enable;
97 
98  this->internal_invalidate();
99 }
100 
101 /** Prepares the mapping from the distortion parameters of a camera.
102  * Must be called before invoking \a undistort().
103  */
104 void CStereoRectifyMap::setFromCamParams(const mrpt::img::TStereoCamera& params)
105 {
106  MRPT_START
107 #if MRPT_HAS_OPENCV
108  const mrpt::img::TCamera& cam1 = params.leftCamera;
109  const mrpt::img::TCamera& cam2 = params.rightCamera;
110 
111  ASSERT_(cam1.ncols == cam2.ncols && cam1.nrows == cam2.nrows);
112 
113  const uint32_t ncols = cam1.ncols;
114  const uint32_t nrows = cam1.nrows;
115 
116  const cv::Size trg_size =
117  m_resize_output
118  ? cv::Size(
119  m_resize_output_value.x,
120  m_resize_output_value.y) // User requested image scaling
121  : cv::Size(); // Default=don't scale
122 
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;
127 
128  // save a copy for future reference
129  m_camera_params = params;
130 
131  // Create OpenCV's wrappers for output maps:
132  m_dat_mapx_left.resize(2 * nrows_out * ncols_out);
133  m_dat_mapy_left.resize(nrows_out * ncols_out);
134 
135  m_dat_mapx_right.resize(2 * nrows_out * ncols_out);
136  m_dat_mapy_right.resize(nrows_out * ncols_out);
137 
138  CvMat mapx_left =
139  cvMat(nrows_out, ncols_out, CV_16SC2, &m_dat_mapx_left[0]);
140  CvMat mapy_left =
141  cvMat(nrows_out, ncols_out, CV_16UC1, &m_dat_mapy_left[0]);
142  CvMat mapx_right =
143  cvMat(nrows_out, ncols_out, CV_16SC2, &m_dat_mapx_right[0]);
144  CvMat mapy_right =
145  cvMat(nrows_out, ncols_out, CV_16UC1, &m_dat_mapy_right[0]);
146 
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);
151 
152  // right camera pose: Rotation
153  CMatrixDouble44 hMatrix;
154  // NOTE!: OpenCV seems to expect the INVERSE of the pose we keep, so invert
155  // it:
156  mrpt::poses::CPose3D(params.rightCameraPose)
157  .getInverseHomogeneousMatrix(hMatrix);
158 
159  double m1[3][3];
160  for (unsigned int i = 0; i < 3; ++i)
161  for (unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);
162 
163  // right camera pose: translation
164  double rcTrans[3] = {hMatrix(0, 3), hMatrix(1, 3), hMatrix(2, 3)};
165 
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)
169  {
170  ipl[i][j] = cam1.intrinsicParams(i, j);
171  ipr[i][j] = cam2.intrinsicParams(i, j);
172  }
173 
174  for (unsigned int i = 0; i < 5; ++i)
175  {
176  dpl[i] = cam1.dist[i];
177  dpr[i] = cam2.dist[i];
178  }
179 
180  const cv::Mat R(3, 3, CV_64F, &m1);
181  const cv::Mat T(3, 1, CV_64F, &rcTrans);
182 
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);
187 
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);
194 
195  const cv::Size img_size(ncols, nrows);
196  const cv::Size real_trg_size =
197  m_resize_output ? trg_size
198  : img_size; // Note: trg_size is Size() by default
199 
200  // OpenCV 2.3+ has this signature:
201  cv::stereoRectify(
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,
204  trg_size // Size() by default=no resize
205  );
206  // Rest of arguments -> default
207 
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);
212 
213  // Populate the parameter matrices of the output rectified images:
214  for (unsigned int i = 0; i < 3; ++i)
215  for (unsigned int j = 0; j < 3; ++j)
216  {
217  m_rectified_image_params.leftCamera.intrinsicParams(i, j) =
218  _P1[i][j];
219  m_rectified_image_params.rightCamera.intrinsicParams(i, j) =
220  _P2[i][j];
221  }
222  // They have no distortion:
223  m_rectified_image_params.leftCamera.dist.fill(0);
224  m_rectified_image_params.rightCamera.dist.fill(0);
225 
226  // Target image size:
227  m_rectified_image_params.leftCamera.ncols = real_trg_size.width;
228  m_rectified_image_params.leftCamera.nrows = real_trg_size.height;
229 
230  m_rectified_image_params.rightCamera.ncols = real_trg_size.width;
231  m_rectified_image_params.rightCamera.nrows = real_trg_size.height;
232 
233  // Rest of params don't change:
234  m_rectified_image_params.leftCamera.focalLengthMeters =
235  params.leftCamera.focalLengthMeters;
236  m_rectified_image_params.rightCamera.focalLengthMeters =
237  params.rightCamera.focalLengthMeters;
238 
239  // R1: Rotation of left camera after rectification:
240  // R2: idem for right cam:
241  {
242  Eigen::Matrix3d Re;
243  cv::cv2eigen(R1, Re);
244  CPose3D RR1;
246  m_rot_left = CPose3DQuat(RR1);
247  }
248  {
249  Eigen::Matrix3d Re;
250  cv::cv2eigen(R2, Re);
251  CPose3D RR;
253  m_rot_right = CPose3DQuat(RR);
254  }
255 
256  m_rectified_image_params.rightCameraPose = params.rightCameraPose;
257 
258 #else
259  THROW_EXCEPTION("MRPT built without OpenCV >=2.0.0!");
260 #endif
261  MRPT_END
262 }
263 
264 void CStereoRectifyMap::rectify(
265  const mrpt::img::CImage& in_left_image,
266  const mrpt::img::CImage& in_right_image, mrpt::img::CImage& out_left_image,
267  mrpt::img::CImage& out_right_image) const
268 {
269  MRPT_START
270 
271 #if MRPT_HAS_OPENCV
272  const uint32_t ncols = m_camera_params.leftCamera.ncols;
273  const uint32_t nrows = m_camera_params.leftCamera.nrows;
274 
275  const CvSize trg_size =
276  m_resize_output
277  ? cvSize(m_resize_output_value.x, m_resize_output_value.y)
278  : cvSize(ncols, nrows);
279 
280  out_left_image.resize(
281  trg_size.width, trg_size.height, in_left_image.getChannelCount());
282  out_right_image.resize(
283  trg_size.width, trg_size.height, in_left_image.getChannelCount());
284 
285  const cv::Mat in_left = in_left_image.asCvMat<cv::Mat>(SHALLOW_COPY);
286  const cv::Mat in_right = in_right_image.asCvMat<cv::Mat>(SHALLOW_COPY);
287 
288  cv::Mat& out_left = out_left_image.asCvMatRef();
289  cv::Mat& out_right = out_right_image.asCvMatRef();
290 
291  do_rectify(
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));
298 
299 #endif
300  MRPT_END
301 }
302 
303 void CStereoRectifyMap::rectify(
305  const bool use_internal_mem_cache) const
306 {
307  MRPT_START
309 
310  // Rectify images:
311  if (use_internal_mem_cache)
312  {
313  static mrpt::img::CImage left_rect, right_rect;
314  this->rectify(o.imageLeft, o.imageRight, left_rect, right_rect);
315  o.imageLeft = left_rect;
316  o.imageRight = right_rect;
317  }
318  else
319  {
320  mrpt::img::CImage left_rect, right_rect;
321  this->rectify(o.imageLeft, o.imageRight, left_rect, right_rect);
322  o.imageLeft = left_rect;
323  o.imageRight = right_rect;
324  }
325 
326  // Copy output image parameters:
327  o.setStereoCameraParams(this->m_rectified_image_params);
328 
329  // Correct poses:
330  o.cameraPose += m_rot_left;
331 
332  const double d = o.rightCameraPose.m_coords.norm();
333  // the translation is now pure in the +X direction:
335 
336  MRPT_END
337 }
338 
339 const mrpt::img::TStereoCamera& CStereoRectifyMap::getRectifiedImageParams()
340  const
341 {
342  if (!isSet())
343  THROW_EXCEPTION("Error: setFromCamParams() must be called before.");
344  return m_rectified_image_params;
345 }
346 
347 const mrpt::img::TCamera& CStereoRectifyMap::getRectifiedLeftImageParams() const
348 {
349  if (!isSet())
350  THROW_EXCEPTION("Error: setFromCamParams() must be called before.");
351  return m_rectified_image_params.leftCamera;
352 }
353 
354 const mrpt::img::TCamera& CStereoRectifyMap::getRectifiedRightImageParams()
355  const
356 {
357  if (!isSet())
358  THROW_EXCEPTION("Error: setFromCamParams() must be called before.");
359  return m_rectified_image_params.rightCamera;
360 }
361 
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)
365 {
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());
370 
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());
375 }
376 
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)
380 {
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);
385 }
mrpt::img::CImage::resize
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content,...
Definition: CImage.cpp:247
mrpt::obs::CObservationStereoImages::setStereoCameraParams
void setStereoCameraParams(const mrpt::img::TStereoCamera &in_params)
Sets leftCamera, rightCamera and rightCameraPose from a TStereoCamera structure.
Definition: CObservationStereoImages.cpp:184
mrpt::vision::CStereoRectifyMap::getResizeOutputSize
mrpt::img::TImageSize getResizeOutputSize() const
Only when isEnabledResizeOutput() returns true, this gets the target size.
Definition: CStereoRectifyMap.h:155
mrpt::img::TCamera::intrinsicParams
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates):
Definition: TCamera.h:50
mrpt::img::CImage::getChannelCount
TImageChannels getChannelCount() const
Returns the number of channels, typically 1 (GRAY) or 3 (RGB)
Definition: CImage.cpp:878
mrpt::img::CImage::asCvMat
void asCvMat(cv::Mat &out_img, copy_type_t copy_type) const
Makes a shallow or deep copy of this image into the provided cv::Mat.
Definition: CImage.cpp:217
mrpt::vision::CStereoRectifyMap::getCameraParams
const mrpt::img::TStereoCamera & getCameraParams() const
Returns the camera parameters which were used to generate the distortion map, as passed by the user t...
Definition: CStereoRectifyMap.h:108
mrpt::vision
Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2020,...
Definition: bundle_adjustment.h:35
mrpt::img::TPixelCoord::y
int y
Definition: TPixelCoord.h:51
mrpt::img::TCamera::dist
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
mrpt::img::TCamera::ncols
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
R
const float R
Definition: CKinematicChain.cpp:137
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:45
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs::CObservationStereoImages::imageRight
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
Definition: CObservationStereoImages.h:56
vision-precomp.h
mrpt::vision::CStereoRectifyMap
Use this class to rectify stereo images if the same distortion maps are reused over and over again.
Definition: CStereoRectifyMap.h:74
mrpt::img::TPixelCoord::x
int x
Definition: TPixelCoord.h:51
mrpt::img::CImage::asCvMatRef
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
Definition: CImage.cpp:227
do_rectify
static void do_rectify(const CStereoRectifyMap &me, const cv::Mat &src_left, const cv::Mat &src_right, cv::Mat &out_left, cv::Mat &out_right, int16_t *map_xl, int16_t *map_xr, uint16_t *map_yl, uint16_t *map_yr, int interp_method)
Definition: CStereoRectifyMap.cpp:25
CStereoRectifyMap.h
mrpt::vision::CStereoRectifyMap::isEnabledResizeOutput
bool isEnabledResizeOutput() const
Returns whether resizing is enabled (default=false)
Definition: CStereoRectifyMap.h:152
mrpt::img
Definition: CCanvas.h:16
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::obs::CObservationStereoImages::imageLeft
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
Definition: CObservationStereoImages.h:52
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::img::TStereoCamera
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
mrpt::poses::CPoseOrPoint::getInverseHomogeneousMatrix
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose.
Definition: CPoseOrPoint.h:290
mrpt::img::TCamera::nrows
uint32_t nrows
Definition: TCamera.h:40
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::img::TCamera
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition: CQuaternion.h:44
mrpt::img::SHALLOW_COPY
@ SHALLOW_COPY
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:75
mrpt::obs::CObservationStereoImages::cameraPose
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
Definition: CObservationStereoImages.h:80
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::vision::CStereoRectifyMap::isSet
bool isSet() const
Returns true if setFromCamParams() has been already called, false otherwise.
Definition: CStereoRectifyMap.h:86
mrpt::img::TStereoCamera::leftCamera
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:28
mrpt::obs::CObservationStereoImages::hasImageRight
bool hasImageRight
Whether imageRight actually contains data (Default upon construction: true)
Definition: CObservationStereoImages.h:69
mrpt::poses::CPose3DQuat::m_coords
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:52
mrpt::poses::CPose3D::setRotationMatrix
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:237
mrpt::math::CMatrixDouble33
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
mrpt::obs::CObservationStereoImages::rightCameraPose
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
Definition: CObservationStereoImages.h:88
mrpt::obs::CObservationStereoImages
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
Definition: CObservationStereoImages.h:38



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020