34 #ifndef _vpPixelMeterConversion_h_
35 #define _vpPixelMeterConversion_h_
42 #include <visp3/core/vpCameraParameters.h>
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpException.h>
45 #include <visp3/core/vpImagePoint.h>
46 #include <visp3/core/vpMath.h>
48 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_IMGPROC)
49 #include <opencv2/calib3d/calib3d.hpp>
50 #include <opencv2/imgproc/imgproc.hpp>
71 double n02_p,
double &xc_m,
double &yc_m,
double &n20_m,
double &n11_m,
double &n02_m);
72 static void convertLine(
const vpCameraParameters &cam,
const double &rho_p,
const double &theta_p,
double &rho_m,
104 switch (cam.m_projModel) {
106 convertPointWithoutDistortion(cam, u, v, x, y);
109 convertPointWithDistortion(cam, u, v, x, y);
112 convertPointWithKannalaBrandtDistortion(cam, u, v, x, y);
146 switch (cam.m_projModel) {
148 convertPointWithoutDistortion(cam, iP, x, y);
151 convertPointWithDistortion(cam, iP, x, y);
154 convertPointWithKannalaBrandtDistortion(cam, iP, x, y);
159 #ifndef DOXYGEN_SHOULD_SKIP_THIS
172 inline static void convertPointWithoutDistortion(
const vpCameraParameters &cam,
const double &u,
const double &v,
173 double &x,
double &y)
175 x = (u - cam.m_u0) * cam.m_inv_px;
176 y = (v - cam.m_v0) * cam.m_inv_py;
197 x = (iP.
get_u() - cam.m_u0) * cam.m_inv_px;
198 y = (iP.
get_v() - cam.m_v0) * cam.m_inv_py;
215 inline static void convertPointWithDistortion(
const vpCameraParameters &cam,
const double &u,
const double &v,
216 double &x,
double &y)
218 double r2 = 1. + cam.m_kdu * (
vpMath::sqr((u - cam.m_u0) * cam.m_inv_px) +
vpMath::sqr((v - cam.m_v0) * cam.m_inv_py));
219 x = (u - cam.m_u0) * r2 * cam.m_inv_px;
220 y = (v - cam.m_v0) * r2 * cam.m_inv_py;
242 double r2 = 1. + cam.m_kdu * (
vpMath::sqr((iP.
get_u() - cam.m_u0) * cam.m_inv_px) +
244 x = (iP.
get_u() - cam.m_u0) * r2 * cam.m_inv_px;
245 y = (iP.
get_v() - cam.m_v0) * r2 * cam.m_inv_py;
268 inline static void convertPointWithKannalaBrandtDistortion(
const vpCameraParameters &cam,
const double &u,
269 const double &v,
double &x,
double &y)
271 double x_d = (u - cam.m_u0) / cam.m_px, y_d = (v - cam.m_v0) / cam.m_py;
275 r_d = std::min<double>(std::max<double>(-M_PI, r_d), M_PI);
279 const double EPS = 1e-8;
285 for (
int j = 0; j < 10; ++j) {
286 double theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta6 * theta2;
287 double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6,
288 k3_theta8 = k[3] * theta8;
290 double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - r_d) /
291 (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8);
292 theta = theta - theta_fix;
293 if (fabs(theta_fix) < EPS)
297 scale = std::tan(theta) / r_d;
324 double &x,
double &y)
326 double x_d = (iP.
get_u() - cam.m_u0) / cam.m_px, y_d = (iP.
get_v() - cam.m_v0) / cam.m_py;
330 r_d = std::min<double>(std::max<double>(-M_PI, r_d), M_PI);
334 const double EPS = 1e-8;
340 for (
int j = 0; j < 10; ++j) {
341 double theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta6 * theta2;
342 double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6,
343 k3_theta8 = k[3] * theta8;
345 double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - r_d) /
346 (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8);
347 theta = theta - theta_fix;
348 if (fabs(theta_fix) < EPS)
352 scale = std::tan(theta) / r_d;
361 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_IMGPROC)
364 static void convertEllipse(
const cv::Mat &cameraMatrix,
const cv::Mat &distCoeffs,
const vpImagePoint ¢er_p,
365 double n20_p,
double n11_p,
double n02_p,
double &xc_m,
double &yc_m,
double &n20_m,
366 double &n11_m,
double &n02_m);
367 static void convertLine(
const cv::Mat &cameraMatrix,
const double &rho_p,
const double &theta_p,
double &rho_m,
369 static void convertMoment(
const cv::Mat &cameraMatrix,
unsigned int order,
const vpMatrix &moment_pixel,
371 static void convertPoint(
const cv::Mat &cameraMatrix,
const cv::Mat &distCoeffs,
const double &u,
const double &v,
372 double &x,
double &y);
373 static void convertPoint(
const cv::Mat &cameraMatrix,
const cv::Mat &distCoeffs,
const vpImagePoint &iP,
double &x,
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double sqr(double x)
Implementation of a matrix and operations on matrices.
static void convertPoint(const vpCameraParameters &cam, const vpImagePoint &iP, double &x, double &y)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)