Main MRPT website > C++ reference
MRPT logo

TCamera.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  TCamera_H
00029 #define  TCamera_H
00030 
00031 #include <mrpt/math/CMatrixTemplateNumeric.h>
00032 #include <mrpt/math/CMatrixFixedNumeric.h>
00033 #include <mrpt/utils/CLoadableOptions.h>
00034 #include <mrpt/utils/CConfigFileBase.h>
00035 #include <mrpt/utils/CConfigFileMemory.h>
00036 #include <mrpt/utils/CSerializable.h>
00037 #include <mrpt/poses/CPose3DQuat.h>
00038 
00039 namespace mrpt
00040 {
00041         namespace utils
00042         {
00043                 using namespace mrpt::math;
00044                 using namespace mrpt::poses;
00045 
00046                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( TCamera, mrpt::utils::CSerializable )
00047 
00048                 /** Structure to hold the parameters of a pinhole camera model.
00049                   *  The parameters obtained for one camera resolution can be used for any other resolution by means of the method TCamera::scaleToResolution()
00050                   *
00051                   * \sa mrpt::vision::CCamModel, the application <a href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera
00052                  */
00053                 class BASE_IMPEXP TCamera : public mrpt::utils::CSerializable
00054                 {
00055                         DEFINE_SERIALIZABLE( TCamera )
00056 
00057                 public:
00058                         TCamera() : ncols(640), nrows(480), focalLengthMeters(0)
00059                         {
00060                                 intrinsicParams.set_unsafe(0,0,507.808);
00061                                 intrinsicParams.set_unsafe(1,1,507.808);
00062                                 intrinsicParams.set_unsafe(0,2,356.2368);
00063                                 intrinsicParams.set_unsafe(1,2,252.9216);
00064                                 intrinsicParams.set_unsafe(2,2,1);
00065                                 for (size_t i=0;i<dist.SizeAtCompileTime ;i++)
00066                                         dist[i] = 0;
00067                         }
00068 
00069                         /** @name Camera parameters
00070                             @{ */
00071 
00072                         uint32_t                        ncols,nrows;        //!< Camera resolution
00073                         CMatrixDouble33         intrinsicParams;    //!< Matrix of intrinsic parameters (containing the focal length and principal point coordinates)
00074                         CArrayDouble<5>         dist;               //!< [k1 k2 t1 t2 t3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (default=0)
00075                         double                          focalLengthMeters;  //!< The focal length of the camera, in meters (can be used among 'intrinsicParams' to determine the pixel size).
00076 
00077                         /** @} */
00078 
00079                         /** Rescale all the parameters for a new camera resolution (it raises an exception if the aspect ratio is modified, which is not permitted).
00080                           */
00081                         void scaleToResolution(uint32_t new_ncols, uint32_t new_nrows);
00082 
00083                         /**  Save as a config block:
00084                           *  \code
00085                           *  [SECTION]
00086                           *  resolution = [NCOLS NROWS]
00087                           *  cx         = CX
00088                           *  cy         = CY
00089                           *  fx         = FX
00090                           *  fy         = FY
00091                           *  dist       = [K1 K2 T1 T2 K3]
00092                           *  focal_length = FOCAL_LENGTH
00093                           *  \endcode
00094                           */
00095                         void saveToConfigFile( const std::string &section, mrpt::utils::CConfigFileBase &cfg ) const;
00096 
00097                         /**  Load all the params from a config source, in the format used in saveToConfigFile(), that is:
00098                           *
00099                           *  \code
00100                           *  [SECTION]
00101                           *  resolution = [NCOLS NROWS]
00102                           *  cx         = CX
00103                           *  cy         = CY
00104                           *  fx         = FX
00105                           *  fy         = FY
00106                           *  dist       = [K1 K2 T1 T2 K3]
00107                           *  focal_length = FOCAL_LENGTH  [optional field]
00108                           *  \endcode
00109                           *  \exception std::exception on missing fields
00110                           */
00111                         void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg );
00112 
00113                         /** Dumps all the parameters as a multi-line string, with the same format than \a saveToConfigFile.  \sa saveToConfigFile */
00114                         std::string dumpAsText()
00115                         {
00116                                 mrpt::utils::CConfigFileMemory cfg;
00117                                 saveToConfigFile("",cfg);
00118                                 return cfg.getContent();
00119                         }
00120 
00121 
00122                         /** Set the matrix of intrinsic params of the camera from the individual values of focal length and principal point coordinates (in pixels)
00123                           */
00124                         inline void setIntrinsicParamsFromValues ( double fx, double fy, double cx, double cy )
00125                         {
00126                                 intrinsicParams.set_unsafe( 0, 0, fx );
00127                                 intrinsicParams.set_unsafe( 1, 1, fy );
00128                                 intrinsicParams.set_unsafe( 0, 2, cx );
00129                                 intrinsicParams.set_unsafe( 1, 2, cy );
00130                         }
00131 
00132                         /** Get the vector of distortion params of the camera  */
00133                         inline void getDistortionParamsVector ( CMatrixDouble15 &distParVector ) const
00134                         {
00135                                 for (size_t i=0;i<5;i++)
00136                                         distParVector.set_unsafe(0,i, dist[i]);
00137                         }
00138 
00139                         /** Get a vector with the distortion params of the camera  */
00140                         inline std::vector<double> getDistortionParamsAsVector () const {
00141                                 std::vector<double>  v(5);
00142                                 for (size_t i=0;i<5;i++)
00143                                         v[i] = dist[i];
00144                                 return v;
00145                         }
00146 
00147                         /** Set the whole vector of distortion params of the camera */
00148                         void setDistortionParamsVector( const CMatrixDouble15 &distParVector )
00149                         {
00150                                 for (size_t i=0;i<5;i++)
00151                                         dist[i] = distParVector.get_unsafe(0,i);
00152                         }
00153 
00154                         /** Set the whole vector of distortion params of the camera from a 4 or 5-vector */
00155                         template <class VECTORLIKE>
00156                         void setDistortionParamsVector( const VECTORLIKE &distParVector )
00157                         {
00158                                 ASSERT_(distParVector.size()==4 || distParVector.size()==5)
00159                                 dist[4] = 0; // Default value
00160                                 for (typename VECTORLIKE::Index i=0;i<distParVector.size();i++)
00161                                         dist[i] = distParVector[i];
00162                         }
00163 
00164                         /** Set the vector of distortion params of the camera from the individual values of the distortion coefficients
00165                           */
00166                         inline void setDistortionParamsFromValues( double k1, double k2, double p1, double p2, double k3 = 0 )
00167                         {
00168                                 dist[0] = k1;
00169                                 dist[1] = k2;
00170                                 dist[2] = p1;
00171                                 dist[3] = p2;
00172                                 dist[4] = k3;
00173                         }
00174 
00175                         /** Get the value of the principal point x-coordinate (in pixels). */
00176                         inline double cx() const { return intrinsicParams(0,2); }
00177                         /** Get the value of the principal point y-coordinate  (in pixels). */
00178                         inline double cy() const { return intrinsicParams(1,2); }
00179                         /** Get the value of the focal length x-value (in pixels). */
00180                         inline double fx() const { return intrinsicParams(0,0); }
00181                         /** Get the value of the focal length y-value (in pixels). */
00182                         inline double fy() const { return intrinsicParams(1,1); }
00183 
00184                         /** Set the value of the principal point x-coordinate (in pixels). */
00185                         inline void cx(double val) { intrinsicParams(0,2)=val; }
00186                         /** Set the value of the principal point y-coordinate  (in pixels). */
00187                         inline void cy(double val) { intrinsicParams(1,2)=val; }
00188                         /** Set the value of the focal length x-value (in pixels). */
00189                         inline void fx(double val) { intrinsicParams(0,0)=val; }
00190                         /** Set the value of the focal length y-value (in pixels). */
00191                         inline void fy(double val) { intrinsicParams(1,1)=val; }
00192 
00193                         /** Get the value of the k1 distortion parameter.  */
00194                         inline double k1() const { return dist[0]; }
00195                         /** Get the value of the k2 distortion parameter.  */
00196                         inline double k2() const { return dist[1]; }
00197                         /** Get the value of the p1 distortion parameter.  */
00198                         inline double p1() const { return dist[2]; }
00199                         /** Get the value of the p2 distortion parameter.  */
00200                         inline double p2() const { return dist[3]; }
00201                         /** Get the value of the k3 distortion parameter.  */
00202                         inline double k3() const { return dist[4]; }
00203 
00204                         /** Get the value of the k1 distortion parameter.  */
00205                         inline void k1(double val) { dist[0]=val; }
00206                         /** Get the value of the k2 distortion parameter.  */
00207                         inline void k2(double val) { dist[1]=val; }
00208                         /** Get the value of the p1 distortion parameter.  */
00209                         inline void p1(double val) { dist[2]=val; }
00210                         /** Get the value of the p2 distortion parameter.  */
00211                         inline void p2(double val) { dist[3]=val; }
00212                         /** Get the value of the k3 distortion parameter.  */
00213                         inline void k3(double val) { dist[4]=val; }
00214                 }; // end class TCamera
00215 
00216                                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( TStereoCamera, mrpt::utils::CSerializable )
00217 
00218                 enum TStereoCameraModel
00219                 {
00220                     Bumblebee = 0,
00221                     Custom,
00222                     Uncalibrated
00223                 };
00224 
00225                 /** Structure to hold the parameters of a pinhole stereo camera model.
00226                   *  The parameters obtained for one camera resolution can be used for any other resolution by means of the method TCamera::scaleToResolution()
00227                   *
00228                   * \sa mrpt::vision::CCamModel, the application <a href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera
00229                  */
00230         class BASE_IMPEXP TStereoCamera : public mrpt::utils::CSerializable
00231                 {
00232             DEFINE_SERIALIZABLE( TStereoCamera )
00233 
00234         public:
00235 
00236                     TStereoCameraModel  model;
00237                     TCamera             leftCamera, rightCamera;
00238                     CPose3DQuat         rightCameraPose;
00239 
00240             // Default constructor:
00241             // Bumblebee with 640x480 images
00242                     TStereoCamera() : model( Bumblebee )
00243                     {
00244                         leftCamera.ncols = rightCamera.ncols = 640;
00245                         leftCamera.nrows = rightCamera.nrows = 480;
00246 
00247                 leftCamera.setIntrinsicParamsFromValues(
00248                     0.81945957*leftCamera.ncols, 1.09261276*leftCamera.nrows,
00249                     0.499950781*leftCamera.ncols, 0.506134245*leftCamera.nrows );
00250                 leftCamera.setDistortionParamsFromValues( -3.627383e-001, 2.099672e-001, 0, 0, -8.575903e-002 );
00251 
00252                 rightCamera.setIntrinsicParamsFromValues(
00253                             0.822166309*leftCamera.ncols, 1.096221745*leftCamera.nrows,
00254                             0.507065918*leftCamera.ncols, 0.524686589*leftCamera.nrows );
00255                 rightCamera.setDistortionParamsFromValues( -3.782850e-001, 2.539438e-001, 0, 0, -1.279638e-001 );
00256 
00257                 leftCamera.focalLengthMeters = rightCamera.focalLengthMeters = 0.0038;      // 3.8 mm
00258 
00259                 // Camera pose
00260                 CMatrixDouble44 A;
00261                 A.set_unsafe(0,0,9.999777e-001);    A.set_unsafe(0,1,-6.262494e-003);   A.set_unsafe(0,2,2.340592e-003);    A.set_unsafe(0,3,1.227338e-001);
00262                 A.set_unsafe(1,0,6.261120e-003);    A.set_unsafe(1,1,9.999802e-001);    A.set_unsafe(1,2,5.939072e-004);    A.set_unsafe(1,3,-3.671682e-004);
00263                 A.set_unsafe(2,0,-2.344265e-003);   A.set_unsafe(2,1,-5.792392e-004);   A.set_unsafe(2,2,9.999971e-001);    A.set_unsafe(2,3,-1.499571e-004);
00264                 A.set_unsafe(3,0,0);                A.set_unsafe(3,1,0);                A.set_unsafe(3,2,0);                A.set_unsafe(3,3,0);
00265                 rightCameraPose = CPose3DQuat( A );
00266                     }
00267 
00268                     /**  Save as a config block:
00269                           *  \code
00270                           *  [SECTION]
00271                           *  resolution = [NCOLS NROWS]
00272                           *  cx         = CX
00273                           *  cy         = CY
00274                           *  fx         = FX
00275                           *  fy         = FY
00276                           *  dist       = [K1 K2 T1 T2 K3]
00277                           *  focal_length = FOCAL_LENGTH
00278                           *  \endcode
00279                           */
00280                         void saveToConfigFile( const std::string &section, mrpt::utils::CConfigFileBase &cfg ) const;
00281 
00282                         /**  Load all the params from a config source, in the format used in saveToConfigFile(), that is:
00283                           *
00284                           *  \code
00285                           *  [SECTION]
00286                           *  resolution = [NCOLS NROWS]
00287                           *  cx         = CX
00288                           *  cy         = CY
00289                           *  fx         = FX
00290                           *  fy         = FY
00291                           *  dist       = [K1 K2 T1 T2 K3]
00292                           *  focal_length = FOCAL_LENGTH  [optional field]
00293                           *  \endcode
00294                           *  \exception std::exception on missing fields
00295                           */
00296                         void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg );
00297 
00298                 }; // end class TStereoCamera
00299 
00300         } // End of namespace
00301 } // end of namespace
00302 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011