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