TCamera.h Source File

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