29 const double Cx =
cam.
cx();
30 const double Cy =
cam.
cy();
31 const double k1 =
cam.
k1();
32 const double k2 =
cam.
k2();
35 const double dy = 1.0 /
cam.
fy();
37 double xd = (
p.x - Cx) * dx;
38 double yd = (
p.y - Cy) * dy;
39 double rd2 = xd * xd + yd * yd;
40 double rd4 = rd2 * rd2;
42 J_undist.setSize(2, 2);
44 (1 + k1 * rd2 + k2 * rd4) +
45 (
p.x - Cx) * (k1 + 2 * k2 * rd2) * (2 * (
p.x - Cx) * dx * dx);
47 (1 + k1 * rd2 + k2 * rd4) +
48 (
p.y - Cy) * (k1 + 2 * k2 * rd2) * (2 * (
p.y - Cy) * dy * dy);
50 (
p.x - Cx) * (k1 + 2 * k2 * rd2) * (2 * (
p.y - Cy) * dy * dy);
52 (
p.y - Cy) * (k1 + 2 * k2 * rd2) * (2 * (
p.x - Cx) * dx * dx);
60 const double dx =
p.x -
cam.
cx();
61 const double dy =
p.y -
cam.
cy();
63 const double inv_f_15 = 1.0 / pow(f, 1.5);
73 J_undistor(0, 0) = (1 - 2 *
cam.
k1() *
square(dy)) * inv_f_15;
74 J_undistor(0, 1) = J_undistor(1, 0) = (2 *
cam.
k1() * dx * dy) * inv_f_15;
75 J_undistor(1, 1) = (1 - 2 *
cam.
k1() *
square(dx)) * inv_f_15;
110 const double dx = (
p.x -
cam.
cx());
113 const double dy = (
p.y -
cam.
cy());
117 const double fact = 1.0 / sqrt(1 + 2 *
cam.
k1() * r2);
119 distorted_p.
x =
cam.
cx() + dx * fact;
120 distorted_p.
y =
cam.
cy() + dy * fact;
129 std::vector<TPixelCoordf> in_p(1), out_p;
136 undistorted_p = out_p[0];
163 const double dx = (p3D.
x / p3D.
z) *
cam.
fx();
164 const double dy = (p3D.
y / p3D.
z) *
cam.
fy();
170 1.0 / sqrt(1 + 2 *
cam.
k1() * r2);
172 distorted_p.
x =
cam.
cx() + dx * fact;
173 distorted_p.
y =
cam.
cy() + dy * fact;
187 const double dx = distorted_p.
x -
cam.
cx();
188 const double dy = distorted_p.
y -
cam.
cy();
190 double factor = 1.0 / sqrt(1 - 2 *
cam.
k1() * r2);
192 p3D.
x = dx * factor /
cam.
fx();
193 p3D.
y = dy * factor /
cam.
fy();
214 du_dy(0, 0) =
cam.
fx() / p3D.
z;
216 du_dy(1, 1) =
cam.
fy() / p3D.
z;
228 const double ux = (p3D.
x / p3D.
z) *
230 const double uy = (p3D.
y / p3D.
z) *
cam.
fy();
232 const double ux_sqr =
square(ux);
233 const double uy_sqr =
square(uy);
235 const double r2 = ux_sqr + uy_sqr;
237 const double f = 1 + 2 *
cam.
k1() * r2;
238 const double f1_2 = sqrt(f);
239 const double f3_2 = f1_2 * f;
244 dh_du *= -2 *
cam.
k1() / f3_2;
245 dh_du(0, 0) += (1 / f1_2);
246 dh_du(1, 1) += (1 / f1_2);
255 dh_dy.multiply(dh_du, du_dy);
270 dy_du(0, 0) = 1.0 /
cam.
fx();
271 dy_du(1, 1) = 1.0 /
cam.
fy();
275 const double dx =
p.x -
cam.
cx();
276 const double dy =
p.y -
cam.
cy();
279 double f = 1 - 2 *
cam.
k1() * radi2;
280 double f1_2 = sqrt(f);
281 double f3_2 = f1_2 * f;
292 du_dh *= 2 *
cam.
k1() / f3_2;
293 du_dh(0, 0) += (1 / f1_2);
294 du_dh(1, 1) += (1 / f1_2);
297 dy_dh.multiply(dy_du, du_dh);
307 double cx = 0.0f, cy = 0.0f, fx = 0.0f, fy = 0.0f;
323 ASSERT_(DD.size() == 4 || DD.size() == 5)
mrpt::utils::TCamera cam
The parameters of a camera.
A pair (x,y) of pixel coordinates (subpixel resolution).
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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 ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
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.
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...
This base provides a set of functions for maths stuff.
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.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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 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 setDistortionParamsVector(const mrpt::math::CMatrixDouble15 &distParVector)
Set the whole vector of distortion params of the camera.
Classes for computer vision, detectors, features, etc.
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...
double x
X,Y,Z coordinates.
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.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
std::vector< double > getDistortionParamsAsVector() const
Get a vector with the distortion params of the camera.
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
GLsizei GLsizei GLchar * source
void undistort_points(const std::vector< mrpt::utils::TPixelCoordf > &srcDistortedPixels, std::vector< mrpt::utils::TPixelCoordf > &dstUndistortedPixels, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams)
Undistort a list of points given by their pixel coordinates, provided the camera matrix and distortio...
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 ...
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
double k2() const
Get the value of the k2 distortion parameter.
CCamModel()
Default Constructor.
void setIntrinsicParamsFromValues(double fx, double fy, double cx, double cy)
Set the matrix of intrinsic params of the camera from the individual values of focal length and princ...