Main MRPT website > C++ reference
MRPT logo
CCamModel.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CCamModel_H
29 #define CCamModel_H
30 
31 #include <mrpt/utils/TCamera.h>
32 #include <mrpt/system/os.h>
33 #include <mrpt/vision/utils.h>
34 #include <mrpt/math/lightweight_geom_data.h>
35 
36 namespace mrpt
37 {
38  namespace vision
39  {
40  /** This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobians
41  *
42  * The camera parameters are accessible in the public member CCamModel::cam
43  *
44  * - Versions:
45  * - First version: By Antonio J. Ortiz de Galistea.
46  * - 2009-2010: Rewritten by various authors.
47  *
48  * \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
49  * \ingroup mrpt_vision_grp
50  */
51  class VISION_IMPEXP CCamModel : public mrpt::utils::CLoadableOptions
52  {
53  public:
54  mrpt::utils::TCamera cam; //!< The parameters of a camera
55 
56  /** Default Constructor */
57  CCamModel();
58 
59  /** This method loads the options from a ".ini"-like file or memory-stored string list.
60  */
61  void loadFromConfigFile(
62  const mrpt::utils::CConfigFileBase &source,
63  const std::string &section);
64 
65  /** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */
66  void dumpToTextStream( CStream &out) const;
67 
68  /** Constructor from a ini file
69  */
70  CCamModel(const mrpt::utils::CConfigFileBase &cfgIni);
71 
72  /** Jacobian for undistortion the image coordinates */
73  void jacob_undistor_fm(const mrpt::vision::TPixelCoordf &uvd, math::CMatrixDouble &J_undist);
74 
75  /** Calculate the image coordinates undistorted
76  */
77  void jacob_undistor(const mrpt::vision::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist );
78 
79  /** Return the pixel position distorted by the camera
80  */
81  void distort_a_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &distorted_p);
82 
83  /** Return the pixel position undistorted by the camera
84  * The input values 'col' and 'row' will be replace for the new values (undistorted)
85  */
86  void undistort_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &undistorted_p);
87 
88  /** Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)
89  * \sa unproject_3D_point
90  */
91  void project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::vision::TPixelCoordf &distorted_p) const;
92 
93  /** Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position
94  * \sa project_3D_point
95  * \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.
96  */
97  void unproject_3D_point(const mrpt::vision::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const;
98 
99  /** 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)
100 
101  We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin at the top-left corner),
102  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).
103 
104  Then this method computes the 2x3 Jacobian:
105 
106  \f[
107  \frac{\partial h}{\partial y} = \frac{\partial h}{\partial u} \frac{\partial u}{\partial y}
108  \f]
109 
110  With:
111 
112  \f[
113  \frac{\partial u}{\partial y} =
114  \left( \begin{array}{ccc}
115  \frac{f_x}{y_z} & 0 & - y \frac{f_x}{y_z^2} \\
116  0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\
117  \end{array} \right)
118  \f]
119 
120  where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and y, respectively.
121  And, if we define:
122 
123  \f[
124  f = 1+ 2 k_1 (u_x^2+u_y^2)
125  \f]
126 
127  then:
128 
129  \f[
130  \frac{\partial h}{\partial u} =
131  \left( \begin{array}{cc}
132  \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
133  -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
134  \end{array} \right)
135  \f]
136 
137  \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian
138  \sa project_3D_point
139  */
140  void jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble & dh_dy ) const;
141 
142 
143  /** 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
144  \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::UnprojectionJacobian
145  \sa unproject_3D_point
146  */
147  void jacobian_unproject_with_distortion(const mrpt::vision::TPixelCoordf &p, math::CMatrixDouble & dy_dh ) const;
148 
149  template<typename T> struct CameraTempVariables {
150  T x_,y_;
151  T x_2,y_2;
152  T R;
153  T K;
154  T x__,y__;
155  };
156  template<typename T,typename POINT> void getTemporaryVariablesForTransform(const POINT &p,CameraTempVariables<T> &v) const {
157  v.x_=p[1]/p[0];
158  v.y_=p[2]/p[0];
159  v.x_2=square(v.x_);
160  v.y_2=square(v.y_);
161  v.R=v.x_2+v.y_2;
162  v.K=1+v.R*(cam.k1()+v.R*(cam.k2()+v.R*cam.k3()));
163  T xy=v.x_*v.y_,p1=cam.p1(),p2=cam.p2();
164  v.x__=v.x_*v.K+2*p1*xy+p2*(3*v.x_2+v.y_2);
165  v.y__=v.y_*v.K+p1*(v.x_2+3*v.y_2)+2*p2*xy;
166  }
167 
168  template<typename T,typename POINT,typename PIXEL> inline void getFullProjection(const POINT &pIn,PIXEL &pOut) const {
169  CameraTempVariables<T> tmp;
170  getTemporaryVariablesForTransform(pIn,tmp);
171  getFullProjectionT(tmp,pOut);
172  }
173 
174  template<typename T,typename PIXEL> inline void getFullProjectionT(const CameraTempVariables<T> &tmp,PIXEL &pOut) const {
175  pOut[0]=cam.fx()*tmp.x__+cam.cx();
176  pOut[1]=cam.fy()*tmp.y__+cam.cy();
177  }
178 
179  template<typename T,typename POINT,typename MATRIX> inline void getFullJacobian(const POINT &pIn,MATRIX &mOut) const {
180  CameraTempVariables<T> tmp;
181  getTemporaryVariablesForTransform(pIn,tmp);
182  getFullJacobianT(pIn,tmp,mOut);
183  }
184 
185  template<typename T,typename POINT,typename MATRIX> void getFullJacobianT(const POINT &pIn,const CameraTempVariables<T> &tmp,MATRIX &mOut) const {
186  T x_=1/pIn[0];
187  T x_2=square(x_);
188  //First two jacobians...
189  CMatrixFixedNumeric<T,3,3> J21;
190  T tmpK=2*(cam.k1()+tmp.R*(2*cam.k2()+3*tmp.R*cam.k3()));
191  T tmpKx=tmpK*tmp.x_;
192  T tmpKy=tmpK*tmp.y_;
193  T yx2=-pIn[1]*x_2;
194  T zx2=-pIn[2]*x_2;
195  J21.set_unsafe(0,0,yx2);
196  J21.set_unsafe(0,1,x_);
197  J21.set_unsafe(0,2,0);
198  J21.set_unsafe(1,0,zx2);
199  J21.set_unsafe(1,1,0);
200  J21.set_unsafe(1,2,x_);
201  J21.set_unsafe(2,0,tmpKx*yx2+tmpKy*zx2);
202  J21.set_unsafe(2,1,tmpKx*x_);
203  J21.set_unsafe(2,2,tmpKy*x_);
204  //Last two jacobians...
205  T pxpy=2*(cam.p1()*tmp.x_+cam.p2()*tmp.y_);
206  T p1y=cam.p1()*tmp.y_;
207  T p2x=cam.p2()*tmp.x_;
208  CMatrixFixedNumeric<T,2,3> J43;
209  T fx=cam.fx(),fy=cam.fy();
210  J43.set_unsafe(0,0,fx*(tmp.K+2*p1y+6*p2x));
211  J43.set_unsafe(0,1,fx*pxpy);
212  J43.set_unsafe(0,2,fx*tmp.x_);
213  J43.set_unsafe(1,0,fy*pxpy);
214  J43.set_unsafe(1,1,fy*(tmp.K+6*p1y+2*p2x));
215  J43.set_unsafe(1,2,fy*tmp.y_);
216  mOut.multiply(J43,J21);
217  //cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;
218  }
219  private:
220  //These functions are little tricks to avoid multiple initialization.
221  //They are intended to initialize the common parts of the jacobians just once,
222  //and not in each iteration.
223  //They are mostly useless outside the scope of this function.
224  CMatrixFixedNumeric<double,2,2> firstInverseJacobian() const {
225  CMatrixFixedNumeric<double,2,2> res;
226  res.set_unsafe(0,1,0);
227  res.set_unsafe(1,0,0);
228  return res;
229  }
230  CMatrixFixedNumeric<double,4,2> secondInverseJacobian() const {
231  CMatrixFixedNumeric<double,4,2> res;
232  res.set_unsafe(0,0,1);
233  res.set_unsafe(0,1,0);
234  res.set_unsafe(1,0,0);
235  res.set_unsafe(1,1,1);
236  return res;
237  }
238  CMatrixFixedNumeric<double,3,4> thirdInverseJacobian() const {
239  CMatrixFixedNumeric<double,3,4> res;
240  res.set_unsafe(0,1,0);
241  res.set_unsafe(0,2,0);
242  res.set_unsafe(1,0,0);
243  res.set_unsafe(1,2,0);
244  res.set_unsafe(2,0,0);
245  res.set_unsafe(2,1,0);
246  res.set_unsafe(2,2,1);
247  res.set_unsafe(2,3,0);
248  return res;
249  }
250  public:
251  template<typename POINTIN,typename POINTOUT,typename MAT22> void getFullInverseModelWithJacobian(const POINTIN &pIn,POINTOUT &pOut,MAT22 &jOut) const {
252  //Temporary variables (well, there are some more, but these are the basics)
253  //WARNING!: this shortcut to avoid repeated initialization makes the method somewhat
254  //faster, but makes it incapable of being used in more than one thread
255  //simultaneously!
256  static CMatrixFixedNumeric<double,2,2> J1(firstInverseJacobian());
257  static CMatrixFixedNumeric<double,4,2> J2(secondInverseJacobian());
258  static CMatrixFixedNumeric<double,3,4> J3(thirdInverseJacobian());
259  static CMatrixFixedNumeric<double,2,3> J4; //This is not initialized in a special way, although declaring it
260  CArray<double,4> tmp1;
261  CArray<double,2> tmp2; //This would be a CArray<double,3>, but to avoid copying, we let "R2" lie in tmp1.
262  //Camera Parameters
263  double cx=cam.cx(),cy=cam.cy(),ifx=1/cam.fx(),ify=1/cam.fy();
264  double K1=cam.k1(),K2=cam.k2(),p1=cam.p1(),p2=cam.p2(),K3=cam.k3();
265  //First step: intrinsic matrix.
266  tmp1[0]=(pIn[0]-cx)*ifx;
267  tmp1[1]=(pIn[1]-cy)*ify;
268  J1.set_unsafe(0,0,ifx);
269  J1.set_unsafe(1,1,ify);
270  //Second step: adding temporary variables, related to the distortion.
271  tmp1[2]=square(tmp1[0])+square(tmp1[1]);
272  double sK1=square(K1);
273  double K12=sK1-K2;
274  double K123=-K1*sK1+2*K1*K2-K3; //-K1³+2K1K2-K3
275  //tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);
276  tmp1[3]=1+tmp1[2]*(-K1+tmp1[2]*(K12+tmp1[2]*K123));
277  J2.set_unsafe(2,0,2*tmp1[0]);
278  J2.set_unsafe(2,1,2*tmp1[1]);
279  double jTemp=-2*K1+4*tmp1[2]*K12+6*square(tmp1[2])*K123;
280  J2.set_unsafe(3,0,tmp1[0]*jTemp);
281  J2.set_unsafe(3,1,tmp1[1]*jTemp);
282  //Third step: radial distortion. Really simple, since most work has been done in the previous step.
283  tmp2[0]=tmp1[0]*tmp1[3];
284  tmp2[1]=tmp1[1]*tmp1[3];
285  J3.set_unsafe(0,0,tmp1[3]);
286  J3.set_unsafe(0,3,tmp1[0]);
287  J3.set_unsafe(1,1,tmp1[3]);
288  J3.set_unsafe(1,3,tmp1[1]);
289  //Fourth step: tangential distorion. A little more complicated, but not much more.
290  double prod=tmp2[0]*tmp2[1];
291  //References to tmp1[2] are not errors! That element is "R2".
292  pOut[0]=tmp2[0]-p1*prod-p2*(tmp1[2]+2*square(tmp2[0]));
293  pOut[1]=tmp2[1]-p1*(tmp1[2]+2*square(tmp2[1]))-p2*prod;
294  J4.set_unsafe(0,0,1-p1*tmp2[1]-4*p2*tmp2[0]);
295  J4.set_unsafe(0,1,-p1*tmp2[0]);
296  J4.set_unsafe(0,2,-p2);
297  J4.set_unsafe(1,0,-p2*tmp2[1]);
298  J4.set_unsafe(1,1,1-4*p1*tmp2[1]-p2*tmp2[0]);
299  J4.set_unsafe(1,2,-p1);
300  //As fast as possible, and without more temporaries, let the jacobian be J4*J3*J2*J1;
301  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).
302  jOut.multiply(jOut,J1);
303  }
304 
305  }; // end class
306 
307  } // end namespace
308 } // end namespace
309 #endif //__CCamModel_H


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013