Go to the documentation of this file.
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);
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);
285 #endif //__CCamModel_H
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...
double p1() const
Get the value of the p1 distortion parameter.
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 getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) const
double k2() const
Get the value of the k2 distortion parameter.
void getFullInverseModelWithJacobian(const POINTIN &pIn, POINTOUT &pOut, MAT22 &jOut) const
mrpt::math::CMatrixFixedNumeric< double, 4, 2 > secondInverseJacobian() const
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.
double k1() const
Get the value of the k1 distortion parameter.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double cy() const
Get the value of the principal point y-coordinate (in pixels).
void jacob_undistor_fm(const mrpt::utils::TPixelCoordf &uvd, math::CMatrixDouble &J_undist)
Jacobian for undistortion the image coordinates.
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 ...
A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
void getFullJacobian(const POINT &pIn, MATRIX &mOut) const
mrpt::utils::TCamera cam
The parameters of a camera.
double fx() const
Get the value of the focal length x-value (in pixels).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
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...
void jacob_undistor(const mrpt::utils::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist)
Calculate the image coordinates undistorted.
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.
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 ,...
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
A numeric matrix of compile-time fixed size.
double p2() const
Get the value of the p2 distortion parameter.
double fy() const
Get the value of the focal length y-value (in pixels).
Structure to hold the parameters of a pinhole camera model.
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian() 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.
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...
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
void getFullProjection(const POINT &pIn, PIXEL &pOut) const
A pair (x,y) of pixel coordinates (subpixel resolution).
CCamModel(const mrpt::utils::CConfigFileBase &cfgIni)
Constructor from a ini file.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Page generated by Doxygen 1.8.20 for MRPT 1.4.0 SVN: at Thu Aug 27 02:40:23 UTC 2020 | | |