42 void loadFromConfigFile(
44 const std::string §ion);
143 v.
K=1+v.
R*(cam.
k1()+v.
R*(cam.
k2()+v.
R*cam.
k3()));
144 T xy=v.
x_*v.
y_,p1=cam.
p1(),p2=cam.
p2();
149 template<
typename T,
typename POINT,
typename PIXEL>
inline void getFullProjection(
const POINT &pIn,PIXEL &pOut)
const {
151 getTemporaryVariablesForTransform(pIn,tmp);
152 getFullProjectionT(tmp,pOut);
156 pOut[0]=cam.
fx()*tmp.
x__+cam.
cx();
157 pOut[1]=cam.
fy()*tmp.
y__+cam.
cy();
160 template<
typename T,
typename POINT,
typename MATRIX>
inline void getFullJacobian(
const POINT &pIn,MATRIX &mOut)
const {
162 getTemporaryVariablesForTransform(pIn,tmp);
163 getFullJacobianT(pIn,tmp,mOut);
171 T tmpK=2*(cam.
k1()+tmp.
R*(2*cam.
k2()+3*tmp.
R*cam.
k3()));
176 J21.set_unsafe(0,0,yx2);
177 J21.set_unsafe(0,1,x_);
178 J21.set_unsafe(0,2,0);
179 J21.set_unsafe(1,0,zx2);
180 J21.set_unsafe(1,1,0);
181 J21.set_unsafe(1,2,x_);
182 J21.set_unsafe(2,0,tmpKx*yx2+tmpKy*zx2);
183 J21.set_unsafe(2,1,tmpKx*x_);
184 J21.set_unsafe(2,2,tmpKy*x_);
186 T pxpy=2*(cam.
p1()*tmp.
x_+cam.
p2()*tmp.
y_);
187 T p1y=cam.
p1()*tmp.
y_;
188 T p2x=cam.
p2()*tmp.
x_;
190 T fx=cam.
fx(),fy=cam.
fy();
191 J43.set_unsafe(0,0,fx*(tmp.
K+2*p1y+6*p2x));
192 J43.set_unsafe(0,1,fx*pxpy);
193 J43.set_unsafe(0,2,fx*tmp.
x_);
194 J43.set_unsafe(1,0,fy*pxpy);
195 J43.set_unsafe(1,1,fy*(tmp.
K+6*p1y+2*p2x));
196 J43.set_unsafe(1,2,fy*tmp.
y_);
197 mOut.multiply(J43,J21);
207 res.set_unsafe(0,1,0);
208 res.set_unsafe(1,0,0);
213 res.set_unsafe(0,0,1);
214 res.set_unsafe(0,1,0);
215 res.set_unsafe(1,0,0);
216 res.set_unsafe(1,1,1);
221 res.set_unsafe(0,1,0);
222 res.set_unsafe(0,2,0);
223 res.set_unsafe(1,0,0);
224 res.set_unsafe(1,2,0);
225 res.set_unsafe(2,0,0);
226 res.set_unsafe(2,1,0);
227 res.set_unsafe(2,2,1);
228 res.set_unsafe(2,3,0);
245 double cx=cam.
cx(),cy=cam.
cy(),ifx=1/cam.
fx(),ify=1/cam.
fy();
246 double K1=cam.
k1(),K2=cam.
k2(),p1=cam.
p1(),p2=cam.
p2(),K3=cam.
k3();
248 tmp1[0]=(pIn[0]-cx)*ifx;
249 tmp1[1]=(pIn[1]-cy)*ify;
250 J1.set_unsafe(0,0,ifx);
251 J1.set_unsafe(1,1,ify);
256 double K123=-K1*sK1+2*K1*K2-K3;
258 tmp1[3]=1+tmp1[2]*(-K1+tmp1[2]*(K12+tmp1[2]*K123));
259 J2.set_unsafe(2,0,2*tmp1[0]);
260 J2.set_unsafe(2,1,2*tmp1[1]);
261 double jTemp=-2*K1+4*tmp1[2]*K12+6*
square(tmp1[2])*K123;
262 J2.set_unsafe(3,0,tmp1[0]*jTemp);
263 J2.set_unsafe(3,1,tmp1[1]*jTemp);
265 tmp2[0]=tmp1[0]*tmp1[3];
266 tmp2[1]=tmp1[1]*tmp1[3];
267 J3.set_unsafe(0,0,tmp1[3]);
268 J3.set_unsafe(0,3,tmp1[0]);
269 J3.set_unsafe(1,1,tmp1[3]);
270 J3.set_unsafe(1,3,tmp1[1]);
272 double prod=tmp2[0]*tmp2[1];
274 pOut[0]=tmp2[0]-p1*prod-p2*(tmp1[2]+2*
square(tmp2[0]));
275 pOut[1]=tmp2[1]-p1*(tmp1[2]+2*
square(tmp2[1]))-p2*prod;
276 J4.set_unsafe(0,0,1-p1*tmp2[1]-4*p2*tmp2[0]);
277 J4.set_unsafe(0,1,-p1*tmp2[0]);
278 J4.set_unsafe(0,2,-p2);
279 J4.set_unsafe(1,0,-p2*tmp2[1]);
280 J4.set_unsafe(1,1,1-4*p1*tmp2[1]-p2*tmp2[0]);
281 J4.set_unsafe(1,2,-p1);
283 jOut.multiply_ABC(J4,J3,J2);
284 jOut.multiply(jOut,J1);
291 #endif //__CCamModel_H mrpt::utils::TCamera cam
The parameters of a camera.
A pair (x,y) of pixel coordinates (subpixel resolution).
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const
void getFullProjection(const POINT &pIn, PIXEL &pOut) const
T square(const T x)
Inline function for the square of a number.
double p1() const
Get the value of the p1 distortion parameter.
double cy() const
Get the value of the principal point y-coordinate (in pixels).
void getFullInverseModelWithJacobian(const POINTIN &pIn, POINTOUT &pOut, MAT22 &jOut) const
This class allows loading and storing values and vectors of different types from a configuration text...
double fx() const
Get the value of the focal length x-value (in pixels).
double fy() const
Get the value of the focal length y-value (in pixels).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A numeric matrix of compile-time fixed size.
double k1() const
Get the value of the k1 distortion parameter.
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
double p2() const
Get the value of the p2 distortion parameter.
void getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) const
double cx() const
Get the value of the principal point x-coordinate (in pixels).
void getFullJacobian(const POINT &pIn, MATRIX &mOut) const
T square(const T x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...
double k3() const
Get the value of the k3 distortion parameter.
A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
double k2() const
Get the value of the k2 distortion parameter.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void VISION_IMPEXP undistort_point(const mrpt::utils::TPixelCoordf &inPt, mrpt::utils::TPixelCoordf &outPt, const mrpt::utils::TCamera &cameraModel)
Undistort one point given by its pixel coordinates and the camera parameters.
Structure to hold the parameters of a pinhole camera model.
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian() const
mrpt::math::CMatrixFixedNumeric< double, 4, 2 > secondInverseJacobian() const