MRPT  1.9.9
pinhole.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/vision/pinhole.h>
13 #include <mrpt/poses/CPose3DQuat.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  ASSERT_(distortionParams.size() == 4 || distortionParams.size() == 5);
65 
66  const size_t N = in_points_3D.size();
67  projectedPoints.resize(N);
68 
69  if (!N) return; // Nothing to do
70 
71  vector<CvPoint3D64f> objPoints(N);
72 
73  // generate points relative to camera:
74  for (size_t i = 0; i < N; i++)
75  cameraPose.inverseComposePoint(
76  in_points_3D[i].x, in_points_3D[i].y, in_points_3D[i].z,
77  objPoints[i].x, objPoints[i].y, objPoints[i].z);
78 
79  // Points are already translated & rotated:
80  static double rotation_matrix[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
81  static double translation_vector[] = {0, 0, 0};
82 
83  // Projection matrix:
84  // 0 1 2
85  // 3 4 5
86  // 6 7 8
87  CVectorDouble proj_matrix(9);
88  proj_matrix[0] = intrinsicParams.get_unsafe(0, 0);
89  proj_matrix[4] = intrinsicParams.get_unsafe(1, 1);
90  proj_matrix[2] = intrinsicParams.get_unsafe(0, 2);
91  proj_matrix[5] = intrinsicParams.get_unsafe(1, 2);
92 
93  // Do the projection:
94  cv::Mat object_points = cv::Mat(N, 1, CV_64FC3, &objPoints[0]);
95 
96  cv::Mat rotvec;
97  cv::Rodrigues(cv::Mat(3, 3, CV_64FC1, rotation_matrix), rotvec);
98 
99  cv::Mat _translation_vector = cv::Mat(3, 1, CV_64FC1, translation_vector);
100  cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, &proj_matrix[0]);
101  cv::Mat dist_coeffs =
102  cv::Mat(5, 1, CV_64FC1, 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. / (1 +
180  ((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 +
233  ((cameraModel.dist[4] * r2 + cameraModel.dist[1]) * r2 +
234  cameraModel.dist[0]) *
235  r2);
236  double deltaX = 2 * cameraModel.dist[2] * x * y +
237  cameraModel.dist[3] * (r2 + 2 * x * x);
238  double deltaY = cameraModel.dist[2] * (r2 + 2 * y * y) +
239  2 * cameraModel.dist[3] * x * y;
240  x = (x0 - deltaX) * icdist;
241  y = (y0 - deltaY) * icdist;
242  }
243 
244  // Save undistorted pixel coords:
245  outPt.x = x * fx + cx;
246  outPt.y = y * fy + cy;
247 
248  MRPT_END
249 }
250 
252  const std::vector<mrpt::math::TPoint3D>& P,
253  const mrpt::img::TCamera& params,
254  const mrpt::poses::CPose3DQuat& cameraPose,
255  std::vector<mrpt::img::TPixelCoordf>& pixels, bool accept_points_behind)
256 {
257  MRPT_START
258 
259  pixels.resize(P.size());
262  unsigned int k = 0;
263  for (itPoints = P.begin(), itPixels = pixels.begin(); itPoints != P.end();
264  ++itPoints, ++itPixels, ++k)
265  {
266  // Change the reference system to that wrt the camera
267  TPoint3D nP;
268  cameraPose.inverseComposePoint(
269  itPoints->x, itPoints->y, itPoints->z, nP.x, nP.y, nP.z);
270 
271  // Pinhole model:
272  const double x = nP.x / nP.z;
273  const double y = nP.y / nP.z;
274 
275  // Radial distortion:
276  const double r2 = square(x) + square(y);
277  const double r4 = square(r2);
278  const double r6 = r2 * r4;
279  const double A =
280  1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
281  const double B = 2 * x * y;
282  if (A > 0 && (accept_points_behind || nP.z > 0))
283  {
284  itPixels->x = params.cx() +
285  params.fx() * (x * A + params.dist[2] * B +
286  params.dist[3] * (r2 + 2 * square(x)));
287  itPixels->y = params.cy() +
288  params.fy() * (y * A + params.dist[3] * B +
289  params.dist[2] * (r2 + 2 * square(y)));
290  }
291  else
292  {
293  itPixels->x = -1.0;
294  itPixels->y = -1.0;
295  }
296  } // end-for
297 
298  MRPT_END
299 }
300 
301 /* -------------------------------------------------------
302  projectPoint_with_distortion
303  ------------------------------------------------------- */
306  mrpt::img::TPixelCoordf& pixel, bool accept_points_behind)
307 {
308  MRPT_UNUSED_PARAM(accept_points_behind);
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 = params.cx() +
319  params.fx() * (x * (1 + params.dist[0] * r2 +
320  params.dist[1] * r4 + params.dist[4] * r6) +
321  2 * params.dist[2] * x * y +
322  params.dist[3] * (r2 + 2 * square(x)));
323  pixel.y = params.cy() +
324  params.fy() * (y * (1 + params.dist[0] * r2 +
325  params.dist[1] * r4 + params.dist[4] * r6) +
326  2 * params.dist[3] * x * y +
327  params.dist[2] * (r2 + 2 * square(y)));
328 }
329 
330 /* -------------------------------------------------------
331  undistortPixels
332  ------------------------------------------------------- */
333 // void mrpt::vision::pinhole::undistortPixels(
334 // const std::vector<mrpt::img::TPixelCoordf> &inputPixels, /*
335 // distorted
336 // pixels in image */
337 // const mrpt::math::CMatrixDouble33 &intrinsicParams, /*
338 // intrinsic
339 // parameters of the camera */
340 // const std::vector<double> &distortionParams, /* k1 k2
341 // p1
342 // p2
343 //*/
344 // const unsigned int &resX, /*
345 // X-resolution
346 // of
347 // the
348 // image
349 //*/
350 // const unsigned int &resY, /*
351 // Y-resolution
352 // of
353 // the
354 // image
355 //*/
356 // const double &pixelSize, /* pixel
357 // size
358 //(square)*/
359 // std::vector<mrpt::img::TPixelCoordf> &outputPixels /*
360 // estimated
361 // undistorted pixels in image */
362 // )
363 //{
364 // MRPT_START
365 //
366 // ASSERT_( distortionParams.size() >= 4 );
367 // const double k1 = distortionParams[0];
368 // const double k2 = distortionParams[1];
369 // const double p1 = distortionParams[2];
370 // const double p2 = distortionParams[3];
371 //
372 // const double fx = intrinsicParams(0,0);
373 // const double fy = intrinsicParams(1,1);
374 // const double cx = intrinsicParams(0,2);
375 // const double cy = intrinsicParams(1,2);
376 //
377 // CMatrixFixedNumeric<double,43,43> dx, dy;
378 //
379 // // Compute the undistortion params according to Heittilä code.
380 // // Generate a regular meshgrid of size 43x43 and distort them
381 // std::vector<mrpt::img::TPixelCoordf> grid; // The
382 // 43x43
383 // grid
384 // with distorted
385 // std::vector<mrpt::img::TPixelCoordf>::iterator itGrid;
386 //
387 // grid.resize( 43 );
388 // unsigned int c;
389 // double px, py;
390 // for( c = 0, itGrid = grid.begin(); itGrid != grid.end(); ++itGrid )
391 // {
392 // px = -resX/40 + c*resX/40;
393 // for( unsigned int k = 0; k < 43; ++k )
394 // {
395 // py = -resY/40 + k*resY/40;
396 // const double dx = ( px - cx )*pixelSize;
397 // const double dy = ( py - cy )*pixelSize;
398 //
399 // const double r2 = dx*dx + dy*dy;
400 // const double delta = k1*r2 + k2*r2*r2;
401 //
402 // const double ncx = dx*(1+delta)+2*p1*dx*dy+p2*(r2+2*dx*dx);
403 // const double ncy = dy*(1+delta)+p1*(r2+2*dy*dy)+2*p2*dx*dy;
404 //
405 // (*itGrid)->x = ncx/pixelSize + cx;
406 // (*itGrid)->y = ncy/pixelSize + cy;
407 // }
408 // } // end-itGrid
409 //
410 // // DISTORT POINTS
411 // dx=(dp(:,1)-Cpx)*Sx/NDX/Asp;
412 // dy=(dp(:,2)-Cpy)*Sy/NDY;
413 //
414 // r2=dx.*dx+dy.*dy;
415 // delta=Rad1*r2+Rad2*r2.*r2;
416 //
417 // cx=dx.*(1+delta)+2*Tan1*dx.*dy+Tan2*(r2+2*dx.*dx);
418 // cy=dy.*(1+delta)+Tan1*(r2+2*dy.*dy)+2*Tan2*dx.*dy;
419 //
420 // p=NDX*Asp*cx/Sx+Cpx;
421 // p(:,2)=NDY*cy/Sy+Cpy;
422 //
423 //
424 // sys=configc(name);
425 // NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4);
426 // Asp=par(1); Foc=par(2);
427 // Cpx=par(3); Cpy=par(4);
428 // Rad1=par(5); Rad2=par(6);
429 // Tan1=par(7); Tan2=par(8);
430 //
431 // // Generate a meshgrid of points
432 // [dx,dy]=meshgrid(-NDX/40:NDX/40:NDX+NDX/40,-NDY/40:NDY/40:NDY+NDY/40);
433 // cc=imcorr(name,par,[dx(:) dy(:)]);
434 // cx=(cc(:,1)-Cpx)/NDX*Sx/Asp;
435 // cy=(cc(:,2)-Cpy)/NDY*Sy;
436 //
437 // r2=cx.*cx+cy.*cy;
438 // delta=Rad1*r2+Rad2*r2.*r2;
439 //
440 // Q=1+(4*Rad1*r2+6*Rad2*r2.*r2+8*Tan1*cy+8*Tan2*cx);
441 //
442 // dx=cx-(cx.*delta+2*Tan1*cx.*cy+Tan2*(r2+2*cx.*cx))./Q;
443 // dy=cy-(cy.*delta+Tan1*(r2+2*cy.*cy)+2*Tan2*cx.*cy)./Q;
444 //
445 //
446 // r2=dx.*dx+dy.*dy;
447 //
448 // Tx=[dx.*r2 dx.*r2.*r2 2*dx.*dy r2+2*dx.*dx];
449 // Ty=[dy.*r2 dy.*r2.*r2 r2+2*dy.*dy 2*dx.*dy];
450 // T=[Tx;Ty];
451 // e=[cx-dx;cy-dy];
452 // a=pinv(T)*e;
453 // par=par(:);
454 // a=[par(1:4);a];
455 //
456 // MRPT_END
457 //}
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:304
Scalar * iterator
Definition: eigen_plugins.h:26
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:163
GLenum GLsizei n
Definition: glext.h:5074
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:165
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
T square(const T x)
Inline function for the square of a number.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:42
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:161
Functions related to pinhole camera models, point projections, etc.
Definition: pinhole.h:23
This namespace contains representation of robot actions and observations.
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:18
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:27
double x
X,Y,Z coordinates.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:46
GLint GLint GLsizei GLsizei GLsizei GLint GLenum GLenum const GLvoid * pixels
Definition: glext.h:3600
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
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:45
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:159
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:86
#define MRPT_END
Definition: exceptions.h:266
GLenum GLint GLint y
Definition: glext.h:3538
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
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
GLenum const GLfloat * params
Definition: glext.h:3534
const Scalar * const_iterator
Definition: eigen_plugins.h:27
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:649
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020