Main MRPT website > C++ reference for MRPT 1.9.9
CCamModel.h
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 #ifndef CCamModel_H
10 #define CCamModel_H
11 
12 #include <mrpt/img/TCamera.h>
13 #include <mrpt/system/os.h>
14 #include <mrpt/vision/utils.h>
17 
18 namespace mrpt
19 {
20 namespace vision
21 {
22 /** This class represent a pinhole camera model for Monocular SLAM and
23  * implements some associated Jacobians
24  *
25  * The camera parameters are accessible in the public member CCamModel::cam
26  *
27  * - Versions:
28  * - First version: By Antonio J. Ortiz de Galistea.
29  * - 2009-2010: Rewritten by various authors.
30  *
31  * \sa mrpt::img::TCamera, CMonoSlam, the application <a
32  * href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a>
33  * for calibrating a camera
34  * \ingroup mrpt_vision_grp
35  */
37 {
38  public:
39  /** The parameters of a camera */
41 
42  /** Default Constructor */
43  CCamModel();
44 
45  void loadFromConfigFile(
47  const std::string& section) override; // See base docs
48  void dumpToTextStream(std::ostream& out) const override; // See base docs
49 
50  /** Constructor from a ini file
51  */
53 
54  /** Jacobian for undistortion the image coordinates */
55  void jacob_undistor_fm(
56  const mrpt::img::TPixelCoordf& uvd, math::CMatrixDouble& J_undist);
57 
58  /** Calculate the image coordinates undistorted
59  */
60  void jacob_undistor(
62 
63  /** Return the pixel position distorted by the camera
64  */
65  void distort_a_point(
67 
68  /** Return the pixel position undistorted by the camera
69  * The input values 'col' and 'row' will be replace for the new values
70  *(undistorted)
71  */
72  void undistort_point(
74  mrpt::img::TPixelCoordf& undistorted_p);
75 
76  /** Return the (distorted) pixel position of a 3D point given in
77  * coordinates relative to the camera (+Z pointing forward, +X to the right)
78  * \sa unproject_3D_point
79  */
80  void project_3D_point(
81  const mrpt::math::TPoint3D& p3D,
82  mrpt::img::TPixelCoordf& distorted_p) const;
83 
84  /** Return the 3D location of a point (at a fixed distance z=1), for the
85  * given (distorted) pixel position
86  * \sa project_3D_point
87  * \note Of course, there is a depth ambiguity, so the returned 3D point
88  * must be considered a direction from the camera focus, or a vector, rather
89  * than a meaninful physical point.
90  */
91  void unproject_3D_point(
92  const mrpt::img::TPixelCoordf& distorted_p,
93  mrpt::math::TPoint3D& p3D) const;
94 
95  /** Jacobian of the projection of 3D points (with distortion), as done in
96  project_3D_point \f$ \frac{\partial h}{\partial y} \f$, evaluated at the
97  point p3D (read below the full explanation)
98 
99  We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin
100  at the top-left corner),
101  and \f$ y=( y_x ~ y_y ~ y_z ) \f$ as the 3D point in space, in coordinates
102  relative to the camera (+Z pointing forwards).
103 
104  Then this method computes the 2x3 Jacobian:
105 
106  \f[
107  \frac{\partial h}{\partial y} = \frac{\partial h}{\partial u}
108  \frac{\partial u}{\partial y}
109  \f]
110 
111  With:
112 
113  \f[
114  \frac{\partial u}{\partial y} =
115  \left( \begin{array}{ccc}
116  \frac{f_x}{y_z} & 0 & - y \frac{f_x}{y_z^2} \\
117  0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\
118  \end{array} \right)
119  \f]
120 
121  where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and
122  y, respectively.
123  And, if we define:
124 
125  \f[
126  f = 1+ 2 k_1 (u_x^2+u_y^2)
127  \f]
128 
129  then:
130 
131  \f[
132  \frac{\partial h}{\partial u} =
133  \left( \begin{array}{cc}
134  \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
135  -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
136  \end{array} \right)
137  \f]
138 
139  \note JLBC: Added in March, 2009. Should be equivalent to Davison's
140  WideCamera::ProjectionJacobian
141  \sa project_3D_point
142  */
144  const mrpt::math::TPoint3D& p3D, math::CMatrixDouble& dh_dy) const;
145 
146  /** Jacobian of the unprojection of a pixel (with distortion) back into a 3D
147  point, as done in unproject_3D_point \f$ \frac{\partial y}{\partial h} \f$,
148  evaluated at the pixel p
149  \note JLBC: Added in March, 2009. Should be equivalent to Davison's
150  WideCamera::UnprojectionJacobian
151  \sa unproject_3D_point
152  */
154  const mrpt::img::TPixelCoordf& p, math::CMatrixDouble& dy_dh) const;
155 
156  template <typename T>
158  {
159  T x_, y_;
160  T x_2, y_2;
161  T R;
162  T K;
163  T x__, y__;
164  };
165  template <typename T, typename POINT>
167  const POINT& p, CameraTempVariables<T>& v) const
168  {
169  v.x_ = p[1] / p[0];
170  v.y_ = p[2] / p[0];
171  v.x_2 = square(v.x_);
172  v.y_2 = square(v.y_);
173  v.R = v.x_2 + v.y_2;
174  v.K = 1 + v.R * (cam.k1() + v.R * (cam.k2() + v.R * cam.k3()));
175  T xy = v.x_ * v.y_, p1 = cam.p1(), p2 = cam.p2();
176  v.x__ = v.x_ * v.K + 2 * p1 * xy + p2 * (3 * v.x_2 + v.y_2);
177  v.y__ = v.y_ * v.K + p1 * (v.x_2 + 3 * v.y_2) + 2 * p2 * xy;
178  }
179 
180  template <typename T, typename POINT, typename PIXEL>
181  inline void getFullProjection(const POINT& pIn, PIXEL& pOut) const
182  {
185  getFullProjectionT(tmp, pOut);
186  }
187 
188  template <typename T, typename PIXEL>
189  inline void getFullProjectionT(
190  const CameraTempVariables<T>& tmp, PIXEL& pOut) const
191  {
192  pOut[0] = cam.fx() * tmp.x__ + cam.cx();
193  pOut[1] = cam.fy() * tmp.y__ + cam.cy();
194  }
195 
196  template <typename T, typename POINT, typename MATRIX>
197  inline void getFullJacobian(const POINT& pIn, MATRIX& mOut) const
198  {
201  getFullJacobianT(pIn, tmp, mOut);
202  }
203 
204  template <typename T, typename POINT, typename MATRIX>
206  const POINT& pIn, const CameraTempVariables<T>& tmp, MATRIX& mOut) const
207  {
208  T x_ = 1 / pIn[0];
209  T x_2 = square(x_);
210  // First two jacobians...
212  T tmpK = 2 * (cam.k1() + tmp.R * (2 * cam.k2() + 3 * tmp.R * cam.k3()));
213  T tmpKx = tmpK * tmp.x_;
214  T tmpKy = tmpK * tmp.y_;
215  T yx2 = -pIn[1] * x_2;
216  T zx2 = -pIn[2] * x_2;
217  J21.set_unsafe(0, 0, yx2);
218  J21.set_unsafe(0, 1, x_);
219  J21.set_unsafe(0, 2, 0);
220  J21.set_unsafe(1, 0, zx2);
221  J21.set_unsafe(1, 1, 0);
222  J21.set_unsafe(1, 2, x_);
223  J21.set_unsafe(2, 0, tmpKx * yx2 + tmpKy * zx2);
224  J21.set_unsafe(2, 1, tmpKx * x_);
225  J21.set_unsafe(2, 2, tmpKy * x_);
226  // Last two jacobians...
227  T pxpy = 2 * (cam.p1() * tmp.x_ + cam.p2() * tmp.y_);
228  T p1y = cam.p1() * tmp.y_;
229  T p2x = cam.p2() * tmp.x_;
231  T fx = cam.fx(), fy = cam.fy();
232  J43.set_unsafe(0, 0, fx * (tmp.K + 2 * p1y + 6 * p2x));
233  J43.set_unsafe(0, 1, fx * pxpy);
234  J43.set_unsafe(0, 2, fx * tmp.x_);
235  J43.set_unsafe(1, 0, fy * pxpy);
236  J43.set_unsafe(1, 1, fy * (tmp.K + 6 * p1y + 2 * p2x));
237  J43.set_unsafe(1, 2, fy * tmp.y_);
238  mOut.multiply(J43, J21);
239  // cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;
240  }
241 
242  private:
243  // These functions are little tricks to avoid multiple initialization.
244  // They are intended to initialize the common parts of the jacobians just
245  // once,
246  // and not in each iteration.
247  // They are mostly useless outside the scope of this function.
249  {
251  res.set_unsafe(0, 1, 0);
252  res.set_unsafe(1, 0, 0);
253  return res;
254  }
256  {
258  res.set_unsafe(0, 0, 1);
259  res.set_unsafe(0, 1, 0);
260  res.set_unsafe(1, 0, 0);
261  res.set_unsafe(1, 1, 1);
262  return res;
263  }
265  {
267  res.set_unsafe(0, 1, 0);
268  res.set_unsafe(0, 2, 0);
269  res.set_unsafe(1, 0, 0);
270  res.set_unsafe(1, 2, 0);
271  res.set_unsafe(2, 0, 0);
272  res.set_unsafe(2, 1, 0);
273  res.set_unsafe(2, 2, 1);
274  res.set_unsafe(2, 3, 0);
275  return res;
276  }
277 
278  public:
279  template <typename POINTIN, typename POINTOUT, typename MAT22>
281  const POINTIN& pIn, POINTOUT& pOut, MAT22& jOut) const
282  {
283  // Temporary variables (well, there are some more, but these are the
284  // basics)
285  // WARNING!: this shortcut to avoid repeated initialization makes the
286  // method somewhat
287  // faster, but makes it incapable of being used in more than one thread
288  // simultaneously!
289  using mrpt::square;
296  static mrpt::math::CMatrixFixedNumeric<double, 2, 3> J4; // This is not
297  // initialized
298  // in a
299  // special
300  // way,
301  // although
302  // declaring
303  // it
305  mrpt::math::CArrayNumeric<double, 2> tmp2; // This would be a
306  // array<double,3>, but to
307  // avoid copying, we let
308  // "R2" lie in tmp1.
309  // Camera Parameters
310  double cx = cam.cx(), cy = cam.cy(), ifx = 1 / cam.fx(),
311  ify = 1 / cam.fy();
312  double K1 = cam.k1(), K2 = cam.k2(), p1 = cam.p1(), p2 = cam.p2(),
313  K3 = cam.k3();
314  // First step: intrinsic matrix.
315  tmp1[0] = (pIn[0] - cx) * ifx;
316  tmp1[1] = (pIn[1] - cy) * ify;
317  J1.set_unsafe(0, 0, ifx);
318  J1.set_unsafe(1, 1, ify);
319  // Second step: adding temporary variables, related to the distortion.
320  tmp1[2] = square(tmp1[0]) + square(tmp1[1]);
321  double sK1 = square(K1);
322  double K12 = sK1 - K2;
323  double K123 = -K1 * sK1 + 2 * K1 * K2 - K3; //-K1^3+2K1K2-K3
324  // tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);
325  tmp1[3] = 1 + tmp1[2] * (-K1 + tmp1[2] * (K12 + tmp1[2] * K123));
326  J2.set_unsafe(2, 0, 2 * tmp1[0]);
327  J2.set_unsafe(2, 1, 2 * tmp1[1]);
328  double jTemp = -2 * K1 + 4 * tmp1[2] * K12 + 6 * square(tmp1[2]) * K123;
329  J2.set_unsafe(3, 0, tmp1[0] * jTemp);
330  J2.set_unsafe(3, 1, tmp1[1] * jTemp);
331  // Third step: radial distortion. Really simple, since most work has
332  // been done in the previous step.
333  tmp2[0] = tmp1[0] * tmp1[3];
334  tmp2[1] = tmp1[1] * tmp1[3];
335  J3.set_unsafe(0, 0, tmp1[3]);
336  J3.set_unsafe(0, 3, tmp1[0]);
337  J3.set_unsafe(1, 1, tmp1[3]);
338  J3.set_unsafe(1, 3, tmp1[1]);
339  // Fourth step: tangential distorion. A little more complicated, but not
340  // much more.
341  double prod = tmp2[0] * tmp2[1];
342  // References to tmp1[2] are not errors! That element is "R2".
343  pOut[0] = tmp2[0] - p1 * prod - p2 * (tmp1[2] + 2 * square(tmp2[0]));
344  pOut[1] = tmp2[1] - p1 * (tmp1[2] + 2 * square(tmp2[1])) - p2 * prod;
345  J4.set_unsafe(0, 0, 1 - p1 * tmp2[1] - 4 * p2 * tmp2[0]);
346  J4.set_unsafe(0, 1, -p1 * tmp2[0]);
347  J4.set_unsafe(0, 2, -p2);
348  J4.set_unsafe(1, 0, -p2 * tmp2[1]);
349  J4.set_unsafe(1, 1, 1 - 4 * p1 * tmp2[1] - p2 * tmp2[0]);
350  J4.set_unsafe(1, 2, -p1);
351  // As fast as possible, and without more temporaries, let the jacobian
352  // be J4*J3*J2*J1;
353  jOut.multiply_ABC(J4, J3, J2); // Note that using the other order is
354  // not possible due to matrix sizes
355  // (jOut may, and most probably will, be
356  // fixed).
357  jOut.multiply(jOut, J1);
358  }
359 
360 }; // end class
361 
362 } // end namespace
363 } // end namespace
364 #endif //__CCamModel_H
double k3() const
Get the value of the k3 distortion parameter.
Definition: TCamera.h:185
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
void getFullProjection(const POINT &pIn, PIXEL &pOut) const
Definition: CCamModel.h:181
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
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
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:20
void getFullInverseModelWithJacobian(const POINTIN &pIn, POINTOUT &pOut, MAT22 &jOut) const
Definition: CCamModel.h:280
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.
A numeric matrix of compile-time fixed size.
This class allows loading and storing values and vectors of different types from a configuration text...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:163
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
Definition: CCamModel.h:166
void jacob_undistor_fm(const mrpt::img::TPixelCoordf &uvd, math::CMatrixDouble &J_undist)
Jacobian for undistortion the image coordinates.
Definition: CCamModel.cpp:26
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
void getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) const
Definition: CCamModel.h:189
GLsizei const GLchar ** string
Definition: glext.h:4101
double p1() const
Get the value of the p1 distortion parameter.
Definition: TCamera.h:181
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
void getFullJacobian(const POINT &pIn, MATRIX &mOut) const
Definition: CCamModel.h:197
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...
Definition: CCamModel.h:36
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
mrpt::math::CMatrixFixedNumeric< double, 4, 2 > secondInverseJacobian() const
Definition: CCamModel.h:255
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
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
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
Definition: CCamModel.h:205
double p2() const
Get the value of the p2 distortion parameter.
Definition: TCamera.h:183
double k1() const
Get the value of the k1 distortion parameter.
Definition: TCamera.h:177
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian() const
Definition: CCamModel.h:264
GLuint res
Definition: glext.h:7268
Lightweight 3D point.
CCamModel()
Default Constructor.
Definition: CCamModel.cpp:24
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const
Definition: CCamModel.h:248



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