Visual Servoing Platform  version 3.4.1 under development (2021-04-20)
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  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  const vpCircle &circle, vpImagePoint &center_p,
104  double &n20_p, double &n11_p, double &n02_p)
105 {
106  // Get the parameters of the ellipse in the image plane
107  double xc_m = circle.p[0];
108  double yc_m = circle.p[1];
109  double n20_m = circle.p[2];
110  double n11_m = circle.p[3];
111  double n02_m = circle.p[4];
112 
113  // Convert from meter to pixels
114  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
115  n20_p = n20_m * vpMath::sqr(cam.get_px());
116  n11_p = n11_m * cam.get_px() * cam.get_py();
117  n02_p = n02_m * vpMath::sqr(cam.get_py());
118 }
119 
146  double &n20_p, double &n11_p, double &n02_p)
147 {
148  // Get the parameters of the ellipse in the image plane
149  double xc_m = sphere.p[0];
150  double yc_m = sphere.p[1];
151  double n20_m = sphere.p[2];
152  double n11_m = sphere.p[3];
153  double n02_m = sphere.p[4];
154 
155  // Convert from meter to pixels
156  vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
157  n20_p = n20_m * vpMath::sqr(cam.get_px());
158  n11_p = n11_m * cam.get_px() * cam.get_py();
159  n02_p = n02_m * vpMath::sqr(cam.get_py());
160 }
161 
179  double xc_m, double yc_m, double n20_m, double n11_m, double n02_m,
180  vpImagePoint &center_p, double &n20_p, 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
190 
199 void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix,
200  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 
249 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix,
250  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 
298 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix,
299  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 
334 void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix,
335  double xc_m, double yc_m, double n20_m, double n11_m, double n02_m,
336  vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
337 {
338  double px = cameraMatrix.at<double>(0,0);
339  double py = cameraMatrix.at<double>(1,1);
340  cv::Mat distCoeffs = cv::Mat::zeros(5,1,CV_64FC1);
341 
342  // Convert from meter to pixels
343  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center_p);
344  n20_p = n20_m * vpMath::sqr(px);
345  n11_p = n11_m * px * py;
346  n02_p = n02_m * vpMath::sqr(py);
347 
348 }
349 
365 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
366  const double &x, 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, distCoeffs, imagePoints_vec);
372  u = imagePoints_vec[0].x;
373  v = imagePoints_vec[0].y;
374 }
375 
390 void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs,
391  const double &x, const double &y, vpImagePoint &iP)
392 {
393  std::vector<cv::Point3d> objectPoints_vec;
394  objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
395  std::vector<cv::Point2d> imagePoints_vec;
396  cv::projectPoints(objectPoints_vec, cv::Mat::eye(3,3,CV_64FC1), cv::Mat::zeros(3,1,CV_64FC1), cameraMatrix, distCoeffs, imagePoints_vec);
397  iP.set_u(imagePoints_vec[0].x);
398  iP.set_v(imagePoints_vec[0].y);
399 }
400 #endif
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
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)
#define vpERROR_TRACE
Definition: vpDebug.h:393
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:83
static double sqr(double x)
Definition: vpMath.h:116
Generic class defining intrinsic camera parameters.
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
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:91
vpColVector p
Definition: vpTracker.h:73