Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
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 BEGIN_VISP_NAMESPACE
55 void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m,
56  double &rho_p, double &theta_p)
57 {
58  double co = cos(theta_m);
59  double si = sin(theta_m);
60  double d = sqrt(vpMath::sqr(cam.m_py * co) + vpMath::sqr(cam.m_px * si));
61 
62  if (fabs(d) < 1e-6) {
63  throw(vpException(vpException::divideByZeroError, "division by zero"));
64  }
65 
66  theta_p = atan2(cam.m_px * si, cam.m_py * co);
67  rho_p = ((cam.m_px * cam.m_py * rho_m) + (cam.m_u0 * cam.m_py * co) + (cam.m_v0 * cam.m_px * si));
68  rho_p /= d;
69 }
70 
97 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpCircle &circle,
98  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
99 {
100  // Get the parameters of the ellipse in the image plane
101  const unsigned int index_0 = 0;
102  const unsigned int index_1 = 1;
103  const unsigned int index_2 = 2;
104  const unsigned int index_3 = 3;
105  const unsigned int index_4 = 4;
106  double xc_m = circle.p[index_0];
107  double yc_m = circle.p[index_1];
108  double n20_m = circle.p[index_2];
109  double n11_m = circle.p[index_3];
110  double n02_m = circle.p[index_4];
111 
112  // Convert from meter to pixels
113  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
114  n20_p = n20_m * vpMath::sqr(cam.get_px());
115  n11_p = n11_m * cam.get_px() * cam.get_py();
116  n02_p = n02_m * vpMath::sqr(cam.get_py());
117 }
118 
145 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere,
146  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
147 {
148  // Get the parameters of the ellipse in the image plane
149  const unsigned int index_0 = 0;
150  const unsigned int index_1 = 1;
151  const unsigned int index_2 = 2;
152  const unsigned int index_3 = 3;
153  const unsigned int index_4 = 4;
154  double xc_m = sphere.p[index_0];
155  double yc_m = sphere.p[index_1];
156  double n20_m = sphere.p[index_2];
157  double n11_m = sphere.p[index_3];
158  double n02_m = sphere.p[index_4];
159 
160  // Convert from meter to pixels
161  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
162  n20_p = n20_m * vpMath::sqr(cam.get_px());
163  n11_p = n11_m * cam.get_px() * cam.get_py();
164  n02_p = n02_m * vpMath::sqr(cam.get_py());
165 }
166 
186 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, double xc_m, double yc_m, double n20_m,
187  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
188  double &n11_p, double &n02_p)
189 {
190  // Convert from meter to pixels
191  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
192  n20_p = n20_m * vpMath::sqr(cam.get_px());
193  n11_p = n11_m * cam.get_px() * cam.get_py();
194  n02_p = n02_m * vpMath::sqr(cam.get_py());
195 }
196 
197 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
208 void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m,
209  double &rho_p, double &theta_p)
210 {
211  double co = cos(theta_m);
212  double si = sin(theta_m);
213  double px = cameraMatrix.at<double>(0, 0);
214  double py = cameraMatrix.at<double>(1, 1);
215  double u0 = cameraMatrix.at<double>(0, 2);
216  double v0 = cameraMatrix.at<double>(1, 2);
217  double d = sqrt(vpMath::sqr(py * co) + vpMath::sqr(px * si));
218 
219  if (fabs(d) < 1e-6) {
220  throw(vpException(vpException::divideByZeroError, "division by zero"));
221  }
222 
223  theta_p = atan2(px * si, py * co);
224  rho_p = (px * py * rho_m + u0 * py * co + v0 * px * si);
225  rho_p /= d;
226 }
227 
257 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center,
258  double &n20_p, double &n11_p, double &n02_p)
259 {
260  const unsigned int index_0 = 0;
261  const unsigned int index_1 = 1;
262  const unsigned int index_2 = 2;
263  const unsigned int index_3 = 3;
264  const unsigned int index_4 = 4;
265  const unsigned int index_5 = 5;
266  double px = cameraMatrix.at<double>(index_0, index_0);
267  double py = cameraMatrix.at<double>(index_1, index_1);
268  cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1);
269  // Get the parameters of the ellipse in the image plane
270  double xc_m = circle.p[index_0];
271  double yc_m = circle.p[index_1];
272  double n20_m = circle.p[index_2];
273  double n11_m = circle.p[index_3];
274  double n02_m = circle.p[index_4];
275 
276  // Convert from meter to pixels
277  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
278  n20_p = n20_m * vpMath::sqr(px);
279  n11_p = n11_m * px * py;
280  n02_p = n02_m * vpMath::sqr(py);
281 }
282 
312 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center,
313  double &n20_p, double &n11_p, double &n02_p)
314 {
315  const unsigned int index_0 = 0;
316  const unsigned int index_1 = 1;
317  const unsigned int index_2 = 2;
318  const unsigned int index_3 = 3;
319  const unsigned int index_4 = 4;
320  const unsigned int index_5 = 5;
321  double px = cameraMatrix.at<double>(index_0, index_0);
322  double py = cameraMatrix.at<double>(index_1, index_1);
323  cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1);
324  // Get the parameters of the ellipse in the image plane
325  double xc_m = sphere.p[index_0];
326  double yc_m = sphere.p[index_1];
327  double n20_m = sphere.p[index_2];
328  double n11_m = sphere.p[index_3];
329  double n02_m = sphere.p[index_4];
330 
331  // Convert from meter to pixels
332  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
333  n20_p = n20_m * vpMath::sqr(px);
334  n11_p = n11_m * px * py;
335  n02_p = n02_m * vpMath::sqr(py);
336 }
337 
356 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m,
357  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
358  double &n11_p, double &n02_p)
359 {
360  double px = cameraMatrix.at<double>(0, 0);
361  double py = cameraMatrix.at<double>(1, 1);
362  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
363 
364  // Convert from meter to pixels
365  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center_p);
366  n20_p = n20_m * vpMath::sqr(px);
367  n11_p = n11_m * px * py;
368  n02_p = n02_m * vpMath::sqr(py);
369 }
370 
386 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
387  const double &y, double &u, double &v)
388 {
389  std::vector<cv::Point3d> objectPoints_vec;
390  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
391  std::vector<cv::Point2d> imagePoints_vec;
392  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
393  distCoeffs, imagePoints_vec);
394  u = imagePoints_vec[0].x;
395  v = imagePoints_vec[0].y;
396 }
397 
412 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
413  const double &y, vpImagePoint &iP)
414 {
415  std::vector<cv::Point3d> objectPoints_vec;
416  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
417  std::vector<cv::Point2d> imagePoints_vec;
418  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
419  distCoeffs, imagePoints_vec);
420  iP.set_u(imagePoints_vec[0].x);
421  iP.set_v(imagePoints_vec[0].y);
422 }
423 #endif
424 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:55
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:145
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