Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpMeterPixelConversion.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
41 #include <visp3/core/vpCameraParameters.h>
42 #include <visp3/core/vpDebug.h>
43 #include <visp3/core/vpException.h>
44 #include <visp3/core/vpMath.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
46 
56 void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m,
57  double &rho_p, double &theta_p)
58 {
59  double co = cos(theta_m);
60  double si = sin(theta_m);
61  double d = sqrt(vpMath::sqr(cam.m_py * co) + vpMath::sqr(cam.m_px * si));
62 
63  if (fabs(d) < 1e-6) {
64  vpERROR_TRACE("division by zero");
65  throw(vpException(vpException::divideByZeroError, "division by zero"));
66  }
67 
68  theta_p = atan2(cam.m_px * si, cam.m_py * co);
69  rho_p = ((cam.m_px * cam.m_py * rho_m) + (cam.m_u0 * cam.m_py * co) + (cam.m_v0 * cam.m_px * si));
70  rho_p /= d;
71 }
72 
100  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
101 {
102  // Get the parameters of the ellipse in the image plane
103  double xc_m = circle.p[0];
104  double yc_m = circle.p[1];
105  double n20_m = circle.p[2];
106  double n11_m = circle.p[3];
107  double n02_m = circle.p[4];
108 
109  // Convert from meter to pixels
110  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
111  n20_p = n20_m * vpMath::sqr(cam.get_px());
112  n11_p = n11_m * cam.get_px() * cam.get_py();
113  n02_p = n02_m * vpMath::sqr(cam.get_py());
114 }
115 
143  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
144 {
145  // Get the parameters of the ellipse in the image plane
146  double xc_m = sphere.p[0];
147  double yc_m = sphere.p[1];
148  double n20_m = sphere.p[2];
149  double n11_m = sphere.p[3];
150  double n02_m = sphere.p[4];
151 
152  // Convert from meter to pixels
153  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
154  n20_p = n20_m * vpMath::sqr(cam.get_px());
155  n11_p = n11_m * cam.get_px() * cam.get_py();
156  n02_p = n02_m * vpMath::sqr(cam.get_py());
157 }
158 
178 void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, double xc_m, double yc_m, double n20_m,
179  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
180  double &n11_p, double &n02_p)
181 {
182  // Convert from meter to pixels
183  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
184  n20_p = n20_m * vpMath::sqr(cam.get_px());
185  n11_p = n11_m * cam.get_px() * cam.get_py();
186  n02_p = n02_m * vpMath::sqr(cam.get_py());
187 }
188 
189 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
200 void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m,
201  double &rho_p, double &theta_p)
202 {
203  double co = cos(theta_m);
204  double si = sin(theta_m);
205  double px = cameraMatrix.at<double>(0, 0);
206  double py = cameraMatrix.at<double>(1, 1);
207  double u0 = cameraMatrix.at<double>(0, 2);
208  double v0 = cameraMatrix.at<double>(1, 2);
209  double d = sqrt(vpMath::sqr(py * co) + vpMath::sqr(px * si));
210 
211  if (fabs(d) < 1e-6) {
212  vpERROR_TRACE("division by zero");
213  throw(vpException(vpException::divideByZeroError, "division by zero"));
214  }
215 
216  theta_p = atan2(px * si, py * co);
217  rho_p = (px * py * rho_m + u0 * py * co + v0 * px * si);
218  rho_p /= d;
219 }
220 
250 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center,
251  double &n20_p, double &n11_p, double &n02_p)
252 {
253  double px = cameraMatrix.at<double>(0, 0);
254  double py = cameraMatrix.at<double>(1, 1);
255  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
256  // Get the parameters of the ellipse in the image plane
257  double xc_m = circle.p[0];
258  double yc_m = circle.p[1];
259  double n20_m = circle.p[2];
260  double n11_m = circle.p[3];
261  double n02_m = circle.p[4];
262 
263  // Convert from meter to pixels
264  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
265  n20_p = n20_m * vpMath::sqr(px);
266  n11_p = n11_m * px * py;
267  n02_p = n02_m * vpMath::sqr(py);
268 }
269 
299 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center,
300  double &n20_p, double &n11_p, double &n02_p)
301 {
302  double px = cameraMatrix.at<double>(0, 0);
303  double py = cameraMatrix.at<double>(1, 1);
304  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
305  // Get the parameters of the ellipse in the image plane
306  double xc_m = sphere.p[0];
307  double yc_m = sphere.p[1];
308  double n20_m = sphere.p[2];
309  double n11_m = sphere.p[3];
310  double n02_m = sphere.p[4];
311 
312  // Convert from meter to pixels
313  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
314  n20_p = n20_m * vpMath::sqr(px);
315  n11_p = n11_m * px * py;
316  n02_p = n02_m * vpMath::sqr(py);
317 }
318 
337 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m,
338  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
339  double &n11_p, double &n02_p)
340 {
341  double px = cameraMatrix.at<double>(0, 0);
342  double py = cameraMatrix.at<double>(1, 1);
343  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
344 
345  // Convert from meter to pixels
346  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center_p);
347  n20_p = n20_m * vpMath::sqr(px);
348  n11_p = n11_m * px * py;
349  n02_p = n02_m * vpMath::sqr(py);
350 }
351 
367 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
368  const double &y, double &u, double &v)
369 {
370  std::vector<cv::Point3d> objectPoints_vec;
371  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
372  std::vector<cv::Point2d> imagePoints_vec;
373  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
374  distCoeffs, imagePoints_vec);
375  u = imagePoints_vec[0].x;
376  v = imagePoints_vec[0].y;
377 }
378 
393 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
394  const double &y, vpImagePoint &iP)
395 {
396  std::vector<cv::Point3d> objectPoints_vec;
397  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
398  std::vector<cv::Point2d> imagePoints_vec;
399  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
400  distCoeffs, imagePoints_vec);
401  iP.set_u(imagePoints_vec[0].x);
402  iP.set_v(imagePoints_vec[0].y);
403 }
404 #endif
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ divideByZeroError
Division by zero.
Definition: vpException.h:69
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:201
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:78
vpColVector p
Definition: vpTracker.h:67
#define vpERROR_TRACE
Definition: vpDebug.h:384