MRPT  1.9.9
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-2019, 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/otherlibs/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 }
bool hasImageRight
Whether imageRight actually contains data (Default upon construction: true)
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:74
uint32_t nrows
Definition: TCamera.h:39
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
bool isEnabledResizeOutput() const
Returns whether resizing is enabled (default=false)
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
Definition: CImage.cpp:234
TImageChannels getChannelCount() const
Returns the number of channels, typically 1 (GRAY) or 3 (RGB)
Definition: CImage.cpp:907
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)
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:237
bool isSet() const
Returns true if setFromCamParams() has been already called, false otherwise.
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:224
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:49
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
This base provides a set of functions for maths stuff.
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:254
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:290
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
Use this class to rectify stereo images if the same distortion maps are reused over and over again...
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:25
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::img::TImageSize getResizeOutputSize() const
Only when isEnabledResizeOutput() returns true, this gets the target size.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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:52
void setStereoCameraParams(const mrpt::img::TStereoCamera &in_params)
Sets leftCamera, rightCamera and rightCameraPose from a TStereoCamera structure.
const mrpt::img::TStereoCamera & getCameraParams() const
Returns the camera parameters which were used to generate the distortion map, as passed by the user t...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
#define MRPT_END
Definition: exceptions.h:245
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:28
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:52
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
GLenum const GLfloat * params
Definition: glext.h:3538
uint32_t ncols
Camera resolution.
Definition: TCamera.h:39
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019