MRPT  1.9.9
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-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/poses/CPose3DQuat.h>
13 #include <mrpt/vision/pinhole.h>
14 
15 // Universal include for all versions of OpenCV
16 #include <mrpt/otherlibs/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 = image_points[i].x;
115  projectedPoints[i].y = 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 = x * fx + cx;
193  out_pixels[i].y = 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 = x * fx + cx;
245  outPt.y = 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 = params.cx() +
284  params.fx() * (x * A + params.dist[2] * B +
285  params.dist[3] * (r2 + 2 * square(x)));
286  itPixels->y = params.cy() +
287  params.fy() * (y * A + params.dist[3] * B +
288  params.dist[2] * (r2 + 2 * square(y)));
289  }
290  else
291  {
292  itPixels->x = -1.0;
293  itPixels->y = -1.0;
294  }
295  } // end-for
296 
297  MRPT_END
298 }
299 
300 /* -------------------------------------------------------
301  projectPoint_with_distortion
302  ------------------------------------------------------- */
305  mrpt::img::TPixelCoordf& pixel, bool accept_points_behind)
306 {
307  MRPT_UNUSED_PARAM(accept_points_behind);
308  // Pinhole model:
309  const double x = P.x / P.z;
310  const double y = P.y / P.z;
311 
312  // Radial distortion:
313  const double r2 = square(x) + square(y);
314  const double r4 = square(r2);
315  const double r6 = r2 * r4;
316 
317  pixel.x = params.cx() +
318  params.fx() * (x * (1 + params.dist[0] * r2 +
319  params.dist[1] * r4 + params.dist[4] * r6) +
320  2 * params.dist[2] * x * y +
321  params.dist[3] * (r2 + 2 * square(x)));
322  pixel.y = params.cy() +
323  params.fy() * (y * (1 + params.dist[0] * r2 +
324  params.dist[1] * r4 + params.dist[4] * r6) +
325  2 * params.dist[3] * x * y +
326  params.dist[2] * (r2 + 2 * square(y)));
327 }
328 
329 /* -------------------------------------------------------
330  undistortPixels
331  ------------------------------------------------------- */
332 // void mrpt::vision::pinhole::undistortPixels(
333 // const std::vector<mrpt::img::TPixelCoordf> &inputPixels, /*
334 // distorted
335 // pixels in image */
336 // const mrpt::math::CMatrixDouble33 &intrinsicParams, /*
337 // intrinsic
338 // parameters of the camera */
339 // const std::vector<double> &distortionParams, /* k1 k2
340 // p1
341 // p2
342 //*/
343 // const unsigned int &resX, /*
344 // X-resolution
345 // of
346 // the
347 // image
348 //*/
349 // const unsigned int &resY, /*
350 // Y-resolution
351 // of
352 // the
353 // image
354 //*/
355 // const double &pixelSize, /* pixel
356 // size
357 //(square)*/
358 // std::vector<mrpt::img::TPixelCoordf> &outputPixels /*
359 // estimated
360 // undistorted pixels in image */
361 // )
362 //{
363 // MRPT_START
364 //
365 // ASSERT_( distortionParams.size() >= 4 );
366 // const double k1 = distortionParams[0];
367 // const double k2 = distortionParams[1];
368 // const double p1 = distortionParams[2];
369 // const double p2 = distortionParams[3];
370 //
371 // const double fx = intrinsicParams(0,0);
372 // const double fy = intrinsicParams(1,1);
373 // const double cx = intrinsicParams(0,2);
374 // const double cy = intrinsicParams(1,2);
375 //
376 // CMatrixFixed<double,43,43> dx, dy;
377 //
378 // // Compute the undistortion params according to Heittilä code.
379 // // Generate a regular meshgrid of size 43x43 and distort them
380 // std::vector<mrpt::img::TPixelCoordf> grid; // The
381 // 43x43
382 // grid
383 // with distorted
384 // std::vector<mrpt::img::TPixelCoordf>::iterator itGrid;
385 //
386 // grid.resize( 43 );
387 // unsigned int c;
388 // double px, py;
389 // for( c = 0, itGrid = grid.begin(); itGrid != grid.end(); ++itGrid )
390 // {
391 // px = -resX/40 + c*resX/40;
392 // for( unsigned int k = 0; k < 43; ++k )
393 // {
394 // py = -resY/40 + k*resY/40;
395 // const double dx = ( px - cx )*pixelSize;
396 // const double dy = ( py - cy )*pixelSize;
397 //
398 // const double r2 = dx*dx + dy*dy;
399 // const double delta = k1*r2 + k2*r2*r2;
400 //
401 // const double ncx = dx*(1+delta)+2*p1*dx*dy+p2*(r2+2*dx*dx);
402 // const double ncy = dy*(1+delta)+p1*(r2+2*dy*dy)+2*p2*dx*dy;
403 //
404 // (*itGrid)->x = ncx/pixelSize + cx;
405 // (*itGrid)->y = ncy/pixelSize + cy;
406 // }
407 // } // end-itGrid
408 //
409 // // DISTORT POINTS
410 // dx=(dp(:,1)-Cpx)*Sx/NDX/Asp;
411 // dy=(dp(:,2)-Cpy)*Sy/NDY;
412 //
413 // r2=dx.*dx+dy.*dy;
414 // delta=Rad1*r2+Rad2*r2.*r2;
415 //
416 // cx=dx.*(1+delta)+2*Tan1*dx.*dy+Tan2*(r2+2*dx.*dx);
417 // cy=dy.*(1+delta)+Tan1*(r2+2*dy.*dy)+2*Tan2*dx.*dy;
418 //
419 // p=NDX*Asp*cx/Sx+Cpx;
420 // p(:,2)=NDY*cy/Sy+Cpy;
421 //
422 //
423 // sys=configc(name);
424 // NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4);
425 // Asp=par(1); Foc=par(2);
426 // Cpx=par(3); Cpy=par(4);
427 // Rad1=par(5); Rad2=par(6);
428 // Tan1=par(7); Tan2=par(8);
429 //
430 // // Generate a meshgrid of points
431 // [dx,dy]=meshgrid(-NDX/40:NDX/40:NDX+NDX/40,-NDY/40:NDY/40:NDY+NDY/40);
432 // cc=imcorr(name,par,[dx(:) dy(:)]);
433 // cx=(cc(:,1)-Cpx)/NDX*Sx/Asp;
434 // cy=(cc(:,2)-Cpy)/NDY*Sy;
435 //
436 // r2=cx.*cx+cy.*cy;
437 // delta=Rad1*r2+Rad2*r2.*r2;
438 //
439 // Q=1+(4*Rad1*r2+6*Rad2*r2.*r2+8*Tan1*cy+8*Tan2*cx);
440 //
441 // dx=cx-(cx.*delta+2*Tan1*cx.*cy+Tan2*(r2+2*cx.*cx))./Q;
442 // dy=cy-(cy.*delta+Tan1*(r2+2*cy.*cy)+2*Tan2*cx.*cy)./Q;
443 //
444 //
445 // r2=dx.*dx+dy.*dy;
446 //
447 // Tx=[dx.*r2 dx.*r2.*r2 2*dx.*dy r2+2*dx.*dx];
448 // Ty=[dy.*r2 dy.*r2.*r2 r2+2*dy.*dy 2*dx.*dy];
449 // T=[Tx;Ty];
450 // e=[cx-dx;cy-dy];
451 // a=pinv(T)*e;
452 // par=par(:);
453 // a=[par(1:4);a];
454 //
455 // MRPT_END
456 //}
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...
Definition: pinhole.cpp:303
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
#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:174
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 .
GLenum GLsizei n
Definition: glext.h:5136
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:176
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:49
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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:172
T square(const T x)
Inline function for the square of a number.
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:25
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
GLint GLint GLsizei GLsizei GLsizei GLint GLenum GLenum const GLvoid * pixels
Definition: glext.h:3606
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
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
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:170
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
GLenum GLint GLint y
Definition: glext.h:3542
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
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
GLenum const GLfloat * params
Definition: glext.h:3538
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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