Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMeterPixelConversion.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Meter to pixel conversion.
32  */
33 
34 #ifndef _vpMeterPixelConversion_h_
35 #define _vpMeterPixelConversion_h_
36 
43 #include <visp3/core/vpCameraParameters.h>
44 #include <visp3/core/vpCircle.h>
45 #include <visp3/core/vpException.h>
46 #include <visp3/core/vpImagePoint.h>
47 #include <visp3/core/vpMath.h>
48 #include <visp3/core/vpSphere.h>
49 
50 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
51 #include <opencv2/calib3d/calib3d.hpp>
52 #endif
53 
66 class VISP_EXPORT vpMeterPixelConversion
67 {
68 public:
71  static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p,
72  double &n20_p, double &n11_p, double &n02_p);
73  static void convertEllipse(const vpCameraParameters &cam, const vpCircle &circle, vpImagePoint &center_p,
74  double &n20_p, double &n11_p, double &n02_p);
75  static void convertEllipse(const vpCameraParameters &cam, double xc_m, double yc_m, double n20_m, double n11_m,
76  double n02_m, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p);
77  static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p,
78  double &theta_p);
79 
105  inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
106  {
107  switch (cam.m_projModel) {
109  convertPointWithoutDistortion(cam, x, y, u, v);
110  break;
112  convertPointWithDistortion(cam, x, y, u, v);
113  break;
115  convertPointWithKannalaBrandtDistortion(cam, x, y, u, v);
116  break;
117  }
118  }
119 
147  inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
148  {
149  switch (cam.m_projModel) {
151  convertPointWithoutDistortion(cam, x, y, iP);
152  break;
154  convertPointWithDistortion(cam, x, y, iP);
155  break;
157  convertPointWithKannalaBrandtDistortion(cam, x, y, iP);
158  break;
159  }
160  }
161 
162 #ifndef DOXYGEN_SHOULD_SKIP_THIS
172  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
173  double &u, double &v)
174  {
175  u = x * cam.m_px + cam.m_u0;
176  v = y * cam.m_py + cam.m_v0;
177  }
178 
189  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
190  vpImagePoint &iP)
191  {
192  iP.set_u(x * cam.m_px + cam.m_u0);
193  iP.set_v(y * cam.m_py + cam.m_v0);
194  }
195 
212  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
213  double &u, double &v)
214  {
215  double r2 = 1. + cam.m_kud * (x * x + y * y);
216  u = cam.m_u0 + cam.m_px * x * r2;
217  v = cam.m_v0 + cam.m_py * y * r2;
218  }
219 
236  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
237  vpImagePoint &iP)
238  {
239  double r2 = 1. + cam.m_kud * (x * x + y * y);
240  iP.set_u(cam.m_u0 + cam.m_px * x * r2);
241  iP.set_v(cam.m_v0 + cam.m_py * y * r2);
242  }
243 
266  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x,
267  const double &y, double &u, double &v)
268  {
269  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
270  double theta = atan(r);
271 
272  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
273 
274  double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta,
275  theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta;
276 
277  double r_d = theta + k[0] * theta3 + k[1] * theta5 + k[2] * theta7 + k[3] * theta9;
278 
279  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : r_d / r;
280 
281  double x_d = x * scale;
282  double y_d = y * scale;
283 
284  u = cam.m_px * x_d + cam.m_u0;
285  v = cam.m_py * y_d + cam.m_v0;
286  }
287 
309  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x,
310  const double &y, vpImagePoint &iP)
311  {
312  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
313  double theta = atan(r);
314 
315  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
316 
317  double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta,
318  theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta;
319 
320  double r_d = theta + k[0] * theta3 + k[1] * theta5 + k[2] * theta7 + k[3] * theta9;
321 
322  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : r_d / r;
323 
324  double x_d = x * scale;
325  double y_d = y * scale;
326 
327  iP.set_u(cam.m_px * x_d + cam.m_u0);
328  iP.set_v(cam.m_py * y_d + cam.m_v0);
329  }
330 
331 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
333 
334 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
337  static void convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center, double &n20_p,
338  double &n11_p, double &n02_p);
339  static void convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center, double &n20_p,
340  double &n11_p, double &n02_p);
341  static void convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m, double n11_m,
342  double n02_m, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p);
343  static void convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m, double &rho_p,
344  double &theta_p);
345  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x, const double &y,
346  double &u, double &v);
347  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x, const double &y,
348  vpImagePoint &iP);
350 #endif
351 };
352 
353 #endif
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 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_u(double u)
Definition: vpImagePoint.h:330
void set_v(double v)
Definition: vpImagePoint.h:341
static double sqr(double x)
Definition: vpMath.h:201
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:78