Visual Servoing Platform  version 3.6.1 under development (2024-12-07)
vpMeterPixelConversion.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 
39 #ifndef VP_METER_PIXEL_CONVERSION_H
40 #define VP_METER_PIXEL_CONVERSION_H
41 
42 #include <visp3/core/vpCameraParameters.h>
43 #include <visp3/core/vpCircle.h>
44 #include <visp3/core/vpException.h>
45 #include <visp3/core/vpImagePoint.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpSphere.h>
48 
49 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
50 #include <opencv2/calib3d/calib3d.hpp>
51 #endif
52 
53 BEGIN_VISP_NAMESPACE
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  default:
118  std::cerr << "projection model not identified" << std::endl;
119  }
120  }
121 
149  inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
150  {
151  switch (cam.m_projModel) {
153  convertPointWithoutDistortion(cam, x, y, iP);
154  break;
156  convertPointWithDistortion(cam, x, y, iP);
157  break;
159  convertPointWithKannalaBrandtDistortion(cam, x, y, iP);
160  break;
161  default:
162  std::cerr << "projection model not identified" << std::endl;
163  }
164  }
165 
166 #ifndef DOXYGEN_SHOULD_SKIP_THIS
176  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
177  double &u, double &v)
178  {
179  u = (x * cam.m_px) + cam.m_u0;
180  v = (y * cam.m_py) + cam.m_v0;
181  }
182 
193  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
194  vpImagePoint &iP)
195  {
196  iP.set_u((x * cam.m_px) + cam.m_u0);
197  iP.set_v((y * cam.m_py) + cam.m_v0);
198  }
199 
216  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
217  double &u, double &v)
218  {
219  double r2 = 1. + (cam.m_kud * ((x * x) + (y * y)));
220  u = cam.m_u0 + (cam.m_px * x * r2);
221  v = cam.m_v0 + (cam.m_py * y * r2);
222  }
223 
240  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
241  vpImagePoint &iP)
242  {
243  double r2 = 1. + (cam.m_kud * ((x * x) + (y * y)));
244  iP.set_u(cam.m_u0 + (cam.m_px * x * r2));
245  iP.set_v(cam.m_v0 + (cam.m_py * y * r2));
246  }
247 
270  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x,
271  const double &y, double &u, double &v)
272  {
273  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
274  double theta = atan(r);
275  const unsigned int index_0 = 0;
276  const unsigned int index_1 = 1;
277  const unsigned int index_2 = 2;
278  const unsigned int index_3 = 3;
279 
280  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
281 
282  double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta,
283  theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta;
284 
285  double r_d = theta + (k[index_0] * theta3) + (k[index_1] * theta5) + (k[index_2] * theta7) + (k[index_3] * theta9);
286 
287  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : (r_d / r);
288 
289  double x_d = x * scale;
290  double y_d = y * scale;
291 
292  u = (cam.m_px * x_d) + cam.m_u0;
293  v = (cam.m_py * y_d) + cam.m_v0;
294  }
295 
317  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x,
318  const double &y, vpImagePoint &iP)
319  {
320  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
321  double theta = atan(r);
322  const unsigned int index_0 = 0;
323  const unsigned int index_1 = 1;
324  const unsigned int index_2 = 2;
325  const unsigned int index_3 = 3;
326 
327  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
328 
329  double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta,
330  theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta;
331 
332  double r_d = theta + (k[index_0] * theta3) + (k[index_1] * theta5) + (k[index_2] * theta7) + (k[index_3] * theta9);
333 
334  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : (r_d / r);
335 
336  double x_d = x * scale;
337  double y_d = y * scale;
338 
339  iP.set_u((cam.m_px * x_d) + cam.m_u0);
340  iP.set_v((cam.m_py * y_d) + cam.m_v0);
341  }
342 
343 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
345 
346 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
349  static void convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center, double &n20_p,
350  double &n11_p, double &n02_p);
351  static void convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center, double &n20_p,
352  double &n11_p, double &n02_p);
353  static void convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m, double n11_m,
354  double n02_m, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p);
355  static void convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m, double &rho_p,
356  double &theta_p);
357  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x, const double &y,
358  double &u, double &v);
359  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x, const double &y,
360  vpImagePoint &iP);
362 #endif
363 };
364 END_VISP_NAMESPACE
365 #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:87
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:335
void set_v(double v)
Definition: vpImagePoint.h:346
static double sqr(double x)
Definition: vpMath.h:203
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:80