Main MRPT website > C++ reference
MRPT logo

CCamModel.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CCamModel_H
00029 #define CCamModel_H
00030 
00031 #include <mrpt/utils/TCamera.h>
00032 #include <mrpt/system/os.h>
00033 #include <mrpt/vision/utils.h>
00034 #include <mrpt/math/lightweight_geom_data.h>
00035 
00036 namespace mrpt
00037 {
00038         namespace vision
00039         {
00040                 /** This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobians
00041                  *
00042                  *  The camera parameters are accessible in the public member CCamModel::cam
00043                  *
00044                  * - Versions:
00045                  *    - First version: By Antonio J. Ortiz de Galistea.
00046                  *    - 2009-2010: Rewritten by various authors.
00047                  *
00048                  * \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
00049                 */
00050                 class VISION_IMPEXP  CCamModel : public mrpt::utils::CLoadableOptions
00051                 {
00052                 public:
00053                         mrpt::utils::TCamera cam;  //!< The parameters of a camera
00054 
00055                         /** Default Constructor */
00056                         CCamModel();
00057 
00058                         /** This method loads the options from a ".ini"-like file or memory-stored string list.
00059                          */
00060                         void  loadFromConfigFile(
00061                                 const mrpt::utils::CConfigFileBase      &source,
00062                                 const std::string               &section);
00063 
00064                         /** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */
00065                         void  dumpToTextStream( CStream         &out) const;
00066 
00067                         /** Constructor from a ini file
00068                          */
00069                         CCamModel(const mrpt::utils::CConfigFileBase &cfgIni);
00070 
00071                         /** Jacobian for undistortion the image coordinates */
00072                         void  jacob_undistor_fm(const mrpt::vision::TPixelCoordf &uvd, math::CMatrixDouble &J_undist);
00073 
00074                         /** Calculate the image coordinates undistorted
00075                          */
00076                         void jacob_undistor(const mrpt::vision::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist );
00077 
00078                         /**     Return the pixel position distorted by the camera
00079                          */
00080                         void  distort_a_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &distorted_p);
00081 
00082                         /**     Return the pixel position undistorted by the camera
00083                          *      The input values 'col' and 'row' will be replace for the new values (undistorted)
00084                          */
00085                         void  undistort_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &undistorted_p);
00086 
00087                         /**     Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)
00088                          * \sa unproject_3D_point
00089                      */
00090                         void  project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::vision::TPixelCoordf &distorted_p) const;
00091 
00092                         /**     Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position
00093                          * \sa project_3D_point
00094                          * \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.
00095                      */
00096                         void  unproject_3D_point(const mrpt::vision::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const;
00097 
00098                         /** 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)
00099 
00100                         We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin at the top-left corner),
00101                         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).
00102 
00103                         Then this method computes the 2x3 Jacobian:
00104 
00105                         \f[
00106                         \frac{\partial h}{\partial y} =  \frac{\partial h}{\partial u} \frac{\partial u}{\partial y}
00107                         \f]
00108 
00109                         With:
00110 
00111                         \f[
00112                         \frac{\partial u}{\partial y} =
00113                         \left( \begin{array}{ccc}
00114                          \frac{f_x}{y_z} &  0 & - y \frac{f_x}{y_z^2} \\
00115                          0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\
00116                         \end{array} \right)
00117                         \f]
00118 
00119                         where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and y, respectively.
00120                         And, if we define:
00121 
00122                         \f[
00123                          f = 1+ 2  k_1  (u_x^2+u_y^2)
00124                         \f]
00125 
00126                         then:
00127 
00128                         \f[
00129                         \frac{\partial h}{\partial u} =
00130                         \left( \begin{array}{cc}
00131                          \frac{ 1+2 k_1 u_y^2 }{f^{3/2}}  &  -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
00132                          -\frac{2 u_x u_y k_1 }{f^{3/2}}  & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
00133                         \end{array} \right)
00134                         \f]
00135 
00136                         \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian
00137                         \sa project_3D_point
00138                         */
00139                         void jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble & dh_dy ) const;
00140 
00141 
00142                         /* 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
00143                         \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::UnprojectionJacobian
00144                         \sa unproject_3D_point
00145                         */
00146                         void jacobian_unproject_with_distortion(const mrpt::vision::TPixelCoordf &p, math::CMatrixDouble & dy_dh ) const;
00147 
00148                         template<typename T> struct CameraTempVariables {
00149                                 T x_,y_;
00150                                 T x_2,y_2;
00151                                 T R;
00152                                 T K;
00153                                 T x__,y__;
00154                         };
00155                         template<typename T,typename POINT> void getTemporaryVariablesForTransform(const POINT &p,CameraTempVariables<T> &v) const      {
00156                                 v.x_=p[1]/p[0];
00157                                 v.y_=p[2]/p[0];
00158                                 v.x_2=square(v.x_);
00159                                 v.y_2=square(v.y_);
00160                                 v.R=v.x_2+v.y_2;
00161                                 v.K=1+v.R*(cam.k1()+v.R*(cam.k2()+v.R*cam.k3()));
00162                                 T xy=v.x_*v.y_,p1=cam.p1(),p2=cam.p2();
00163                                 v.x__=v.x_*v.K+2*p1*xy+p2*(3*v.x_2+v.y_2);
00164                                 v.y__=v.y_*v.K+p1*(v.x_2+3*v.y_2)+2*p2*xy;
00165                         }
00166 
00167                         template<typename T,typename POINT,typename PIXEL> inline void getFullProjection(const POINT &pIn,PIXEL &pOut) const    {
00168                                 CameraTempVariables<T> tmp;
00169                                 getTemporaryVariablesForTransform(pIn,tmp);
00170                                 getFullProjectionT(tmp,pOut);
00171                         }
00172 
00173                         template<typename T,typename PIXEL> inline void getFullProjectionT(const CameraTempVariables<T> &tmp,PIXEL &pOut) const {
00174                                 pOut[0]=cam.fx()*tmp.x__+cam.cx();
00175                                 pOut[1]=cam.fy()*tmp.y__+cam.cy();
00176                         }
00177 
00178                         template<typename T,typename POINT,typename MATRIX> inline void getFullJacobian(const POINT &pIn,MATRIX &mOut) const    {
00179                                 CameraTempVariables<T> tmp;
00180                                 getTemporaryVariablesForTransform(pIn,tmp);
00181                                 getFullJacobianT(pIn,tmp,mOut);
00182                         }
00183 
00184                         template<typename T,typename POINT,typename MATRIX> void getFullJacobianT(const POINT &pIn,const CameraTempVariables<T> &tmp,MATRIX &mOut) const        {
00185                                 T x_=1/pIn[0];
00186                                 T x_2=square(x_);
00187                                 //First two jacobians...
00188                                 CMatrixFixedNumeric<T,3,3> J21;
00189                                 T tmpK=2*(cam.k1()+tmp.R*(2*cam.k2()+3*tmp.R*cam.k3()));
00190                                 T tmpKx=tmpK*tmp.x_;
00191                                 T tmpKy=tmpK*tmp.y_;
00192                                 T yx2=-pIn[1]*x_2;
00193                                 T zx2=-pIn[2]*x_2;
00194                                 J21.set_unsafe(0,0,yx2);
00195                                 J21.set_unsafe(0,1,x_);
00196                                 J21.set_unsafe(0,2,0);
00197                                 J21.set_unsafe(1,0,zx2);
00198                                 J21.set_unsafe(1,1,0);
00199                                 J21.set_unsafe(1,2,x_);
00200                                 J21.set_unsafe(2,0,tmpKx*yx2+tmpKy*zx2);
00201                                 J21.set_unsafe(2,1,tmpKx*x_);
00202                                 J21.set_unsafe(2,2,tmpKy*x_);
00203                                 //Last two jacobians...
00204                                 T pxpy=2*(cam.p1()*tmp.x_+cam.p2()*tmp.y_);
00205                                 T p1y=cam.p1()*tmp.y_;
00206                                 T p2x=cam.p2()*tmp.x_;
00207                                 CMatrixFixedNumeric<T,2,3> J43;
00208                                 T fx=cam.fx(),fy=cam.fy();
00209                                 J43.set_unsafe(0,0,fx*(tmp.K+2*p1y+6*p2x));
00210                                 J43.set_unsafe(0,1,fx*pxpy);
00211                                 J43.set_unsafe(0,2,fx*tmp.x_);
00212                                 J43.set_unsafe(1,0,fy*pxpy);
00213                                 J43.set_unsafe(1,1,fy*(tmp.K+6*p1y+2*p2x));
00214                                 J43.set_unsafe(1,2,fy*tmp.y_);
00215                                 mOut.multiply(J43,J21);
00216                                 //cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;
00217                         }
00218                 private:
00219                         //These functions are little tricks to avoid multiple initialization.
00220                         //They are intended to initialize the common parts of the jacobians just once,
00221                         //and not in each iteration.
00222                         //They are mostly useless outside the scope of this function.
00223                         CMatrixFixedNumeric<double,2,2> firstInverseJacobian() const    {
00224                                 CMatrixFixedNumeric<double,2,2> res;
00225                                 res.set_unsafe(0,1,0);
00226                                 res.set_unsafe(1,0,0);
00227                                 return res;
00228                         }
00229                         CMatrixFixedNumeric<double,4,2> secondInverseJacobian() const   {
00230                                 CMatrixFixedNumeric<double,4,2> res;
00231                                 res.set_unsafe(0,0,1);
00232                                 res.set_unsafe(0,1,0);
00233                                 res.set_unsafe(1,0,0);
00234                                 res.set_unsafe(1,1,1);
00235                                 return res;
00236                         }
00237                         CMatrixFixedNumeric<double,3,4> thirdInverseJacobian() const    {
00238                                 CMatrixFixedNumeric<double,3,4> res;
00239                                 res.set_unsafe(0,1,0);
00240                                 res.set_unsafe(0,2,0);
00241                                 res.set_unsafe(1,0,0);
00242                                 res.set_unsafe(1,2,0);
00243                                 res.set_unsafe(2,0,0);
00244                                 res.set_unsafe(2,1,0);
00245                                 res.set_unsafe(2,2,1);
00246                                 res.set_unsafe(2,3,0);
00247                                 return res;
00248                         }
00249                 public:
00250                         template<typename POINTIN,typename POINTOUT,typename MAT22> void getFullInverseModelWithJacobian(const POINTIN &pIn,POINTOUT &pOut,MAT22 &jOut) const   {
00251                                 //Temporary variables (well, there are some more, but these are the basics)
00252                                 //WARNING!: this shortcut to avoid repeated initialization makes the method somewhat
00253                                 //faster, but makes it incapable of being used in more than one thread
00254                                 //simultaneously!
00255                                 static CMatrixFixedNumeric<double,2,2> J1(firstInverseJacobian());
00256                                 static CMatrixFixedNumeric<double,4,2> J2(secondInverseJacobian());
00257                                 static CMatrixFixedNumeric<double,3,4> J3(thirdInverseJacobian());
00258                                 static CMatrixFixedNumeric<double,2,3> J4;      //This is not initialized in a special way, although declaring it
00259                                 CArray<double,4> tmp1;
00260                                 CArray<double,2> tmp2;  //This would be a CArray<double,3>, but to avoid copying, we let "R2" lie in tmp1.
00261                                 //Camera Parameters
00262                                 double cx=cam.cx(),cy=cam.cy(),ifx=1/cam.fx(),ify=1/cam.fy();
00263                                 double K1=cam.k1(),K2=cam.k2(),p1=cam.p1(),p2=cam.p2(),K3=cam.k3();
00264                                 //First step: intrinsic matrix.
00265                                 tmp1[0]=(pIn[0]-cx)*ifx;
00266                                 tmp1[1]=(pIn[1]-cy)*ify;
00267                                 J1.set_unsafe(0,0,ifx);
00268                                 J1.set_unsafe(1,1,ify);
00269                                 //Second step: adding temporary variables, related to the distortion.
00270                                 tmp1[2]=square(tmp1[0])+square(tmp1[1]);
00271                                 double sK1=square(K1);
00272                                 double K12=sK1-K2;
00273                                 double K123=-K1*sK1+2*K1*K2-K3; //-K1³+2K1K2-K3
00274                                 //tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);
00275                                 tmp1[3]=1+tmp1[2]*(-K1+tmp1[2]*(K12+tmp1[2]*K123));
00276                                 J2.set_unsafe(2,0,2*tmp1[0]);
00277                                 J2.set_unsafe(2,1,2*tmp1[1]);
00278                                 double jTemp=-2*K1+4*tmp1[2]*K12+6*square(tmp1[2])*K123;
00279                                 J2.set_unsafe(3,0,tmp1[0]*jTemp);
00280                                 J2.set_unsafe(3,1,tmp1[1]*jTemp);
00281                                 //Third step: radial distortion. Really simple, since most work has been done in the previous step.
00282                                 tmp2[0]=tmp1[0]*tmp1[3];
00283                                 tmp2[1]=tmp1[1]*tmp1[3];
00284                                 J3.set_unsafe(0,0,tmp1[3]);
00285                                 J3.set_unsafe(0,3,tmp1[0]);
00286                                 J3.set_unsafe(1,1,tmp1[3]);
00287                                 J3.set_unsafe(1,3,tmp1[1]);
00288                                 //Fourth step: tangential distorion. A little more complicated, but not much more.
00289                                 double prod=tmp2[0]*tmp2[1];
00290                                 //References to tmp1[2] are not errors! That element is "R2".
00291                                 pOut[0]=tmp2[0]-p1*prod-p2*(tmp1[2]+2*square(tmp2[0]));
00292                                 pOut[1]=tmp2[1]-p1*(tmp1[2]+2*square(tmp2[1]))-p2*prod;
00293                                 J4.set_unsafe(0,0,1-p1*tmp2[1]-4*p2*tmp2[0]);
00294                                 J4.set_unsafe(0,1,-p1*tmp2[0]);
00295                                 J4.set_unsafe(0,2,-p2);
00296                                 J4.set_unsafe(1,0,-p2*tmp2[1]);
00297                                 J4.set_unsafe(1,1,1-4*p1*tmp2[1]-p2*tmp2[0]);
00298                                 J4.set_unsafe(1,2,-p1);
00299                                 //As fast as possible, and without more temporaries, let the jacobian be J4*J3*J2*J1;
00300                                 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).
00301                                 jOut.multiply(jOut,J1);
00302                         }
00303 
00304                 }; // end class
00305 
00306         } // end namespace
00307 } // end namespace
00308 #endif //__CCamModel_H



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011