Visual Servoing Platform  version 3.5.1 under development (2022-07-07)
vpMeterPixelConversion.cpp
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 
45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpMath.h>
49 #include <visp3/core/vpMeterPixelConversion.h>
50 
60 void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m,
61  double &rho_p, double &theta_p)
62 {
63  double co = cos(theta_m);
64  double si = sin(theta_m);
65  double d = sqrt(vpMath::sqr(cam.py * co) + vpMath::sqr(cam.px * si));
66 
67  if (fabs(d) < 1e-6) {
68  vpERROR_TRACE("division by zero");
69  throw(vpException(vpException::divideByZeroError, "division by zero"));
70  }
71 
72  theta_p = atan2(cam.px * si, cam.py * co);
73  rho_p = (cam.px * cam.py * rho_m + cam.u0 * cam.py * co + cam.v0 * cam.px * si);
74  rho_p /= d;
75 }
76 
103  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
104 {
105  // Get the parameters of the ellipse in the image plane
106  double xc_m = circle.p[0];
107  double yc_m = circle.p[1];
108  double n20_m = circle.p[2];
109  double n11_m = circle.p[3];
110  double n02_m = circle.p[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  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
146 {
147  // Get the parameters of the ellipse in the image plane
148  double xc_m = sphere.p[0];
149  double yc_m = sphere.p[1];
150  double n20_m = sphere.p[2];
151  double n11_m = sphere.p[3];
152  double n02_m = sphere.p[4];
153 
154  // Convert from meter to pixels
155  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
156  n20_p = n20_m * vpMath::sqr(cam.get_px());
157  n11_p = n11_m * cam.get_px() * cam.get_py();
158  n02_p = n02_m * vpMath::sqr(cam.get_py());
159 }
160 
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 VISP_HAVE_OPENCV_VERSION >= 0x020300
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 
335 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m,
336  double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
337  double &n11_p, double &n02_p)
338 {
339  double px = cameraMatrix.at<double>(0, 0);
340  double py = cameraMatrix.at<double>(1, 1);
341  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
342 
343  // Convert from meter to pixels
344  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center_p);
345  n20_p = n20_m * vpMath::sqr(px);
346  n11_p = n11_m * px * py;
347  n02_p = n02_m * vpMath::sqr(py);
348 }
349 
365 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
366  const double &y, double &u, double &v)
367 {
368  std::vector<cv::Point3d> objectPoints_vec;
369  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
370  std::vector<cv::Point2d> imagePoints_vec;
371  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
372  distCoeffs, imagePoints_vec);
373  u = imagePoints_vec[0].x;
374  v = imagePoints_vec[0].y;
375 }
376 
391 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
392  const double &y, vpImagePoint &iP)
393 {
394  std::vector<cv::Point3d> objectPoints_vec;
395  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
396  std::vector<cv::Point2d> imagePoints_vec;
397  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
398  distCoeffs, imagePoints_vec);
399  iP.set_u(imagePoints_vec[0].x);
400  iP.set_v(imagePoints_vec[0].y);
401 }
402 #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:92
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void set_u(double u)
Definition: vpImagePoint.h:335
void set_v(double v)
Definition: vpImagePoint.h:346
static double sqr(double x)
Definition: vpMath.h:123
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:83
vpColVector p
Definition: vpTracker.h:73
#define vpERROR_TRACE
Definition: vpDebug.h:393