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, 4, 2 > secondInverseJacobian() const
T square(const T x)
Inline function for the square of a number.
double p2() const
Get the value of the p2 distortion parameter.
double k1() const
Get the value of the k1 distortion parameter.
double k2() const
Get the value of the k2 distortion parameter.
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian() const
This class allows loading and storing values and vectors of different types from a configuration text...
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
double fy() const
Get the value of the focal length y-value (in pixels).
A numeric matrix of compile-time fixed size.
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
double k3() const
Get the value of the k3 distortion parameter.
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...
A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...
void getFullJacobian(const POINT &pIn, MATRIX &mOut) const
double fx() const
Get the value of the focal length x-value (in pixels).
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const
double p1() const
Get the value of the p1 distortion parameter.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
void getFullInverseModelWithJacobian(const POINTIN &pIn, POINTOUT &pOut, MAT22 &jOut) const
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) const
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.
void getFullProjection(const POINT &pIn, PIXEL &pOut) const
double cy() const
Get the value of the principal point y-coordinate (in pixels).