159 template <
typename T>
168 template <
typename T,
typename POINT>
179 v.x__ =
v.x_ *
v.K + 2 * p1 * xy + p2 * (3 *
v.x_2 +
v.y_2);
180 v.y__ =
v.y_ *
v.K + p1 * (
v.x_2 + 3 *
v.y_2) + 2 * p2 * xy;
183 template <
typename T,
typename POINT,
typename PIXEL>
191 template <
typename T,
typename PIXEL>
199 template <
typename T,
typename POINT,
typename MATRIX>
207 template <
typename T,
typename POINT,
typename MATRIX>
216 T tmpKx = tmpK * tmp.
x_;
217 T tmpKy = tmpK * tmp.
y_;
218 T yx2 = -pIn[1] * x_2;
219 T zx2 = -pIn[2] * x_2;
220 J21.set_unsafe(0, 0, yx2);
221 J21.set_unsafe(0, 1, x_);
222 J21.set_unsafe(0, 2, 0);
223 J21.set_unsafe(1, 0, zx2);
224 J21.set_unsafe(1, 1, 0);
225 J21.set_unsafe(1, 2, x_);
226 J21.set_unsafe(2, 0, tmpKx * yx2 + tmpKy * zx2);
227 J21.set_unsafe(2, 1, tmpKx * x_);
228 J21.set_unsafe(2, 2, tmpKy * x_);
235 J43.set_unsafe(0, 0, fx * (tmp.
K + 2 * p1y + 6 * p2x));
236 J43.set_unsafe(0, 1, fx * pxpy);
237 J43.set_unsafe(0, 2, fx * tmp.
x_);
238 J43.set_unsafe(1, 0, fy * pxpy);
239 J43.set_unsafe(1, 1, fy * (tmp.
K + 6 * p1y + 2 * p2x));
240 J43.set_unsafe(1, 2, fy * tmp.
y_);
241 mOut.multiply(J43, J21);
254 res.set_unsafe(0, 1, 0);
255 res.set_unsafe(1, 0, 0);
261 res.set_unsafe(0, 0, 1);
262 res.set_unsafe(0, 1, 0);
263 res.set_unsafe(1, 0, 0);
264 res.set_unsafe(1, 1, 1);
270 res.set_unsafe(0, 1, 0);
271 res.set_unsafe(0, 2, 0);
272 res.set_unsafe(1, 0, 0);
273 res.set_unsafe(1, 2, 0);
274 res.set_unsafe(2, 0, 0);
275 res.set_unsafe(2, 1, 0);
276 res.set_unsafe(2, 2, 1);
277 res.set_unsafe(2, 3, 0);
282 template <
typename POINTIN,
typename POINTOUT,
typename MAT22>
284 const POINTIN& pIn, POINTOUT& pOut, MAT22& jOut)
const 318 tmp1[0] = (pIn[0] - cx) * ifx;
319 tmp1[1] = (pIn[1] - cy) * ify;
320 J1.set_unsafe(0, 0, ifx);
321 J1.set_unsafe(1, 1, ify);
325 double K12 = sK1 - K2;
326 double K123 = -K1 * sK1 + 2 * K1 * K2 - K3;
328 tmp1[3] = 1 + tmp1[2] * (-K1 + tmp1[2] * (K12 + tmp1[2] * K123));
329 J2.set_unsafe(2, 0, 2 * tmp1[0]);
330 J2.set_unsafe(2, 1, 2 * tmp1[1]);
331 double jTemp = -2 * K1 + 4 * tmp1[2] * K12 + 6 *
square(tmp1[2]) * K123;
332 J2.set_unsafe(3, 0, tmp1[0] * jTemp);
333 J2.set_unsafe(3, 1, tmp1[1] * jTemp);
336 tmp2[0] = tmp1[0] * tmp1[3];
337 tmp2[1] = tmp1[1] * tmp1[3];
338 J3.set_unsafe(0, 0, tmp1[3]);
339 J3.set_unsafe(0, 3, tmp1[0]);
340 J3.set_unsafe(1, 1, tmp1[3]);
341 J3.set_unsafe(1, 3, tmp1[1]);
344 double prod = tmp2[0] * tmp2[1];
346 pOut[0] = tmp2[0] - p1 * prod - p2 * (tmp1[2] + 2 *
square(tmp2[0]));
347 pOut[1] = tmp2[1] - p1 * (tmp1[2] + 2 *
square(tmp2[1])) - p2 * prod;
348 J4.set_unsafe(0, 0, 1 - p1 * tmp2[1] - 4 * p2 * tmp2[0]);
349 J4.set_unsafe(0, 1, -p1 * tmp2[0]);
350 J4.set_unsafe(0, 2, -p2);
351 J4.set_unsafe(1, 0, -p2 * tmp2[1]);
352 J4.set_unsafe(1, 1, 1 - 4 * p1 * tmp2[1] - p2 * tmp2[0]);
353 J4.set_unsafe(1, 2, -p1);
356 jOut.multiply_ABC(J4, J3, J2);
360 jOut.multiply(jOut, J1);
367 #endif //__CCamModel_H mrpt::utils::TCamera cam
The parameters of a camera.
A pair (x,y) of pixel coordinates (subpixel resolution).
void getFullProjection(const POINT &pIn, PIXEL &pOut) const
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 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 ...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
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...
T square(const T x)
Inline function for the square of a number.
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 jacob_undistor_fm(const mrpt::utils::TPixelCoordf &uvd, math::CMatrixDouble &J_undist)
Jacobian for undistortion the image coordinates.
void getTemporaryVariablesForTransform(const POINT &p, CameraTempVariables< T > &v) const
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_...
double p2() const
Get the value of the p2 distortion parameter.
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 getFullProjectionT(const CameraTempVariables< T > &tmp, PIXEL &pOut) 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 jacob_undistor(const mrpt::utils::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undist)
Calculate the image coordinates undistorted.
GLsizei const GLchar ** string
double cx() const
Get the value of the principal point x-coordinate (in pixels).
void distort_a_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &distorted_p)
Return the pixel position distorted by the camera.
void getFullJacobian(const POINT &pIn, MATRIX &mOut) const
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.
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method displays clearly all the contents of the structure in textual form, sending it to a CStre...
mrpt::math::CMatrixFixedNumeric< double, 4, 2 > secondInverseJacobian() const
T square(const T x)
Inline function for the square of a number.
GLsizei GLsizei GLchar * source
void getFullJacobianT(const POINT &pIn, const CameraTempVariables< T > &tmp, MATRIX &mOut) const
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 ...
mrpt::math::CMatrixFixedNumeric< double, 3, 4 > thirdInverseJacobian() const
double k2() const
Get the value of the k2 distortion parameter.
CCamModel()
Default Constructor.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Structure to hold the parameters of a pinhole camera model.
mrpt::math::CMatrixFixedNumeric< double, 2, 2 > firstInverseJacobian() const