MRPT  2.0.4
pinhole.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/poses/CPose3DQuat.h>
13 #include <mrpt/vision/pinhole.h>
14 
15 // Universal include for all versions of OpenCV
16 #include <mrpt/3rdparty/do_opencv_includes.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::vision;
20 using namespace mrpt::img;
21 using namespace mrpt::vision::pinhole;
22 using namespace mrpt::obs;
23 using namespace mrpt::maps;
24 using namespace mrpt::math;
25 using namespace mrpt::system;
26 using namespace std;
27 
28 /* -------------------------------------------------------
29  projectPoints_no_distortion
30  ------------------------------------------------------- */
32  const std::vector<mrpt::math::TPoint3D>& in_points_3D,
33  const mrpt::poses::CPose3D& cameraPose,
34  const mrpt::math::CMatrixDouble33& intrinsicParams,
35  std::vector<TPixelCoordf>& projectedPoints, bool accept_points_behind)
36 {
38 
39  // Do NOT distort points:
40  static const std::vector<double> distortion_dummy(4, 0);
41 
43  in_points_3D, cameraPose, intrinsicParams, distortion_dummy,
44  projectedPoints, accept_points_behind);
45  MRPT_END
46 }
47 
48 /* -------------------------------------------------------
49  projectPoints_with_distortion
50  ------------------------------------------------------- */
52  const std::vector<mrpt::math::TPoint3D>& in_points_3D,
53  const mrpt::poses::CPose3D& cameraPose,
54  const mrpt::math::CMatrixDouble33& intrinsicParams,
55  const std::vector<double>& distortionParams,
56  std::vector<mrpt::img::TPixelCoordf>& projectedPoints,
57  bool accept_points_behind)
58 {
60 #if MRPT_HAS_OPENCV
61 
62  ASSERT_(intrinsicParams.rows() == 3);
63  ASSERT_(intrinsicParams.cols() == 3);
64 
65  const size_t N = in_points_3D.size();
66  projectedPoints.resize(N);
67 
68  if (!N) return; // Nothing to do
69 
70  vector<CvPoint3D64f> objPoints(N);
71 
72  // generate points relative to camera:
73  for (size_t i = 0; i < N; i++)
74  cameraPose.inverseComposePoint(
75  in_points_3D[i].x, in_points_3D[i].y, in_points_3D[i].z,
76  objPoints[i].x, objPoints[i].y, objPoints[i].z);
77 
78  // Points are already translated & rotated:
79  static double rotation_matrix[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
80  static double translation_vector[] = {0, 0, 0};
81 
82  // Projection matrix:
83  // 0 1 2
84  // 3 4 5
85  // 6 7 8
86  std::vector<double> proj_matrix(9);
87  proj_matrix[0] = intrinsicParams(0, 0);
88  proj_matrix[4] = intrinsicParams(1, 1);
89  proj_matrix[2] = intrinsicParams(0, 2);
90  proj_matrix[5] = intrinsicParams(1, 2);
91 
92  // Do the projection:
93  cv::Mat object_points = cv::Mat(N, 1, CV_64FC3, &objPoints[0]);
94 
95  cv::Mat rotvec;
96  cv::Rodrigues(cv::Mat(3, 3, CV_64FC1, rotation_matrix), rotvec);
97 
98  cv::Mat _translation_vector = cv::Mat(3, 1, CV_64FC1, translation_vector);
99  cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, &proj_matrix[0]);
100  cv::Mat dist_coeffs = cv::Mat(
101  distortionParams.size(), 1, CV_64FC1,
102  const_cast<double*>(&distortionParams[0]));
103 
104  vector<cv::Point2d> image_points;
105 
106  cv::projectPoints(
107  object_points, rotvec, _translation_vector, camera_matrix, dist_coeffs,
108  image_points);
109 
110  for (size_t i = 0; i < N; i++)
111  {
112  if (accept_points_behind || objPoints[i].z > 0)
113  { // Valid point or we accept them:
114  projectedPoints[i].x = d2f(image_points[i].x);
115  projectedPoints[i].y = d2f(image_points[i].y);
116  }
117  else
118  { // Invalid point behind the camera:
119  projectedPoints[i].x = -1;
120  projectedPoints[i].y = -1;
121  }
122  }
123 
124 #else
125  THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV");
126 #endif
127  MRPT_END
128 }
129 
130 /* -------------------------------------------------------
131  undistort_points
132  ------------------------------------------------------- */
134  const std::vector<mrpt::img::TPixelCoordf>& in_dist_pixels,
135  std::vector<mrpt::img::TPixelCoordf>& out_pixels,
136  const mrpt::math::CMatrixDouble33& A, const std::vector<double>& Dk)
137 { // Hub function:
138  TCamera cam;
139  cam.intrinsicParams = A;
140  ASSERT_(Dk.size() <= cam.dist.size());
141  for (size_t i = 0; i < cam.dist.size(); i++) cam.dist[i] = Dk[i];
142  undistort_points(in_dist_pixels, out_pixels, cam);
143 }
144 
146  const std::vector<mrpt::img::TPixelCoordf>& in_dist_pixels,
147  std::vector<mrpt::img::TPixelCoordf>& out_pixels,
148  const mrpt::img::TCamera& cameraModel)
149 {
150  MRPT_START
151 
152  // based on code from OpenCV 1.1.0, function cvUndistortPoints, file
153  // cvundistort.cpp
154  // Jose Luis: Great code clean up wrt opencv's since we assume C++ and
155  // availability of MRPT's matrices.
156  const size_t n = in_dist_pixels.size();
157  out_pixels.resize(n);
158 
159  const double fx = cameraModel.fx();
160  const double fy = cameraModel.fy();
161  const double ifx = 1. / fx;
162  const double ify = 1. / fy;
163  const double cx = cameraModel.cx();
164  const double cy = cameraModel.cy();
165 
166  for (size_t i = 0; i < n; i++)
167  {
168  double x = in_dist_pixels[i].x;
169  double y = in_dist_pixels[i].y;
170 
171  double x0 = x = (x - cx) * ifx;
172  double y0 = y = (y - cy) * ify;
173 
174  // compensate distortion iteratively
175  for (unsigned int j = 0; j < 5; j++)
176  {
177  double r2 = x * x + y * y;
178  double icdist =
179  1. /
180  (1 + ((cameraModel.dist[4] * r2 + cameraModel.dist[1]) * r2 +
181  cameraModel.dist[0]) *
182  r2);
183  double deltaX = 2 * cameraModel.dist[2] * x * y +
184  cameraModel.dist[3] * (r2 + 2 * x * x);
185  double deltaY = cameraModel.dist[2] * (r2 + 2 * y * y) +
186  2 * cameraModel.dist[3] * x * y;
187  x = (x0 - deltaX) * icdist;
188  y = (y0 - deltaY) * icdist;
189  }
190 
191  // Save undistorted pixel coords:
192  out_pixels[i].x = d2f(x * fx + cx);
193  out_pixels[i].y = d2f(y * fy + cy);
194 
195  } // end for i
196 
197  MRPT_END
198 }
199 
200 /** Undistort one point given by its pixel coordinates and the camera
201  * parameters.
202  * \sa undistort_points
203  */
205  const TPixelCoordf& inPt, TPixelCoordf& outPt,
206  const mrpt::img::TCamera& cameraModel)
207 {
208  MRPT_START
209 
210  // based on code from OpenCV 1.1.0, function cvUndistortPoints, file
211  // cvundistort.cpp
212  // Jose Luis: Great code clean up wrt opencv's since we assume C++ and
213  // availability of MRPT's matrices.
214  const double fx = cameraModel.fx();
215  const double fy = cameraModel.fy();
216  const double ifx = 1. / fx;
217  const double ify = 1. / fy;
218  const double cx = cameraModel.cx();
219  const double cy = cameraModel.cy();
220 
221  double x = inPt.x;
222  double y = inPt.y;
223 
224  double x0 = x = (x - cx) * ifx;
225  double y0 = y = (y - cy) * ify;
226 
227  // compensate distortion iteratively
228  for (unsigned int j = 0; j < 5; j++)
229  {
230  double r2 = x * x + y * y;
231  double icdist =
232  1. / (1 + ((cameraModel.dist[4] * r2 + cameraModel.dist[1]) * r2 +
233  cameraModel.dist[0]) *
234  r2);
235  double deltaX = 2 * cameraModel.dist[2] * x * y +
236  cameraModel.dist[3] * (r2 + 2 * x * x);
237  double deltaY = cameraModel.dist[2] * (r2 + 2 * y * y) +
238  2 * cameraModel.dist[3] * x * y;
239  x = (x0 - deltaX) * icdist;
240  y = (y0 - deltaY) * icdist;
241  }
242 
243  // Save undistorted pixel coords:
244  outPt.x = d2f(x * fx + cx);
245  outPt.y = d2f(y * fy + cy);
246 
247  MRPT_END
248 }
249 
251  const std::vector<mrpt::math::TPoint3D>& P,
252  const mrpt::img::TCamera& params,
253  const mrpt::poses::CPose3DQuat& cameraPose,
254  std::vector<mrpt::img::TPixelCoordf>& pixels, bool accept_points_behind)
255 {
256  MRPT_START
257 
258  pixels.resize(P.size());
259  std::vector<mrpt::math::TPoint3D>::const_iterator itPoints;
260  std::vector<mrpt::img::TPixelCoordf>::iterator itPixels;
261  unsigned int k = 0;
262  for (itPoints = P.begin(), itPixels = pixels.begin(); itPoints != P.end();
263  ++itPoints, ++itPixels, ++k)
264  {
265  // Change the reference system to that wrt the camera
266  TPoint3D nP;
267  cameraPose.inverseComposePoint(
268  itPoints->x, itPoints->y, itPoints->z, nP.x, nP.y, nP.z);
269 
270  // Pinhole model:
271  const double x = nP.x / nP.z;
272  const double y = nP.y / nP.z;
273 
274  // Radial distortion:
275  const double r2 = square(x) + square(y);
276  const double r4 = square(r2);
277  const double r6 = r2 * r4;
278  const double A =
279  1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
280  const double B = 2 * x * y;
281  if (A > 0 && (accept_points_behind || nP.z > 0))
282  {
283  itPixels->x =
284  d2f(params.cx() +
285  params.fx() * (x * A + params.dist[2] * B +
286  params.dist[3] * (r2 + 2 * square(x))));
287  itPixels->y =
288  d2f(params.cy() +
289  params.fy() * (y * A + params.dist[3] * B +
290  params.dist[2] * (r2 + 2 * square(y))));
291  }
292  else
293  {
294  itPixels->x = -1.0;
295  itPixels->y = -1.0;
296  }
297  } // end-for
298 
299  MRPT_END
300 }
301 
302 /* -------------------------------------------------------
303  projectPoint_with_distortion
304  ------------------------------------------------------- */
307  mrpt::img::TPixelCoordf& pixel, [[maybe_unused]] bool accept_points_behind)
308 {
309  // Pinhole model:
310  const double x = P.x / P.z;
311  const double y = P.y / P.z;
312 
313  // Radial distortion:
314  const double r2 = square(x) + square(y);
315  const double r4 = square(r2);
316  const double r6 = r2 * r4;
317 
318  pixel.x =
319  d2f(params.cx() +
320  params.fx() * (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
321  params.dist[4] * r6) +
322  2 * params.dist[2] * x * y +
323  params.dist[3] * (r2 + 2 * square(x))));
324  pixel.y =
325  d2f(params.cy() +
326  params.fy() * (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
327  params.dist[4] * r6) +
328  2 * params.dist[3] * x * y +
329  params.dist[2] * (r2 + 2 * square(y))));
330 }
331 
332 /* -------------------------------------------------------
333  undistortPixels
334  ------------------------------------------------------- */
335 // void mrpt::vision::pinhole::undistortPixels(
336 // const std::vector<mrpt::img::TPixelCoordf> &inputPixels, /*
337 // distorted
338 // pixels in image */
339 // const mrpt::math::CMatrixDouble33 &intrinsicParams, /*
340 // intrinsic
341 // parameters of the camera */
342 // const std::vector<double> &distortionParams, /* k1 k2
343 // p1
344 // p2
345 //*/
346 // const unsigned int &resX, /*
347 // X-resolution
348 // of
349 // the
350 // image
351 //*/
352 // const unsigned int &resY, /*
353 // Y-resolution
354 // of
355 // the
356 // image
357 //*/
358 // const double &pixelSize, /* pixel
359 // size
360 //(square)*/
361 // std::vector<mrpt::img::TPixelCoordf> &outputPixels /*
362 // estimated
363 // undistorted pixels in image */
364 // )
365 //{
366 // MRPT_START
367 //
368 // ASSERT_( distortionParams.size() >= 4 );
369 // const double k1 = distortionParams[0];
370 // const double k2 = distortionParams[1];
371 // const double p1 = distortionParams[2];
372 // const double p2 = distortionParams[3];
373 //
374 // const double fx = intrinsicParams(0,0);
375 // const double fy = intrinsicParams(1,1);
376 // const double cx = intrinsicParams(0,2);
377 // const double cy = intrinsicParams(1,2);
378 //
379 // CMatrixFixed<double,43,43> dx, dy;
380 //
381 // // Compute the undistortion params according to Heittilä code.
382 // // Generate a regular meshgrid of size 43x43 and distort them
383 // std::vector<mrpt::img::TPixelCoordf> grid; // The
384 // 43x43
385 // grid
386 // with distorted
387 // std::vector<mrpt::img::TPixelCoordf>::iterator itGrid;
388 //
389 // grid.resize( 43 );
390 // unsigned int c;
391 // double px, py;
392 // for( c = 0, itGrid = grid.begin(); itGrid != grid.end(); ++itGrid )
393 // {
394 // px = -resX/40 + c*resX/40;
395 // for( unsigned int k = 0; k < 43; ++k )
396 // {
397 // py = -resY/40 + k*resY/40;
398 // const double dx = ( px - cx )*pixelSize;
399 // const double dy = ( py - cy )*pixelSize;
400 //
401 // const double r2 = dx*dx + dy*dy;
402 // const double delta = k1*r2 + k2*r2*r2;
403 //
404 // const double ncx = dx*(1+delta)+2*p1*dx*dy+p2*(r2+2*dx*dx);
405 // const double ncy = dy*(1+delta)+p1*(r2+2*dy*dy)+2*p2*dx*dy;
406 //
407 // (*itGrid)->x = ncx/pixelSize + cx;
408 // (*itGrid)->y = ncy/pixelSize + cy;
409 // }
410 // } // end-itGrid
411 //
412 // // DISTORT POINTS
413 // dx=(dp(:,1)-Cpx)*Sx/NDX/Asp;
414 // dy=(dp(:,2)-Cpy)*Sy/NDY;
415 //
416 // r2=dx.*dx+dy.*dy;
417 // delta=Rad1*r2+Rad2*r2.*r2;
418 //
419 // cx=dx.*(1+delta)+2*Tan1*dx.*dy+Tan2*(r2+2*dx.*dx);
420 // cy=dy.*(1+delta)+Tan1*(r2+2*dy.*dy)+2*Tan2*dx.*dy;
421 //
422 // p=NDX*Asp*cx/Sx+Cpx;
423 // p(:,2)=NDY*cy/Sy+Cpy;
424 //
425 //
426 // sys=configc(name);
427 // NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4);
428 // Asp=par(1); Foc=par(2);
429 // Cpx=par(3); Cpy=par(4);
430 // Rad1=par(5); Rad2=par(6);
431 // Tan1=par(7); Tan2=par(8);
432 //
433 // // Generate a meshgrid of points
434 // [dx,dy]=meshgrid(-NDX/40:NDX/40:NDX+NDX/40,-NDY/40:NDY/40:NDY+NDY/40);
435 // cc=imcorr(name,par,[dx(:) dy(:)]);
436 // cx=(cc(:,1)-Cpx)/NDX*Sx/Asp;
437 // cy=(cc(:,2)-Cpy)/NDY*Sy;
438 //
439 // r2=cx.*cx+cy.*cy;
440 // delta=Rad1*r2+Rad2*r2.*r2;
441 //
442 // Q=1+(4*Rad1*r2+6*Rad2*r2.*r2+8*Tan1*cy+8*Tan2*cx);
443 //
444 // dx=cx-(cx.*delta+2*Tan1*cx.*cy+Tan2*(r2+2*cx.*cx))./Q;
445 // dy=cy-(cy.*delta+Tan1*(r2+2*cy.*cy)+2*Tan2*cx.*cy)./Q;
446 //
447 //
448 // r2=dx.*dx+dy.*dy;
449 //
450 // Tx=[dx.*r2 dx.*r2.*r2 2*dx.*dy r2+2*dx.*dx];
451 // Ty=[dy.*r2 dy.*r2.*r2 r2+2*dy.*dy 2*dx.*dy];
452 // T=[Tx;Ty];
453 // e=[cx-dx;cy-dy];
454 // a=pinv(T)*e;
455 // par=par(:);
456 // a=[par(1:4);a];
457 //
458 // MRPT_END
459 //}
void projectPoint_with_distortion(const mrpt::math::TPoint3D &in_point_wrt_cam, const mrpt::img::TCamera &in_cam_params, mrpt::img::TPixelCoordf &out_projectedPoints, bool accept_points_behind=false)
Project one 3D point into a camera using its calibration matrix and distortion parameters (radial and...
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
mrpt::vision::TStereoCalibParams params
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
This base provides a set of functions for maths stuff.
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:173
Functions related to pinhole camera models, point projections, etc.
Definition: pinhole.h:22
This namespace contains representation of robot actions and observations.
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
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
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
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
void projectPoints_with_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams, std::vector< mrpt::img::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix and dis...
Definition: pinhole.cpp:51
void undistort_point(const mrpt::img::TPixelCoordf &inPt, mrpt::img::TPixelCoordf &outPt, const mrpt::img::TCamera &cameraModel)
Undistort one point given by its pixel coordinates and the camera parameters.
Definition: pinhole.cpp:204
return_t square(const num_t x)
Inline function for the square of a number.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void undistort_points(const std::vector< mrpt::img::TPixelCoordf > &srcDistortedPixels, std::vector< mrpt::img::TPixelCoordf > &dstUndistortedPixels, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams)
Undistort a list of points given by their pixel coordinates, provided the camera matrix and distortio...
Definition: pinhole.cpp:133
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
#define MRPT_END
Definition: exceptions.h:245
void projectPoints_no_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, std::vector< mrpt::img::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix (undist...
Definition: pinhole.cpp:31
constexpr size_type cols() const
Number of columns in the matrix.
Definition: CMatrixFixed.h:230



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020