Main MRPT website > C++ reference for MRPT 1.5.7
CCamModel.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 #include <mrpt/vision/CCamModel.h>
12 #include <mrpt/vision/pinhole.h>
14 #include <mrpt/utils/types_math.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::vision;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace mrpt::utils;
21 
22 
23 /** Constructor */
25 {
26 }
27 
28 /**********************************************************************************************************************/
30 {
31  // JL: CHECK!!!!
32  const double Cx = cam.cx();
33  const double Cy = cam.cy();
34  const double k1 = cam.k1();
35  const double k2 = cam.k2();
36  const double dx = 1.0 / cam.fx(); // JL: Check if formulas hold with this change!!!
37  const double dy = 1.0 / cam.fy();
38 
39  double xd = (p.x-Cx)*dx;
40  double yd = (p.y-Cy)*dy;
41  double rd2=xd*xd+yd*yd;
42  double rd4=rd2*rd2;
43 
44  J_undist.setSize(2,2);
45  J_undist(0,0) = (1+k1*rd2+k2*rd4) + (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
46  J_undist(1,1) = (1+k1*rd2+k2*rd4) + (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
47  J_undist(0,1) = (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
48  J_undist(1,0) = (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
49 
50 }
51 /******************************************************************************************************************************/
52 
54 {
55 
56  //J_undistor.setSize(2,2);
57  const double dx = p.x-cam.cx();
58  const double dy = p.y-cam.cy();
59  const double f = 1 - 2*cam.k1()*square(dx)*square(dy);
60  const double inv_f_15 = 1.0/pow(f,1.5);
61 
62  //// dy/du
63  //CMatrixDouble dy_du(2,2); // Default is all zeroes
64  //dy_du(0,0) = 1.0/cam.fx;
65  //dy_du(1,1) = 1.0/cam.fy;
66 
67  // du/dh
68  //CMatrixDouble du_dh(2,2);
69 
70  J_undistor(0,0) = ( 1 - 2*cam.k1()*square(dy) ) * inv_f_15;
71  J_undistor(0,1) =
72  J_undistor(1,0) = ( 2*cam.k1()*dx*dy ) * inv_f_15;
73  J_undistor(1,1) = ( 1 - 2*cam.k1()*square(dx) ) * inv_f_15;
74 
75 
76  //// JL: TODO: CHECK!
77  //const double Cx = cam.cx;
78  //const double Cy = cam.cy;
79  //const double k1 = cam.k1();
80  //const double k2 = cam.k2();
81  //const double dx = 1.0 / cam.fx; // JL: Check if formulas hold with this change!!!
82  //const double dy = 1.0 / cam.fy;
83 
84  //double xd = (p.x-Cx)*dx;
85  //double yd = (p.y-Cy)*dy;
86 
87  //double rd2 = xd*xd + yd*yd;
88  //double rd4 = rd2 * rd2;
89 
90  //J_undistor.setSize(2,2);
91 
92  //J_undistor(0,0) = (1+k1*rd2+k2*rd4) + (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
93  //J_undistor(1,1) = (1+k1*rd2+k2*rd4) + (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
94  //J_undistor(0,1) = (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
95  //J_undistor(1,0) = (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
96 
97 
98 
99 }
100 /**********************************************************************************************************************/
101 
103 {
104  // JLBC: Added from Davison's SceneLib:
105  //
106  // 1 distortion coefficient model
107  //
108  const double dx = (p.x-cam.cx()); // / cam.fx; //JL: commented out cam.fxy... (dx,dy) units must be pixels
109  const double dy = (p.y-cam.cy()); // / cam.fy;
110 
111  const double r2 = square(dx) + square(dy);
112 
113  const double fact = 1.0/sqrt(1+2*cam.k1()*r2);
114 
115  distorted_p.x = cam.cx() + dx*fact;
116  distorted_p.y = cam.cy() + dy*fact;
117  return;
118 }
119 /*************************************************************************************************************************/
120 // Removes distortion of a pair of pixel coordinates x,y.
122 {
123  std::vector<TPixelCoordf> in_p(1), out_p;
124  in_p[0] = p;
125 
127  in_p,
128  out_p,
131  );
132 
133  ASSERT_(out_p.size()==1);
134  undistorted_p = out_p[0];
135 
136  // It's explained fine in page 3, "A visual compass based on SLAM"
137 }
138 
139 
140 /*************************************************************************************************************************/
141 /*************************************************************************************************************************/
142 /**************************************************Davison Style**********************************************************/
143 /*************************************************************************************************************************/
144 
145 
146 /** Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)
147  */
149 {
150  // JLBC: From Davison's SceneLib:
151  //
152  // 1 distortion coefficient model (+ projection)
153  //
154 
155  // Offsets from the image center for the undistorted projection, in units of pixels:
156 
157  ASSERT_(p3D.z!=0)
158  const double dx = (p3D.x / p3D.z)*cam.fx();
159  const double dy = (p3D.y / p3D.z)*cam.fy();
160 
161  // 1 distortion coeff. model:
162  const double r2 = square(dx) + square(dy);
163 
164  const double fact = 1.0/sqrt(1+2*cam.k1()*r2); // Note the "+2" sign
165 
166  distorted_p.x = cam.cx() + dx*fact;
167  distorted_p.y = cam.cy() + dy*fact;
168 }
169 
170 /** Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position
171  */
173 {
174  // JLBC: From Davison's SceneLib:
175  //
176  // 1 distortion coefficient model (+ projection)
177  //
178  const double dx = distorted_p.x - cam.cx();
179  const double dy = distorted_p.y - cam.cy();
180  const double r2 = square(dx)+square(dy);
181  double factor = 1.0/sqrt(1 - 2*cam.k1()*r2); // Note the "-2" sign
182 
183  p3D.x = dx * factor / cam.fx();
184  p3D.y = dy * factor / cam.fy();
185  p3D.z = 1.0;
186 }
187 
188 
189 // Jacobian of the projection of 3D points (with distortion), as done in project_3D_point \f$ \frac{\partial \vct{h}}{\partial \vct{y}} \f$
190 // JL: See .h file for all the formulas
192 {
193  /*
194  \frac{\partial \vct{u}}{\partial \vct{y}} =
195  \left( \begin{array}{ccc}
196  \frac{f_x}{y_z} & 0 & - y \frac f_x}{y_z^2} \\
197  0 & \frac{f_y}{y_z} & - y \frac f_y}{y_z^2} \\
198  \end{array} \right)
199  */
200  ASSERT_(p3D.z!=0)
201 
202  CMatrixDouble du_dy(2,3); // Default all to zeroes.
203 
204  du_dy(0,0) = cam.fx() / p3D.z; du_dy(0,2) = - p3D.x * cam.fx() / square(p3D.z);
205  du_dy(1,1) = cam.fy() / p3D.z; du_dy(1,2) = - p3D.y * cam.fy() / square(p3D.z);
206 
207  /*
208  f = 1+ 2 k_1 (u_x^2+u_y^2), then:
209 
210  \frac{\partial \vct{h}}{\partial \vct{u}} =
211  \left( \begin{array}{cc}
212  \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
213  -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
214  \end{array} \right)
215  */
216  const double ux = (p3D.x / p3D.z)*cam.fx(); // coordinates with (0,0) at the image center
217  const double uy = (p3D.y / p3D.z)*cam.fy();
218 
219  const double ux_sqr = square(ux);
220  const double uy_sqr = square(uy);
221 
222  const double r2 = ux_sqr + uy_sqr;
223 
224  const double f = 1+2*cam.k1()*r2;
225  const double f1_2 = sqrt(f);
226  const double f3_2 = f1_2 * f;
227 
228  // Now form the proper dh_by_du by manipulating the outer product matrix
229 
230  CMatrixDouble dh_du(2,2);
231  dh_du *= -2 * cam.k1() / f3_2;
232  dh_du(0,0) += (1/f1_2);
233  dh_du(1,1) += (1/f1_2);
234 
235  //dh_du(0,0) = 1+2*cam.k1()*uy_sqr;
236  //dh_du(1,1) = 1+2*cam.k1()*ux_sqr;
237 
238  //dh_du(0,1) =
239  //dh_du(1,0) = -2*ux*uy*cam.k1()/pow(f,1.5);
240 
241  // Jacobian dh_dy = dh_du * du_dy (Result is 2x3)
242  dh_dy.multiply( dh_du, du_dy );
243 }
244 
245 /* Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_3D_point \f$ \frac{\partial \vct{y}}{\partial \vct{h}} \f$, evaluated at the pixel p
246 \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian
247 \sa unproject_3D_point
248 */
250 {
251  // dy/du
252  CMatrixDouble dy_du(3,2); // Default is all zeroes
253  dy_du(0,0) = 1.0/cam.fx();
254  dy_du(1,1) = 1.0/cam.fy();
255 
256  //MAAA:
257  //// du/dh
258  const double dx = p.x-cam.cx();
259  const double dy = p.y-cam.cy();
260  const double radi2 = square(dx)+square(dy);
261 
262  double f = 1 - 2 * cam.k1() * radi2;
263  double f1_2 = sqrt(f);
264  double f3_2 = f1_2 * f;
265 
266  CMatrixDouble du_dh(2,3);
267 
268  //const double f = 1 - 2*cam.k1()*radi2;
269  //const double inv_f_15 = 1.0f/powf(f,1.5f);
270  //du_dh(0,0) = ( 1 - 2*cam.k1()*square(dy) ) * inv_f_15;
271  //du_dh(0,1) =
272  //du_dh(1,0) = ( 2*cam.k1()*dx*dy ) * inv_f_15;
273  //du_dh(1,1) = ( 1 - 2*cam.k1()*square(dx) ) * inv_f_15;
274 
275  du_dh *= 2 * cam.k1() / f3_2;
276  du_dh(0,0) += (1/f1_2);
277  du_dh(1,1) += (1/f1_2);
278 
279  // Jacobian dy_dh = dy_du * du_dh (Result is 3x2)
280  dy_dh.multiply( dy_du, du_dh );
281 
282 }
283 
286  const std::string &section)
287 {
288  MRPT_START
289 
290  // Read camera parameters: They are mandatory, we'll raise an exception if not found:
291  double cx = 0.0f, cy = 0.0f, fx = 0.0f, fy = 0.0f;
292 
293  //MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(nrows,int,cam.nrows, source, section)
294  //MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(ncols,int,cam.ncols, source, section)
295 
296  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(cx,double,cx, source, section)
297  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(cy,double,cy, source, section)
298  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(fx,double,fx, source, section)
299  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(fy,double,fy, source, section)
300 
301  cam.setIntrinsicParamsFromValues( fx, fy, cx, cy );
302 
303  CVectorDouble DD;
304  source.read_vector(section,"dist_params",CVectorDouble(),DD,true);
305  ASSERT_( DD.size()==4 || DD.size()==5 )
306 
307  this->cam.setDistortionParamsVector(DD);
308 
309  MRPT_END
310 }
311 
312 /** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */
314 {
315  MRPT_UNUSED_PARAM(out);
316 }
317 
mrpt::utils::TCamera cam
The parameters of a camera.
Definition: CCamModel.h:36
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
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:156
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CCamModel.cpp:284
void jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble &dh_dy) const
Jacobian of the projection of 3D points (with distortion), as done in project_3D_point ...
Definition: CCamModel.cpp:191
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
double z
X,Y,Z coordinates.
This class allows loading and storing values and vectors of different types from a configuration text...
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 class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
double k1() const
Get the value of the k1 distortion parameter.
Definition: TCamera.h:172
void jacob_undistor_fm(const mrpt::utils::TPixelCoordf &uvd, math::CMatrixDouble &J_undist)
Jacobian for undistortion the image coordinates.
Definition: CCamModel.cpp:29
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void jacobian_unproject_with_distortion(const mrpt::utils::TPixelCoordf &p, math::CMatrixDouble &dy_dh) const
Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_...
Definition: CCamModel.cpp:249
void unproject_3D_point(const mrpt::utils::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const
Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position...
Definition: CCamModel.cpp:172
void setDistortionParamsVector(const mrpt::math::CMatrixDouble15 &distParVector)
Set the whole vector of distortion params of the camera.
Definition: TCamera.h:126
Classes for computer vision, detectors, features, etc.
void project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::utils::TPixelCoordf &distorted_p) const
Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z p...
Definition: CCamModel.cpp:148
void jacob_undistor(const mrpt::utils::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist)
Calculate the image coordinates undistorted.
Definition: CCamModel.cpp:53
GLsizei const GLchar ** string
Definition: glext.h:3919
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:154
void distort_a_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &distorted_p)
Return the pixel position distorted by the camera.
Definition: CCamModel.cpp:102
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:54
std::vector< double > getDistortionParamsAsVector() const
Get a vector with the distortion params of the camera.
Definition: TCamera.h:118
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#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
void undistort_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &undistorted_p)
Return the pixel position undistorted by the camera The input values &#39;col&#39; and &#39;row&#39; will be replace ...
Definition: CCamModel.cpp:121
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:37
double k2() const
Get the value of the k2 distortion parameter.
Definition: TCamera.h:174
Lightweight 3D point.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method displays clearly all the contents of the structure in textual form, sending it to a CStre...
Definition: CCamModel.cpp:313
CCamModel()
Default Constructor.
Definition: CCamModel.cpp:24
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
GLfloat GLfloat p
Definition: glext.h:5587
void setIntrinsicParamsFromValues(double fx, double fy, double cx, double cy)
Set the matrix of intrinsic params of the camera from the individual values of focal length and princ...
Definition: TCamera.h:102



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