Visual Servoing Platform  version 3.6.1 under development (2024-06-19)
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 
39 #ifndef _vpMeterPixelConversion_h_
40 #define _vpMeterPixelConversion_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 
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 
276  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
277 
278  double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta,
279  theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta;
280 
281  double r_d = theta + (k[0] * theta3) + (k[1] * theta5) + (k[2] * theta7) + (k[3] * theta9);
282 
283  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : (r_d / r);
284 
285  double x_d = x * scale;
286  double y_d = y * scale;
287 
288  u = (cam.m_px * x_d) + cam.m_u0;
289  v = (cam.m_py * y_d) + cam.m_v0;
290  }
291 
313  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x,
314  const double &y, vpImagePoint &iP)
315  {
316  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
317  double theta = atan(r);
318 
319  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
320 
321  double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta,
322  theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta;
323 
324  double r_d = theta + (k[0] * theta3) + (k[1] * theta5) + (k[2] * theta7) + (k[3] * theta9);
325 
326  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : (r_d / r);
327 
328  double x_d = x * scale;
329  double y_d = y * scale;
330 
331  iP.set_u((cam.m_px * x_d) + cam.m_u0);
332  iP.set_v((cam.m_py * y_d) + cam.m_v0);
333  }
334 
335 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
337 
338 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
341  static void convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center, double &n20_p,
342  double &n11_p, double &n02_p);
343  static void convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center, double &n20_p,
344  double &n11_p, double &n02_p);
345  static void convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m, double n11_m,
346  double n02_m, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p);
347  static void convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m, double &rho_p,
348  double &theta_p);
349  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x, const double &y,
350  double &u, double &v);
351  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x, const double &y,
352  vpImagePoint &iP);
354 #endif
355 };
357 #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:88
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:331
void set_v(double v)
Definition: vpImagePoint.h:342
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:81