00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00049
00050
00051
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
00070
00071
00072 uint32_t ncols,nrows;
00073 CMatrixDouble33 intrinsicParams;
00074 CArrayDouble<5> dist;
00075 double focalLengthMeters;
00076
00077
00078
00079
00080
00081 void scaleToResolution(uint32_t new_ncols, uint32_t new_nrows);
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095 void saveToConfigFile( const std::string §ion, mrpt::utils::CConfigFileBase &cfg ) const;
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 void loadFromConfigFile(const std::string §ion, const mrpt::utils::CConfigFileBase &cfg );
00112
00113
00114 std::string dumpAsText()
00115 {
00116 mrpt::utils::CConfigFileMemory cfg;
00117 saveToConfigFile("",cfg);
00118 return cfg.getContent();
00119 }
00120
00121
00122
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
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
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
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
00155 template <class VECTORLIKE>
00156 void setDistortionParamsVector( const VECTORLIKE &distParVector )
00157 {
00158 ASSERT_(distParVector.size()==4 || distParVector.size()==5)
00159 dist[4] = 0;
00160 for (typename VECTORLIKE::Index i=0;i<distParVector.size();i++)
00161 dist[i] = distParVector[i];
00162 }
00163
00164
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
00176 inline double cx() const { return intrinsicParams(0,2); }
00177
00178 inline double cy() const { return intrinsicParams(1,2); }
00179
00180 inline double fx() const { return intrinsicParams(0,0); }
00181
00182 inline double fy() const { return intrinsicParams(1,1); }
00183
00184
00185 inline void cx(double val) { intrinsicParams(0,2)=val; }
00186
00187 inline void cy(double val) { intrinsicParams(1,2)=val; }
00188
00189 inline void fx(double val) { intrinsicParams(0,0)=val; }
00190
00191 inline void fy(double val) { intrinsicParams(1,1)=val; }
00192
00193
00194 inline double k1() const { return dist[0]; }
00195
00196 inline double k2() const { return dist[1]; }
00197
00198 inline double p1() const { return dist[2]; }
00199
00200 inline double p2() const { return dist[3]; }
00201
00202 inline double k3() const { return dist[4]; }
00203
00204
00205 inline void k1(double val) { dist[0]=val; }
00206
00207 inline void k2(double val) { dist[1]=val; }
00208
00209 inline void p1(double val) { dist[2]=val; }
00210
00211 inline void p2(double val) { dist[3]=val; }
00212
00213 inline void k3(double val) { dist[4]=val; }
00214 };
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
00226
00227
00228
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
00241
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;
00258
00259
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
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280 void saveToConfigFile( const std::string §ion, mrpt::utils::CConfigFileBase &cfg ) const;
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296 void loadFromConfigFile(const std::string §ion, const mrpt::utils::CConfigFileBase &cfg );
00297
00298 };
00299
00300 }
00301 }
00302 #endif