Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
56void 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.py * co) + vpMath::sqr(cam.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.px * si, cam.py * co);
69 rho_p = (cam.px * cam.py * rho_m + cam.u0 * cam.py * co + cam.v0 * cam.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
178void 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)
200void 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
250void 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
299void 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
337void 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
367void 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
393void 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:87
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ divideByZeroError
Division by zero.
Definition vpException.h:82
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_u(double u)
void set_v(double v)
static double sqr(double x)
Definition vpMath.h:124
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:68
#define vpERROR_TRACE
Definition vpDebug.h:388