Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpMeterPixelConversion.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Meter to pixel conversion.
32 */
33
38
39#include <visp3/core/vpCameraParameters.h>
40#include <visp3/core/vpException.h>
41#include <visp3/core/vpMath.h>
42#include <visp3/core/vpMeterPixelConversion.h>
43
44#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x050000) && defined(HAVE_OPENCV_CALIB3D)
45#include <opencv2/calib3d/calib3d.hpp>
46#endif
47#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x050000) && defined(HAVE_OPENCV_CALIB) && defined(HAVE_OPENCV_3D)
48#include <opencv2/calib.hpp>
49#include <opencv2/3d.hpp>
50#endif
51
52BEGIN_VISP_NAMESPACE
64void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m,
65 double &rho_p, double &theta_p)
66{
67 double co = cos(theta_m);
68 double si = sin(theta_m);
69 double d = sqrt(vpMath::sqr(cam.m_py * co) + vpMath::sqr(cam.m_px * si));
70
71 if (fabs(d) < 1e-6) {
72 throw(vpException(vpException::divideByZeroError, "division by zero"));
73 }
74
75 theta_p = atan2(cam.m_px * si, cam.m_py * co);
76 rho_p = ((cam.m_px * cam.m_py * rho_m) + (cam.m_u0 * cam.m_py * co) + (cam.m_v0 * cam.m_px * si));
77 rho_p /= d;
78}
79
110void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpCircle &circle,
111 vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
112{
113 // Get the parameters of the ellipse in the image plane
114 const unsigned int index_0 = 0;
115 const unsigned int index_1 = 1;
116 const unsigned int index_2 = 2;
117 const unsigned int index_3 = 3;
118 const unsigned int index_4 = 4;
119 double xc_m = circle.p[index_0];
120 double yc_m = circle.p[index_1];
121 double n20_m = circle.p[index_2];
122 double n11_m = circle.p[index_3];
123 double n02_m = circle.p[index_4];
124
125 // Convert from meter to pixels
126 vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
127 n20_p = n20_m * vpMath::sqr(cam.get_px());
128 n11_p = n11_m * cam.get_px() * cam.get_py();
129 n02_p = n02_m * vpMath::sqr(cam.get_py());
130}
131
162void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere,
163 vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
164{
165 // Get the parameters of the ellipse in the image plane
166 const unsigned int index_0 = 0;
167 const unsigned int index_1 = 1;
168 const unsigned int index_2 = 2;
169 const unsigned int index_3 = 3;
170 const unsigned int index_4 = 4;
171 double xc_m = sphere.p[index_0];
172 double yc_m = sphere.p[index_1];
173 double n20_m = sphere.p[index_2];
174 double n11_m = sphere.p[index_3];
175 double n02_m = sphere.p[index_4];
176
177 // Convert from meter to pixels
178 vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
179 n20_p = n20_m * vpMath::sqr(cam.get_px());
180 n11_p = n11_m * cam.get_px() * cam.get_py();
181 n02_p = n02_m * vpMath::sqr(cam.get_py());
182}
183
216void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, double xc_m, double yc_m, double n20_m,
217 double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
218 double &n11_p, double &n02_p)
219{
220 // Convert from meter to pixels
221 vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p);
222 n20_p = n20_m * vpMath::sqr(cam.get_px());
223 n11_p = n11_m * cam.get_px() * cam.get_py();
224 n02_p = n02_m * vpMath::sqr(cam.get_py());
225}
226
227#if defined(VISP_HAVE_OPENCV) && \
228 (((VISP_HAVE_OPENCV_VERSION < 0x050000) && defined(HAVE_OPENCV_CALIB3D)) || \
229 ((VISP_HAVE_OPENCV_VERSION >= 0x050000) && defined(HAVE_OPENCV_CALIB) && defined(HAVE_OPENCV_3D)))
241void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix, const double &rho_m, const double &theta_m,
242 double &rho_p, double &theta_p)
243{
244 double co = cos(theta_m);
245 double si = sin(theta_m);
246 double px = cameraMatrix.at<double>(0, 0);
247 double py = cameraMatrix.at<double>(1, 1);
248 double u0 = cameraMatrix.at<double>(0, 2);
249 double v0 = cameraMatrix.at<double>(1, 2);
250 double d = sqrt(vpMath::sqr(py * co) + vpMath::sqr(px * si));
251
252 if (fabs(d) < 1e-6) {
253 throw(vpException(vpException::divideByZeroError, "division by zero"));
254 }
255
256 theta_p = atan2(px * si, py * co);
257 rho_p = (px * py * rho_m + u0 * py * co + v0 * px * si);
258 rho_p /= d;
259}
260
293void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint &center,
294 double &n20_p, double &n11_p, double &n02_p)
295{
296 const unsigned int index_0 = 0;
297 const unsigned int index_1 = 1;
298 const unsigned int index_2 = 2;
299 const unsigned int index_3 = 3;
300 const unsigned int index_4 = 4;
301 const unsigned int index_5 = 5;
302 double px = cameraMatrix.at<double>(index_0, index_0);
303 double py = cameraMatrix.at<double>(index_1, index_1);
304 cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1);
305 // Get the parameters of the ellipse in the image plane
306 double xc_m = circle.p[index_0];
307 double yc_m = circle.p[index_1];
308 double n20_m = circle.p[index_2];
309 double n11_m = circle.p[index_3];
310 double n02_m = circle.p[index_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
351void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint &center,
352 double &n20_p, double &n11_p, double &n02_p)
353{
354 const unsigned int index_0 = 0;
355 const unsigned int index_1 = 1;
356 const unsigned int index_2 = 2;
357 const unsigned int index_3 = 3;
358 const unsigned int index_4 = 4;
359 const unsigned int index_5 = 5;
360 double px = cameraMatrix.at<double>(index_0, index_0);
361 double py = cameraMatrix.at<double>(index_1, index_1);
362 cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1);
363 // Get the parameters of the ellipse in the image plane
364 double xc_m = sphere.p[index_0];
365 double yc_m = sphere.p[index_1];
366 double n20_m = sphere.p[index_2];
367 double n11_m = sphere.p[index_3];
368 double n02_m = sphere.p[index_4];
369
370 // Convert from meter to pixels
371 vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center);
372 n20_p = n20_m * vpMath::sqr(px);
373 n11_p = n11_m * px * py;
374 n02_p = n02_m * vpMath::sqr(py);
375}
376
407void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, double xc_m, double yc_m, double n20_m,
408 double n11_m, double n02_m, vpImagePoint &center_p, double &n20_p,
409 double &n11_p, double &n02_p)
410{
411 double px = cameraMatrix.at<double>(0, 0);
412 double py = cameraMatrix.at<double>(1, 1);
413 cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
414
415 // Convert from meter to pixels
416 vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center_p);
417 n20_p = n20_m * vpMath::sqr(px);
418 n11_p = n11_m * px * py;
419 n02_p = n02_m * vpMath::sqr(py);
420}
421
437void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
438 const double &y, double &u, double &v)
439{
440 std::vector<cv::Point3d> objectPoints_vec;
441 objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
442 std::vector<cv::Point2d> imagePoints_vec;
443 cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
444 distCoeffs, imagePoints_vec);
445 u = imagePoints_vec[0].x;
446 v = imagePoints_vec[0].y;
447}
448
463void vpMeterPixelConversion::convertPoint(const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const double &x,
464 const double &y, vpImagePoint &iP)
465{
466 std::vector<cv::Point3d> objectPoints_vec;
467 objectPoints_vec.push_back(cv::Point3d(x, y, 1.0));
468 std::vector<cv::Point2d> imagePoints_vec;
469 cv::projectPoints(objectPoints_vec, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
470 distCoeffs, imagePoints_vec);
471 iP.set_u(imagePoints_vec[0].x);
472 iP.set_v(imagePoints_vec[0].y);
473}
474#endif
475END_VISP_NAMESPACE
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition vpCameraParameters.h:311
vpCircle
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition vpCircle.h:87
vpException
error that can be emitted by ViSP classes.
Definition vpException.h:60
vpException::divideByZeroError
@ divideByZeroError
Division by zero.
Definition vpException.h:70
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition vpImagePoint.h:82
vpImagePoint::set_u
void set_u(double u)
Definition vpImagePoint.h:335
vpImagePoint::set_v
void set_v(double v)
Definition vpImagePoint.h:346
vpMath::sqr
static double sqr(double x)
Definition vpMath.h:203
vpMeterPixelConversion::convertLine
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
Definition vpMeterPixelConversion.cpp:64
vpMeterPixelConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Definition vpMeterPixelConversion.h:105
vpMeterPixelConversion::convertEllipse
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
Definition vpMeterPixelConversion.cpp:162
vpSphere
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition vpSphere.h:80
vpTracker::p
vpColVector p
Definition vpTracker.h:69
BEGIN_VISP_NAMESPACE
Definition vpMbtDistanceCircle.cpp:55