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();
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);
150 pOut[0]=cam.
fx()*tmp.
x__+cam.
cx();
151 pOut[1]=cam.
fy()*tmp.
y__+cam.
cy();
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);
165 T tmpK=2*(cam.
k1()+tmp.
R*(2*cam.
k2()+3*tmp.
R*cam.
k3()));
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_);
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);
201 res.set_unsafe(0,1,0);
202 res.set_unsafe(1,0,0);
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);
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);
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();
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);
247 tmp1[2]=square(tmp1[0])+square(tmp1[1]);
248 double sK1=square(K1);
250 double K123=-K1*sK1+2*K1*K2-K3;
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);
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]);
266 double prod=tmp2[0]*tmp2[1];
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);
277 jOut.multiply_ABC(J4,J3,J2);
278 jOut.multiply(jOut,J1);
A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...
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...
Structure to hold the parameters of a pinhole camera model.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
double k3() const
Get the value of the k3 distortion parameter.
double fx() const
Get the value of the focal length x-value (in pixels).
double p2() const
Get the value of the p2 distortion parameter.
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).
double fy() const
Get the value of the focal length y-value (in pixels).
double k1() const
Get the value of the k1 distortion parameter.
double k2() const
Get the value of the k2 distortion parameter.
This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...
void getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) const
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
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 §ion) 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
CCamModel(const mrpt::utils::CConfigFileBase &cfgIni)
Constructor from a ini file.
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
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
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const
mrpt::utils::TCamera cam
The parameters of a camera.
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
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
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
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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.
A pair (x,y) of pixel coordinates (subpixel resolution).