Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpMeterPixelConversion.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Meter to pixel conversion.
33  *
34  * Authors:
35  * Eric Marchand
36  * Anthony Saunier
37  *
38  *****************************************************************************/
39 
40 #ifndef vpMeterPixelConversion_H
41 #define vpMeterPixelConversion_H
42 
49 #include <visp3/core/vpCameraParameters.h>
50 #include <visp3/core/vpCircle.h>
51 #include <visp3/core/vpSphere.h>
52 #include <visp3/core/vpException.h>
53 #include <visp3/core/vpImagePoint.h>
54 #include <visp3/core/vpMath.h>
55 
56 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
57 # include <opencv2/calib3d/calib3d.hpp>
58 #endif
59 
72 class VISP_EXPORT vpMeterPixelConversion
73 {
74 public:
77  static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p,
78  double &n20_p, double &n11_p, double &n02_p);
79  static void convertEllipse(const vpCameraParameters &cam, const vpCircle &circle, vpImagePoint &center_p,
80  double &n20_p, double &n11_p, double &n02_p);
81  static void convertEllipse(const vpCameraParameters &cam,
82  double xc_m, double yc_m, double n20_m, double n11_m, double n02_m,
83  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p);
84  static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p,
85  double &theta_p);
86 
112  inline static void convertPoint(const vpCameraParameters &cam,
113  const double &x, const double &y, double &u, double &v)
114  {
115  switch (cam.projModel) {
117  convertPointWithoutDistortion(cam, x, y, u, v);
118  break;
120  convertPointWithDistortion(cam, x, y, u, v);
121  break;
123  convertPointWithKannalaBrandtDistortion(cam, x, y, u, v);
124  break;
125  }
126  }
127 
155  inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
156  {
157  switch (cam.projModel) {
159  convertPointWithoutDistortion(cam, x, y, iP);
160  break;
162  convertPointWithDistortion(cam, x, y, iP);
163  break;
165  convertPointWithKannalaBrandtDistortion(cam, x, y, iP);
166  break;
167  }
168  }
169 
170 #ifndef DOXYGEN_SHOULD_SKIP_THIS
171 
180  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
181  double &u, double &v)
182  {
183  u = x * cam.px + cam.u0;
184  v = y * cam.py + cam.v0;
185  }
186 
197  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
198  vpImagePoint &iP)
199  {
200  iP.set_u(x * cam.px + cam.u0);
201  iP.set_v(y * cam.py + cam.v0);
202  }
203 
220  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
221  double &u, double &v)
222  {
223  double r2 = 1. + cam.kud * (x * x + y * y);
224  u = cam.u0 + cam.px * x * r2;
225  v = cam.v0 + cam.py * y * r2;
226  }
227 
244  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
245  vpImagePoint &iP)
246  {
247  double r2 = 1. + cam.kud * (x * x + y * y);
248  iP.set_u(cam.u0 + cam.px * x * r2);
249  iP.set_v(cam.v0 + cam.py * y * r2);
250  }
251 
274  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x, const double &y,
275  double &u, double &v)
276  {
277  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
278  double theta = atan(r);
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[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[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.px * x_d + cam.u0;
293  v = cam.py * y_d + cam.v0;
294  }
295 
317  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x, const double &y,
318  vpImagePoint &iP)
319  {
320  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
321  double theta = atan(r);
322 
323  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
324 
325  double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
326  theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
327 
328  double r_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
329 
330  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : r_d / r;
331 
332  double x_d = x * scale;
333  double y_d = y * scale;
334 
335  iP.set_u(cam.px * x_d + cam.u0);
336  iP.set_v(cam.py * y_d + cam.v0);
337  }
338 
339 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
340 
341 
342 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
343 
345  static void convertEllipse(const cv::Mat &cameraMatrix,
346  const vpCircle &circle, vpImagePoint &center,
347  double &n20_p, double &n11_p, double &n02_p);
348  static void convertEllipse(const cv::Mat &cameraMatrix,
349  const vpSphere &sphere, vpImagePoint &center,
350  double &n20_p, double &n11_p, double &n02_p);
351  static void convertEllipse(const cv::Mat &cameraMatrix,
352  double xc_m, double yc_m, double n20_m, double n11_m, double n02_m,
353  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p);
354  static void convertLine(const cv::Mat &cameraMatrix,
355  const double &rho_m, const double &theta_m,
356  double &rho_p, double &theta_p);
357  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
358  const double &x, const double &y, double &u, double &v);
359  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
360  const double &x, const double &y, vpImagePoint &iP);
362 #endif
363 };
364 
365 #endif
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
void set_u(double u)
Definition: vpImagePoint.h:225
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:82
static double sqr(double x)
Definition: vpMath.h:116
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
void set_v(double v)
Definition: vpImagePoint.h:236
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:91