Visual Servoing Platform version 3.5.0
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
199void 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
249void 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
298void 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
334void 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
365void 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
390void 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
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:88
void set_u(double u)
Definition: vpImagePoint.h:225
void set_v(double v)
Definition: vpImagePoint.h:236
static double sqr(double x)
Definition: vpMath.h:116
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