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 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
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 class VISION_IMPEXP CCamModel : public mrpt::utils::CLoadableOptions
00051 {
00052 public:
00053 mrpt::utils::TCamera cam;
00054
00055
00056 CCamModel();
00057
00058
00059
00060 void loadFromConfigFile(
00061 const mrpt::utils::CConfigFileBase &source,
00062 const std::string §ion);
00063
00064
00065 void dumpToTextStream( CStream &out) const;
00066
00067
00068
00069 CCamModel(const mrpt::utils::CConfigFileBase &cfgIni);
00070
00071
00072 void jacob_undistor_fm(const mrpt::vision::TPixelCoordf &uvd, math::CMatrixDouble &J_undist);
00073
00074
00075
00076 void jacob_undistor(const mrpt::vision::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist );
00077
00078
00079
00080 void distort_a_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &distorted_p);
00081
00082
00083
00084
00085 void undistort_point(const mrpt::vision::TPixelCoordf &p, mrpt::vision::TPixelCoordf &undistorted_p);
00086
00087
00088
00089
00090 void project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::vision::TPixelCoordf &distorted_p) const;
00091
00092
00093
00094
00095
00096 void unproject_3D_point(const mrpt::vision::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const;
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139 void jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble & dh_dy ) const;
00140
00141
00142
00143
00144
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
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
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
00217 }
00218 private:
00219
00220
00221
00222
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
00252
00253
00254
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;
00259 CArray<double,4> tmp1;
00260 CArray<double,2> tmp2;
00261
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
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
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;
00274
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
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
00289 double prod=tmp2[0]*tmp2[1];
00290
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
00300 jOut.multiply_ABC(J4,J3,J2);
00301 jOut.multiply(jOut,J1);
00302 }
00303
00304 };
00305
00306 }
00307 }
00308 #endif //__CCamModel_H