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


Page generated by Doxygen 1.9.1 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at mar 26 may 2026 13:12:03 CEST