00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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/math/CMatrixTemplateNumeric.h> 00032 #include <mrpt/math/CVectorTemplate.h> 00033 #include <mrpt/utils/CLoadableOptions.h> 00034 #include <mrpt/system/os.h> 00035 #include <mrpt/vision/utils.h> 00036 #include <mrpt/math/lightweight_geom_data.h> 00037 00038 namespace mrpt 00039 { 00040 namespace vision 00041 { 00042 /** Structure to hold the parameters of a pinhole camera model (for use in mono-slam). 00043 * 00044 * \sa CCamModel, the application <a href="http://babel.isa.uma.es/mrpt/index.php/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera 00045 */ 00046 struct MRPTDLLIMPEXP TCamera{ 00047 TCamera() 00048 { 00049 dist[0]=dist[1]=dist[2]=dist[3]=dist[4]= 0; 00050 } 00051 00052 unsigned int nrows; 00053 unsigned int ncols; 00054 double cx; //!< 'x' position of the optic center over the image 00055 double cy; //!< 'y' position of the optic center over the image 00056 double fx; //!< the focal distance of the camera, in 'x' pixels 00057 double fy; //!< the focal distance of the camera, in 'y' pixels 00058 00059 double dist[5]; //!< [k1 k2 t1 t2 0] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (default=0) 00060 }; 00061 00062 /** This class represent the camera model for Monocular SLAM. 00063 * For now, the only model is the pinhole model. 00064 * 00065 * - Versions: 00066 * - First version: By Antonio J. Ortiz de Galistea. 00067 * - March 2009, ... 00068 * 00069 * \sa CMonoSlam, , the application <a href="http://babel.isa.uma.es/mrpt/index.php/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera 00070 */ 00071 class MRPTDLLIMPEXP CCamModel : public mrpt::utils::CLoadableOptions 00072 { 00073 protected: 00074 vision::TCamera cam; 00075 00076 public: 00077 /** Defalult Constructor 00078 */ 00079 CCamModel(); 00080 00081 /** This method loads the options from a ".ini"-like file or memory-stored string list. 00082 */ 00083 void loadFromConfigFile( 00084 const mrpt::utils::CConfigFileBase &source, 00085 const std::string §ion); 00086 00087 /** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */ 00088 void dumpToTextStream( CStream &out) const; 00089 00090 00091 /** Constructor from a init file 00092 */ 00093 CCamModel(const mrpt::utils::CConfigFileBase &cfgIni); 00094 00095 /** Return the rows number for a image captured by the camera. 00096 */ 00097 unsigned int cam_nrows()const {return cam.nrows;} 00098 00099 /** Return the columns number for a image captured by the camera. 00100 */ 00101 unsigned int cam_ncols()const {return cam.ncols;} 00102 00103 /** Return the 'x' position of the optic center over the image 00104 */ 00105 double cam_cx()const {return cam.cx;} 00106 00107 /** Return the 'y' position of the optic center over the image 00108 */ 00109 double cam_cy()const {return cam.cy;} 00110 00111 /** Return the first parameter of radial distortion 00112 */ 00113 double cam_k1()const {return cam.dist[0];} 00114 00115 /** Return the second parameter of radial distortion 00116 */ 00117 double cam_k2()const {return cam.dist[1];} 00118 00119 /** Return the focal distance of the camera, in 'x' PIXELS (Element (1,1) in the camera matrix) 00120 */ 00121 double cam_fx()const {return cam.fx;} 00122 00123 /** Return the focal distance of the camera, in 'y' PIXELS (Element (2,2) in the camera matrix) 00124 */ 00125 double cam_fy()const {return cam.fy;} 00126 00127 /** Jacobian for undistortion the image coordinates 00128 */ 00129 void jacob_undistor_fm(const mrpt::vision::TPixelCoordf &uvd, math::CMatrixDouble &J_undist); 00130 00131 /** Calculate the image coordinates undistorted 00132 */ 00133 void jacob_undistor(const mrpt::vision::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist ); 00134 00135 /** Return the pixel position distorted by the camera 00136 */ 00137 void distort_a_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &distorted_p); 00138 00139 /** Return the pixel position undistorted by the camera 00140 * The input values 'col' and 'row' will be replace for the new values (undistorted) 00141 */ 00142 void undistort_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &undistorted_p); 00143 00144 00145 /** Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right) 00146 * \sa unproject_3D_point 00147 */ 00148 void project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::vision::TPixelCoordf &distorted_p) const; 00149 00150 /** Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position 00151 * \sa project_3D_point 00152 * \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. 00153 */ 00154 void unproject_3D_point(const mrpt::vision::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const; 00155 00156 /** 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) 00157 00158 We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin at the top-left corner), 00159 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). 00160 00161 Then this method computes the 2x3 Jacobian: 00162 00163 \f[ 00164 \frac{\partial h}{\partial y} = \frac{\partial h}{\partial u} \frac{\partial u}{\partial y} 00165 \f] 00166 00167 With: 00168 00169 \f[ 00170 \frac{\partial u}{\partial y} = 00171 \left( \begin{array}{ccc} 00172 \frac{f_x}{y_z} & 0 & - y \frac{f_x}{y_z^2} \\ 00173 0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\ 00174 \end{array} \right) 00175 \f] 00176 00177 where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and y, respectively. 00178 And, if we define: 00179 00180 \f[ 00181 f = 1+ 2 k_1 (u_x^2+u_y^2) 00182 \f] 00183 00184 then: 00185 00186 \f[ 00187 \frac{\partial h}{\partial u} = 00188 \left( \begin{array}{cc} 00189 \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\ 00190 -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}} 00191 \end{array} \right) 00192 \f] 00193 00194 \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian 00195 \sa project_3D_point 00196 */ 00197 void jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble & dh_dy ) const; 00198 00199 00200 /* 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 00201 \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::UnprojectionJacobian 00202 \sa unproject_3D_point 00203 */ 00204 void jacobian_unproject_with_distortion(const mrpt::vision::TPixelCoordf &p, math::CMatrixDouble & dy_dh ) const; 00205 00206 }; // end class 00207 00208 } // end namespace 00209 } // end namespace 00210 #endif //__CCamModel_H
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:02:22 EDT 2009 |