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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019