Visual Servoing Platform  version 3.6.1 under development (2025-02-18)
vpMeterPixelConversion.cpp
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 *****************************************************************************/
34 
40 #include <visp3/core/vpCameraParameters.h>
41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 
45 #if (VISP_HAVE_OPENCV_VERSION < 0x050000) && defined(HAVE_OPENCV_CALIB3D)
46 #include <opencv2/calib3d/calib3d.hpp>
47 #endif
48 #if (VISP_HAVE_OPENCV_VERSION >= 0x050000) && defined(HAVE_OPENCV_CALIB) && defined(HAVE_OPENCV_3D)
49 #include <opencv2/calib.hpp>
50 #include <opencv2/3d.hpp>
51 #endif
52 
53 BEGIN_VISP_NAMESPACE
63 void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m,
64  double &rho_p, double &theta_p)
65 {
66  double co = cos(theta_m);
67  double si = sin(theta_m);
68  double d = sqrt(vpMath::sqr(cam.m_py * co) + vpMath::sqr(cam.m_px * si));
69 
70  if (fabs(d) < 1e-6) {
71  throw(vpException(vpException::divideByZeroError, "division by zero"));
72  }
73 
74  theta_p = atan2(cam.m_px * si, cam.m_py * co);
75  rho_p = ((cam.m_px * cam.m_py * rho_m) + (cam.m_u0 * cam.m_py * co) + (cam.m_v0 * cam.m_px * si));
76  rho_p /= d;
77 }
78 
105 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpCircle &circle,
106  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
107 {
108  // Get the parameters of the ellipse in the image plane
109  const unsigned int index_0 = 0;
110  const unsigned int index_1 = 1;
111  const unsigned int index_2 = 2;
112  const unsigned int index_3 = 3;
113  const unsigned int index_4 = 4;
114  double xc_m = circle.p[index_0];
115  double yc_m = circle.p[index_1];
116  double n20_m = circle.p[index_2];
117  double n11_m = circle.p[index_3];
118  double n02_m = circle.p[index_4];
119 
120  // Convert from meter to pixels
121  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
122  n20_p = n20_m * vpMath::sqr(cam.get_px());
123  n11_p = n11_m * cam.get_px() * cam.get_py();
124  n02_p = n02_m * vpMath::sqr(cam.get_py());
125 }
126 
153 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere,
154  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
155 {
156  // Get the parameters of the ellipse in the image plane
157  const unsigned int index_0 = 0;
158  const unsigned int index_1 = 1;
159  const unsigned int index_2 = 2;
160  const unsigned int index_3 = 3;
161  const unsigned int index_4 = 4;
162  double xc_m = sphere.p[index_0];
163  double yc_m = sphere.p[index_1];
164  double n20_m = sphere.p[index_2];
165  double n11_m = sphere.p[index_3];
166  double n02_m = sphere.p[index_4];
167 
168  // Convert from meter to pixels
169  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
170  n20_p = n20_m * vpMath::sqr(cam.get_px());
171  n11_p = n11_m * cam.get_px() * cam.get_py();
172  n02_p = n02_m * vpMath::sqr(cam.get_py());
173 }
174 
194 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, double xc_m, double yc_m, double n20_m,
195  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
196  double &n11_p, double &n02_p)
197 {
198  // Convert from meter to pixels
199  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
200  n20_p = n20_m * vpMath::sqr(cam.get_px());
201  n11_p = n11_m * cam.get_px() * cam.get_py();
202  n02_p = n02_m * vpMath::sqr(cam.get_py());
203 }
204 
205 #if (((VISP_HAVE_OPENCV_VERSION < 0x050000) && defined(HAVE_OPENCV_CALIB3D)) || ((VISP_HAVE_OPENCV_VERSION >= 0x050000) && defined(HAVE_OPENCV_CALIB) && defined(HAVE_OPENCV_3D)))
216 void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m,
217  double &rho_p, double &theta_p)
218 {
219  double co = cos(theta_m);
220  double si = sin(theta_m);
221  double px = cameraMatrix.at<double>(0, 0);
222  double py = cameraMatrix.at<double>(1, 1);
223  double u0 = cameraMatrix.at<double>(0, 2);
224  double v0 = cameraMatrix.at<double>(1, 2);
225  double d = sqrt(vpMath::sqr(py * co) + vpMath::sqr(px * si));
226 
227  if (fabs(d) < 1e-6) {
228  throw(vpException(vpException::divideByZeroError, "division by zero"));
229  }
230 
231  theta_p = atan2(px * si, py * co);
232  rho_p = (px * py * rho_m + u0 * py * co + v0 * px * si);
233  rho_p /= d;
234 }
235 
265 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center,
266  double &n20_p, double &n11_p, double &n02_p)
267 {
268  const unsigned int index_0 = 0;
269  const unsigned int index_1 = 1;
270  const unsigned int index_2 = 2;
271  const unsigned int index_3 = 3;
272  const unsigned int index_4 = 4;
273  const unsigned int index_5 = 5;
274  double px = cameraMatrix.at<double>(index_0, index_0);
275  double py = cameraMatrix.at<double>(index_1, index_1);
276  cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1);
277  // Get the parameters of the ellipse in the image plane
278  double xc_m = circle.p[index_0];
279  double yc_m = circle.p[index_1];
280  double n20_m = circle.p[index_2];
281  double n11_m = circle.p[index_3];
282  double n02_m = circle.p[index_4];
283 
284  // Convert from meter to pixels
285  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
286  n20_p = n20_m * vpMath::sqr(px);
287  n11_p = n11_m * px * py;
288  n02_p = n02_m * vpMath::sqr(py);
289 }
290 
320 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center,
321  double &n20_p, double &n11_p, double &n02_p)
322 {
323  const unsigned int index_0 = 0;
324  const unsigned int index_1 = 1;
325  const unsigned int index_2 = 2;
326  const unsigned int index_3 = 3;
327  const unsigned int index_4 = 4;
328  const unsigned int index_5 = 5;
329  double px = cameraMatrix.at<double>(index_0, index_0);
330  double py = cameraMatrix.at<double>(index_1, index_1);
331  cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1);
332  // Get the parameters of the ellipse in the image plane
333  double xc_m = sphere.p[index_0];
334  double yc_m = sphere.p[index_1];
335  double n20_m = sphere.p[index_2];
336  double n11_m = sphere.p[index_3];
337  double n02_m = sphere.p[index_4];
338 
339  // Convert from meter to pixels
340  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
341  n20_p = n20_m * vpMath::sqr(px);
342  n11_p = n11_m * px * py;
343  n02_p = n02_m * vpMath::sqr(py);
344 }
345 
364 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m,
365  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
366  double &n11_p, double &n02_p)
367 {
368  double px = cameraMatrix.at<double>(0, 0);
369  double py = cameraMatrix.at<double>(1, 1);
370  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
371 
372  // Convert from meter to pixels
373  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center_p);
374  n20_p = n20_m * vpMath::sqr(px);
375  n11_p = n11_m * px * py;
376  n02_p = n02_m * vpMath::sqr(py);
377 }
378 
394 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
395  const double &y, double &u, double &v)
396 {
397  std::vector<cv::Point3d> objectPoints_vec;
398  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
399  std::vector<cv::Point2d> imagePoints_vec;
400  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
401  distCoeffs, imagePoints_vec);
402  u = imagePoints_vec[0].x;
403  v = imagePoints_vec[0].y;
404 }
405 
420 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
421  const double &y, vpImagePoint &iP)
422 {
423  std::vector<cv::Point3d> objectPoints_vec;
424  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
425  std::vector<cv::Point2d> imagePoints_vec;
426  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
427  distCoeffs, imagePoints_vec);
428  iP.set_u(imagePoints_vec[0].x);
429  iP.set_v(imagePoints_vec[0].y);
430 }
431 #endif
432 END_VISP_NAMESPACE
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:310
vpCircle
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:87
vpException
error that can be emitted by ViSP classes.
Definition: vpException.h:60
vpException::divideByZeroError
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
vpImagePoint::set_u
void set_u(double u)
Definition: vpImagePoint.h:335
vpImagePoint::set_v
void set_v(double v)
Definition: vpImagePoint.h:346
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:203
vpMeterPixelConversion::convertLine
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
Definition: vpMeterPixelConversion.cpp:63
vpMeterPixelConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Definition: vpMeterPixelConversion.h:105
vpMeterPixelConversion::convertEllipse
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
Definition: vpMeterPixelConversion.cpp:153
vpSphere
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:80
vpTracker::p
vpColVector p
Definition: vpTracker.h:69