Visual Servoing Platform  version 3.3.1 under development (2020-07-03)
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,
78  double &mu20_p, double &mu11_p, double &mu02_p);
79  static void convertEllipse(const vpCameraParameters &cam, const vpCircle &circle, vpImagePoint &center,
80  double &mu20_p, double &mu11_p, double &mu02_p);
81  static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p,
82  double &theta_p);
83 
110  inline static void convertPoint(const vpCameraParameters &cam,
111  const double &x, const double &y, double &u, double &v)
112  {
113  switch (cam.projModel) {
115  convertPointWithoutDistortion(cam, x, y, u, v);
116  break;
118  convertPointWithDistortion(cam, x, y, u, v);
119  break;
121  convertPointWithKannalaBrandtDistortion(cam, x, y, u, v);
122  break;
123  }
124  }
125 
153  inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
154  {
155  switch (cam.projModel) {
157  convertPointWithoutDistortion(cam, x, y, iP);
158  break;
160  convertPointWithDistortion(cam, x, y, iP);
161  break;
163  convertPointWithKannalaBrandtDistortion(cam, x, y, iP);
164  break;
165  }
166  }
167 
168 #ifndef DOXYGEN_SHOULD_SKIP_THIS
169 
178  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
179  double &u, double &v)
180  {
181  u = x * cam.px + cam.u0;
182  v = y * cam.py + cam.v0;
183  }
184 
195  inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &x, const double &y,
196  vpImagePoint &iP)
197  {
198  iP.set_u(x * cam.px + cam.u0);
199  iP.set_v(y * cam.py + cam.v0);
200  }
201 
218  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
219  double &u, double &v)
220  {
221  double r2 = 1. + cam.kud * (x * x + y * y);
222  u = cam.u0 + cam.px * x * r2;
223  v = cam.v0 + cam.py * y * r2;
224  }
225 
242  inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
243  vpImagePoint &iP)
244  {
245  double r2 = 1. + cam.kud * (x * x + y * y);
246  iP.set_u(cam.u0 + cam.px * x * r2);
247  iP.set_v(cam.v0 + cam.py * y * r2);
248  }
249 
272  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x, const double &y,
273  double &u, double &v)
274  {
275  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
276  double theta = atan(r);
277 
278  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
279 
280  double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
281  theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
282 
283  double r_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
284 
285  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : r_d / r;
286 
287  double x_d = x * scale;
288  double y_d = y * scale;
289 
290  u = cam.px * x_d + cam.u0;
291  v = cam.py * y_d + cam.v0;
292  }
293 
315  inline static void convertPointWithKannalaBrandtDistortion(const vpCameraParameters &cam, const double &x, const double &y,
316  vpImagePoint &iP)
317  {
318  double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y));
319  double theta = atan(r);
320 
321  std::vector<double> k = cam.getKannalaBrandtDistortionCoefficients();
322 
323  double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
324  theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
325 
326  double r_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
327 
328  double scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : r_d / r;
329 
330  double x_d = x * scale;
331  double y_d = y * scale;
332 
333  iP.set_u(cam.px * x_d + cam.u0);
334  iP.set_v(cam.py * y_d + cam.v0);
335  }
336 
337 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
338 
339 
340 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
341 
343  static void convertEllipse(const cv::Mat &cameraMatrix,
344  const vpCircle &circle, vpImagePoint &center,
345  double &mu20_p, double &mu11_p, double &mu02_p);
346  static void convertEllipse(const cv::Mat &cameraMatrix,
347  const vpSphere &sphere, vpImagePoint &center,
348  double &mu20_p, double &mu11_p, double &mu02_p);
349  static void convertLine(const cv::Mat &cameraMatrix,
350  const double &rho_m, const double &theta_m,
351  double &rho_p, double &theta_p);
352  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
353  const double &x, const double &y, double &u, double &v);
354  static void convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
355  const double &x, const double &y, vpImagePoint &iP);
357 #endif
358 };
359 
360 #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:79
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:87