Main MRPT website > C++ reference for MRPT 1.4.0
CCamModel.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef CCamModel_H
10#define CCamModel_H
11
12#include <mrpt/utils/TCamera.h>
13#include <mrpt/system/os.h>
14#include <mrpt/vision/utils.h>
16
17namespace mrpt
18{
19 namespace vision
20 {
21 /** This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobians
22 *
23 * The camera parameters are accessible in the public member CCamModel::cam
24 *
25 * - Versions:
26 * - First version: By Antonio J. Ortiz de Galistea.
27 * - 2009-2010: Rewritten by various authors.
28 *
29 * \sa mrpt::utils::TCamera, CMonoSlam, the application <a href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera
30 * \ingroup mrpt_vision_grp
31 */
33 {
34 public:
35 mrpt::utils::TCamera cam; //!< The parameters of a camera
36
37 /** Default Constructor */
39
40 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
41 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
42
43 /** Constructor from a ini file
44 */
46
47 /** Jacobian for undistortion the image coordinates */
49
50 /** Calculate the image coordinates undistorted
51 */
53
54 /** Return the pixel position distorted by the camera
55 */
57
58 /** Return the pixel position undistorted by the camera
59 * The input values 'col' and 'row' will be replace for the new values (undistorted)
60 */
62
63 /** Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)
64 * \sa unproject_3D_point
65 */
67
68 /** Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position
69 * \sa project_3D_point
70 * \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.
71 */
73
74 /** 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)
75
76 We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin at the top-left corner),
77 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).
78
79 Then this method computes the 2x3 Jacobian:
80
81 \f[
82 \frac{\partial h}{\partial y} = \frac{\partial h}{\partial u} \frac{\partial u}{\partial y}
83 \f]
84
85 With:
86
87 \f[
88 \frac{\partial u}{\partial y} =
89 \left( \begin{array}{ccc}
90 \frac{f_x}{y_z} & 0 & - y \frac{f_x}{y_z^2} \\
91 0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\
92 \end{array} \right)
93 \f]
94
95 where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and y, respectively.
96 And, if we define:
97
98 \f[
99 f = 1+ 2 k_1 (u_x^2+u_y^2)
100 \f]
101
102 then:
103
104 \f[
105 \frac{\partial h}{\partial u} =
106 \left( \begin{array}{cc}
107 \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
108 -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
109 \end{array} \right)
110 \f]
111
112 \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian
113 \sa project_3D_point
114 */
116
117
118 /** 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
119 \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::UnprojectionJacobian
120 \sa unproject_3D_point
121 */
123
124 template<typename T> struct CameraTempVariables {
125 T x_,y_;
126 T x_2,y_2;
127 T R;
128 T K;
129 T x__,y__;
130 };
131 template<typename T,typename POINT> void getTemporaryVariablesForTransform(const POINT &p,CameraTempVariables<T> &v) const {
132 v.x_=p[1]/p[0];
133 v.y_=p[2]/p[0];
134 v.x_2=square(v.x_);
135 v.y_2=square(v.y_);
136 v.R=v.x_2+v.y_2;
137 v.K=1+v.R*(cam.k1()+v.R*(cam.k2()+v.R*cam.k3()));
138 T xy=v.x_*v.y_,p1=cam.p1(),p2=cam.p2();
139 v.x__=v.x_*v.K+2*p1*xy+p2*(3*v.x_2+v.y_2);
140 v.y__=v.y_*v.K+p1*(v.x_2+3*v.y_2)+2*p2*xy;
141 }
142
143 template<typename T,typename POINT,typename PIXEL> inline void getFullProjection(const POINT &pIn,PIXEL &pOut) const {
145 getTemporaryVariablesForTransform(pIn,tmp);
146 getFullProjectionT(tmp,pOut);
147 }
148
149 template<typename T,typename PIXEL> inline void getFullProjectionT(const CameraTempVariables<T> &tmp,PIXEL &pOut) const {
150 pOut[0]=cam.fx()*tmp.x__+cam.cx();
151 pOut[1]=cam.fy()*tmp.y__+cam.cy();
152 }
153
154 template<typename T,typename POINT,typename MATRIX> inline void getFullJacobian(const POINT &pIn,MATRIX &mOut) const {
156 getTemporaryVariablesForTransform(pIn,tmp);
157 getFullJacobianT(pIn,tmp,mOut);
158 }
159
160 template<typename T,typename POINT,typename MATRIX> void getFullJacobianT(const POINT &pIn,const CameraTempVariables<T> &tmp,MATRIX &mOut) const {
161 T x_=1/pIn[0];
162 T x_2=square(x_);
163 //First two jacobians...
165 T tmpK=2*(cam.k1()+tmp.R*(2*cam.k2()+3*tmp.R*cam.k3()));
166 T tmpKx=tmpK*tmp.x_;
167 T tmpKy=tmpK*tmp.y_;
168 T yx2=-pIn[1]*x_2;
169 T zx2=-pIn[2]*x_2;
170 J21.set_unsafe(0,0,yx2);
171 J21.set_unsafe(0,1,x_);
172 J21.set_unsafe(0,2,0);
173 J21.set_unsafe(1,0,zx2);
174 J21.set_unsafe(1,1,0);
175 J21.set_unsafe(1,2,x_);
176 J21.set_unsafe(2,0,tmpKx*yx2+tmpKy*zx2);
177 J21.set_unsafe(2,1,tmpKx*x_);
178 J21.set_unsafe(2,2,tmpKy*x_);
179 //Last two jacobians...
180 T pxpy=2*(cam.p1()*tmp.x_+cam.p2()*tmp.y_);
181 T p1y=cam.p1()*tmp.y_;
182 T p2x=cam.p2()*tmp.x_;
184 T fx=cam.fx(),fy=cam.fy();
185 J43.set_unsafe(0,0,fx*(tmp.K+2*p1y+6*p2x));
186 J43.set_unsafe(0,1,fx*pxpy);
187 J43.set_unsafe(0,2,fx*tmp.x_);
188 J43.set_unsafe(1,0,fy*pxpy);
189 J43.set_unsafe(1,1,fy*(tmp.K+6*p1y+2*p2x));
190 J43.set_unsafe(1,2,fy*tmp.y_);
191 mOut.multiply(J43,J21);
192 //cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;
193 }
194 private:
195 //These functions are little tricks to avoid multiple initialization.
196 //They are intended to initialize the common parts of the jacobians just once,
197 //and not in each iteration.
198 //They are mostly useless outside the scope of this function.
201 res.set_unsafe(0,1,0);
202 res.set_unsafe(1,0,0);
203 return res;
204 }
207 res.set_unsafe(0,0,1);
208 res.set_unsafe(0,1,0);
209 res.set_unsafe(1,0,0);
210 res.set_unsafe(1,1,1);
211 return res;
212 }
215 res.set_unsafe(0,1,0);
216 res.set_unsafe(0,2,0);
217 res.set_unsafe(1,0,0);
218 res.set_unsafe(1,2,0);
219 res.set_unsafe(2,0,0);
220 res.set_unsafe(2,1,0);
221 res.set_unsafe(2,2,1);
222 res.set_unsafe(2,3,0);
223 return res;
224 }
225 public:
226 template<typename POINTIN,typename POINTOUT,typename MAT22> void getFullInverseModelWithJacobian(const POINTIN &pIn,POINTOUT &pOut,MAT22 &jOut) const {
227 //Temporary variables (well, there are some more, but these are the basics)
228 //WARNING!: this shortcut to avoid repeated initialization makes the method somewhat
229 //faster, but makes it incapable of being used in more than one thread
230 //simultaneously!
232 static mrpt::math::CMatrixFixedNumeric<double,2,2> J1(firstInverseJacobian());
233 static mrpt::math::CMatrixFixedNumeric<double,4,2> J2(secondInverseJacobian());
234 static mrpt::math::CMatrixFixedNumeric<double,3,4> J3(thirdInverseJacobian());
235 static mrpt::math::CMatrixFixedNumeric<double,2,3> J4; //This is not initialized in a special way, although declaring it
237 mrpt::math::CArray<double,2> tmp2; //This would be a CArray<double,3>, but to avoid copying, we let "R2" lie in tmp1.
238 //Camera Parameters
239 double cx=cam.cx(),cy=cam.cy(),ifx=1/cam.fx(),ify=1/cam.fy();
240 double K1=cam.k1(),K2=cam.k2(),p1=cam.p1(),p2=cam.p2(),K3=cam.k3();
241 //First step: intrinsic matrix.
242 tmp1[0]=(pIn[0]-cx)*ifx;
243 tmp1[1]=(pIn[1]-cy)*ify;
244 J1.set_unsafe(0,0,ifx);
245 J1.set_unsafe(1,1,ify);
246 //Second step: adding temporary variables, related to the distortion.
247 tmp1[2]=square(tmp1[0])+square(tmp1[1]);
248 double sK1=square(K1);
249 double K12=sK1-K2;
250 double K123=-K1*sK1+2*K1*K2-K3; //-K1^3+2K1K2-K3
251 //tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);
252 tmp1[3]=1+tmp1[2]*(-K1+tmp1[2]*(K12+tmp1[2]*K123));
253 J2.set_unsafe(2,0,2*tmp1[0]);
254 J2.set_unsafe(2,1,2*tmp1[1]);
255 double jTemp=-2*K1+4*tmp1[2]*K12+6*square(tmp1[2])*K123;
256 J2.set_unsafe(3,0,tmp1[0]*jTemp);
257 J2.set_unsafe(3,1,tmp1[1]*jTemp);
258 //Third step: radial distortion. Really simple, since most work has been done in the previous step.
259 tmp2[0]=tmp1[0]*tmp1[3];
260 tmp2[1]=tmp1[1]*tmp1[3];
261 J3.set_unsafe(0,0,tmp1[3]);
262 J3.set_unsafe(0,3,tmp1[0]);
263 J3.set_unsafe(1,1,tmp1[3]);
264 J3.set_unsafe(1,3,tmp1[1]);
265 //Fourth step: tangential distorion. A little more complicated, but not much more.
266 double prod=tmp2[0]*tmp2[1];
267 //References to tmp1[2] are not errors! That element is "R2".
268 pOut[0]=tmp2[0]-p1*prod-p2*(tmp1[2]+2*square(tmp2[0]));
269 pOut[1]=tmp2[1]-p1*(tmp1[2]+2*square(tmp2[1]))-p2*prod;
270 J4.set_unsafe(0,0,1-p1*tmp2[1]-4*p2*tmp2[0]);
271 J4.set_unsafe(0,1,-p1*tmp2[0]);
272 J4.set_unsafe(0,2,-p2);
273 J4.set_unsafe(1,0,-p2*tmp2[1]);
274 J4.set_unsafe(1,1,1-4*p1*tmp2[1]-p2*tmp2[0]);
275 J4.set_unsafe(1,2,-p1);
276 //As fast as possible, and without more temporaries, let the jacobian be J4*J3*J2*J1;
277 jOut.multiply_ABC(J4,J3,J2); //Note that using the other order is not possible due to matrix sizes (jOut may, and most probably will, be fixed).
278 jOut.multiply(jOut,J1);
279 }
280
281 }; // end class
282
283 } // end namespace
284} // end namespace
285#endif //__CCamModel_H
A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...
Definition CArray.h:55
A numeric matrix of compile-time fixed size.
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
Structure to hold the parameters of a pinhole camera model.
Definition TCamera.h:32
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition TCamera.h:154
double k3() const
Get the value of the k3 distortion parameter.
Definition TCamera.h:180
double fx() const
Get the value of the focal length x-value (in pixels).
Definition TCamera.h:158
double p2() const
Get the value of the p2 distortion parameter.
Definition TCamera.h:178
double p1() const
Get the value of the p1 distortion parameter.
Definition TCamera.h:176
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition TCamera.h:156
double fy() const
Get the value of the focal length y-value (in pixels).
Definition TCamera.h:160
double k1() const
Get the value of the k1 distortion parameter.
Definition TCamera.h:172
double k2() const
Get the value of the k2 distortion parameter.
Definition TCamera.h:174
This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...
Definition CCamModel.h:33
void getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) const
Definition CCamModel.h:149
void jacob_undistor_fm(const mrpt::utils::TPixelCoordf &uvd, math::CMatrixDouble &J_undist)
Jacobian for undistortion the image coordinates.
void getFullJacobian(const POINT &pIn, MATRIX &mOut) const
Definition CCamModel.h:154
void project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::utils::TPixelCoordf &distorted_p) const
Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z p...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::math::CMatrixFixedNumeric< double, 4, 2 > secondInverseJacobian() const
Definition CCamModel.h:205
CCamModel(const mrpt::utils::CConfigFileBase &cfgIni)
Constructor from a ini file.
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
Definition CCamModel.h:131
void jacob_undistor(const mrpt::utils::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist)
Calculate the image coordinates undistorted.
void getFullInverseModelWithJacobian(const POINTIN &pIn, POINTOUT &pOut, MAT22 &jOut) const
Definition CCamModel.h:226
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const
Definition CCamModel.h:199
mrpt::utils::TCamera cam
The parameters of a camera.
Definition CCamModel.h:35
void undistort_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &undistorted_p)
Return the pixel position undistorted by the camera The input values 'col' and 'row' will be replace ...
void jacobian_unproject_with_distortion(const mrpt::utils::TPixelCoordf &p, math::CMatrixDouble &dy_dh) const
Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_...
void jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble &dh_dy) const
Jacobian of the projection of 3D points (with distortion), as done in project_3D_point ,...
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian() const
Definition CCamModel.h:213
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
Definition CCamModel.h:160
void unproject_3D_point(const mrpt::utils::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const
Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
void distort_a_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &distorted_p)
Return the pixel position distorted by the camera.
CCamModel()
Default Constructor.
void getFullProjection(const POINT &pIn, PIXEL &pOut) const
Definition CCamModel.h:143
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
T square(const T x)
Inline function for the square of a number.
Definition bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 3D point.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition TPixelCoord.h:22



Page generated by Doxygen 1.9.8 for MRPT 1.4.0 SVN: at Thu Dec 14 16:54:58 UTC 2023