Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testCameraParametersConversion.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  * Performs various tests on the vpPixelMeterConversion and
33  * vpPixelMeterConversion class.
34  *
35 *****************************************************************************/
36 
44 #include <stdio.h>
45 #include <stdlib.h>
46 #include <visp3/core/vpCameraParameters.h>
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpMath.h>
49 #include <visp3/core/vpMeterPixelConversion.h>
50 #include <visp3/core/vpPixelMeterConversion.h>
51 
52 int main()
53 {
54  try {
55  {
56  std::cout << "* Test operator=()" << std::endl;
57  vpCameraParameters cam1, cam2;
58  cam1.initPersProjWithDistortion(700.0, 700.0, 320.0, 240.0, 0.1, 0.1);
59  cam2.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0);
60  if (cam1 == cam2) {
61  std::cerr << "Issue with vpCameraParameters comparison operator." << std::endl;
62  return EXIT_FAILURE;
63  }
64 
65  cam2 = cam1;
66  if (cam1 != cam2) {
67  std::cerr << "Issue with vpCameraParameters comparison operator." << std::endl;
68  return EXIT_FAILURE;
69  }
70 
71  std::cout << "* Test computeFov()" << std::endl;
72  cam2.computeFov(640u, 480u);
73  if (cam1 == cam2) {
74  std::cerr << "Issue with vpCameraParameters comparison operator." << std::endl;
75  return EXIT_FAILURE;
76  }
77  }
78 
80  double px, py, u0, v0;
81  px = 1657.429131;
82  py = 1658.818598;
83  u0 = 322.2437833;
84  v0 = 230.8012737;
85  vpCameraParameters camDist;
86  double px_dist, py_dist, u0_dist, v0_dist, kud_dist, kdu_dist;
87  px_dist = 1624.824731;
88  py_dist = 1625.263641;
89  u0_dist = 324.0923411;
90  v0_dist = 245.2421388;
91  kud_dist = -0.1741532338;
92  kdu_dist = 0.1771165148;
93 
94  cam.initPersProjWithoutDistortion(px, py, u0, v0);
95  camDist.initPersProjWithDistortion(px_dist, py_dist, u0_dist, v0_dist, kud_dist, kdu_dist);
96 
97  double u1 = 320;
98  double v1 = 240;
99  double x1 = 0, y1 = 0;
100  double u2 = 0, v2 = 0;
101  std::cout << "* Test point conversion without distortion" << std::endl;
102  vpPixelMeterConversion::convertPoint(cam, u1, v1, x1, y1);
103  vpMeterPixelConversion::convertPoint(cam, x1, y1, u2, v2);
104  if (!vpMath::equal(u1, u2) || !vpMath::equal(v1, v2)) {
105  std::cerr << "Error in point conversion without distortion:\n"
106  << "u1 = " << u1 << ", u2 = " << u2 << std::endl
107  << "v1 = " << v1 << ", v2 = " << v2 << std::endl;
108  return EXIT_FAILURE;
109  }
110 
111  std::cout << "* Test point conversion with distortion" << std::endl;
112  vpPixelMeterConversion::convertPoint(camDist, u1, v1, x1, y1);
113  vpMeterPixelConversion::convertPoint(camDist, x1, y1, u2, v2);
114  if (!vpMath::equal(u1, u2) || !vpMath::equal(v1, v2)) {
115  std::cerr << "Error in point conversion without distortion:\n"
116  << "u1 = " << u1 << ", u2 = " << u2 << std::endl
117  << "v1 = " << v1 << ", v2 = " << v2 << std::endl;
118  return EXIT_FAILURE;
119  }
120 
121 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_IMGPROC)
122  {
123  std::cout << "* Compare ViSP and OpenCV point pixel meter conversion without distortion" << std::endl;
124  cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << px, 0, u0, 0, py, v0, 0, 0, 1);
125  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
126  double x2, y2;
127 
128  vpPixelMeterConversion::convertPoint(cam, u1, v1, x1, y1);
129  vpPixelMeterConversion::convertPoint(cameraMatrix, distCoeffs, u1, v1, x2, y2);
130  if (!vpMath::equal(x1, x2, 1e-6) || !vpMath::equal(y1, y2, 1e-6)) {
131  std::cerr << "Error in point pixel meter conversion: visp result (" << x1 << ", " << y1 << ") "
132  << "differ from OpenCV result (" << x2 << ", " << y2 << ")" << std::endl;
133  return EXIT_FAILURE;
134  }
135 
136  vpImagePoint ip(v1, u1);
137  vpPixelMeterConversion::convertPoint(cam, ip, x1, y1);
138  vpPixelMeterConversion::convertPoint(cameraMatrix, distCoeffs, ip, x2, y2);
139  if (!vpMath::equal(x1, x2, 1e-6) || !vpMath::equal(y1, y2, 1e-6)) {
140  std::cerr << "Error in point pixel meter conversion: visp result (" << x1 << ", " << y1 << ") "
141  << "differ from OpenCV result (" << x2 << ", " << y2 << ")" << std::endl;
142  return EXIT_FAILURE;
143  }
144 
145  std::cout << "* Compare ViSP and OpenCV point meter pixel conversion without distortion" << std::endl;
146  vpMeterPixelConversion::convertPoint(cam, x1, y1, u1, v1);
147  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, x1, y1, u2, v2);
148  if (!vpMath::equal(u1, u2, 1e-6) || !vpMath::equal(v1, v2, 1e-6)) {
149  std::cerr << "Error in point meter pixel conversion: visp result (" << u1 << ", " << v1 << ") "
150  << "differ from OpenCV result (" << u2 << ", " << v2 << ")" << std::endl;
151  return EXIT_FAILURE;
152  }
153 
154  vpImagePoint iP1, iP2;
155  vpMeterPixelConversion::convertPoint(cam, x1, y1, iP1);
156  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, x1, y1, iP2);
157  if (vpImagePoint::distance(iP1, iP2) > 1e-6) {
158  std::cerr << "Error in point meter pixel conversion: visp result (" << u1 << ", " << v1 << ") "
159  << "differ from OpenCV result (" << u2 << ", " << v2 << ")" << std::endl;
160  return EXIT_FAILURE;
161  }
162  }
163 
164  {
165  std::cout << "* Compare ViSP and OpenCV point pixel meter conversion with distortion" << std::endl;
166  cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << px_dist, 0, u0_dist, 0, py_dist, v0_dist, 0, 0, 1);
167  cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1);
168  distCoeffs.at<double>(0, 0) = kdu_dist;
169  double x2, y2;
170 
171  vpPixelMeterConversion::convertPoint(camDist, u1, v1, x1, y1);
172  vpPixelMeterConversion::convertPoint(cameraMatrix, distCoeffs, u1, v1, x2, y2);
173  if (!vpMath::equal(x1, x2, 1e-6) || !vpMath::equal(y1, y2, 1e-6)) {
174  std::cerr << "Error in point conversion: visp result (" << x1 << ", " << y1 << ") "
175  << "differ from OpenCV result (" << x2 << ", " << y2 << ")" << std::endl;
176  return EXIT_FAILURE;
177  }
178 
179  std::cout << "* Compare ViSP and OpenCV point meter pixel conversion with distortion" << std::endl;
180  distCoeffs.at<double>(0, 0) = kud_dist;
181  vpMeterPixelConversion::convertPoint(camDist, x1, y1, u1, v1);
182  vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, x1, y1, u2, v2);
183  if (!vpMath::equal(u1, u2, 1e-6) || !vpMath::equal(v1, v2, 1e-6)) {
184  std::cerr << "Error in point meter pixel conversion: visp result (" << u1 << ", " << v1 << ") "
185  << "differ from OpenCV result (" << u2 << ", " << v2 << ")" << std::endl;
186  return EXIT_FAILURE;
187  }
188  }
189 
190  {
191  std::cout << "* Compare ViSP and OpenCV line pixel meter conversion without distortion" << std::endl;
192  cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << px, 0, u0, 0, py, v0, 0, 0, 1);
193  double rho_p = 100, theta_p = vpMath::rad(45);
194  double rho_m1, theta_m1, rho_m2, theta_m2;
195 
196  vpPixelMeterConversion::convertLine(cam, rho_p, theta_p, rho_m1, theta_m1);
197  vpPixelMeterConversion::convertLine(cameraMatrix, rho_p, theta_p, rho_m2, theta_m2);
198  if (!vpMath::equal(rho_m1, rho_m2, 1e-6) || !vpMath::equal(theta_m1, theta_m2, 1e-6)) {
199  std::cerr << "Error in line pixel meter conversion: visp result (" << rho_m1 << ", " << theta_m1 << ") "
200  << "differ from OpenCV result (" << rho_m2 << ", " << theta_m1 << ")" << std::endl;
201  return EXIT_FAILURE;
202  }
203 
204  std::cout << "* Compare ViSP and OpenCV line meter pixel conversion without distortion" << std::endl;
205  double rho_p1, theta_p1, rho_p2, theta_p2;
206  vpMeterPixelConversion::convertLine(cam, rho_m1, theta_m1, rho_p1, theta_p1);
207  vpMeterPixelConversion::convertLine(cameraMatrix, rho_m1, theta_m1, rho_p2, theta_p2);
208  if (!vpMath::equal(rho_p1, rho_p2, 1e-6) || !vpMath::equal(theta_p1, theta_p2, 1e-6)) {
209  std::cerr << "Error in line meter pixel conversion: visp result (" << rho_p1 << ", " << theta_p1 << ") "
210  << "differ from OpenCV result (" << rho_p2 << ", " << theta_p1 << ")" << std::endl;
211  return EXIT_FAILURE;
212  }
213  }
214 
215  {
216  std::cout << "* Compare ViSP and OpenCV moments pixel meter conversion without distortion" << std::endl;
217  cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << px, 0, u0, 0, py, v0, 0, 0, 1);
218  unsigned int order = 3;
219  double m00 = 2442, m10 = 414992, m01 = 470311, m11 = 7.99558e+07, m02 = 9.09603e+07, m20 = 7.11158e+07;
220 
221  vpMatrix mp(order, order);
222  vpMatrix m1(order, order), m2(order, order);
223 
224  mp[0][0] = m00;
225  mp[1][0] = m10;
226  mp[0][1] = m01;
227  mp[2][0] = m20;
228  mp[1][1] = m11;
229  mp[0][2] = m02;
230 
231  vpPixelMeterConversion::convertMoment(cam, order, mp, m1);
232  vpPixelMeterConversion::convertMoment(cameraMatrix, order, mp, m2);
233  for (unsigned int i = 0; i < m1.getRows(); i++) {
234  for (unsigned int j = 0; j < m1.getCols(); j++) {
235  if (!vpMath::equal(m1[i][j], m1[i][j], 1e-6)) {
236  std::cerr << "Error in moments pixel meter conversion: visp result for [" << i << "][" << j << "] ("
237  << m1[i][j] << ") "
238  << "differ from OpenCV result (" << m2[i][j] << ")" << std::endl;
239  return EXIT_FAILURE;
240  }
241  }
242  }
243  }
244 
245  {
246  std::cout << "* Compare ViSP and OpenCV ellipse from circle meter pixel conversion without distortion"
247  << std::endl;
248  cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << px, 0, u0, 0, py, v0, 0, 0, 1);
249  vpCircle circle;
250  circle.setWorldCoordinates(0, 0, 1, 0, 0, 0, 0.1); // plane:(Z=0),X0=0,Y0=0,Z=0,R=0.1
251  vpHomogeneousMatrix cMo(0.1, 0.2, 0.5, vpMath::rad(10), vpMath::rad(5), vpMath::rad(45));
252  circle.changeFrame(cMo);
253  circle.projection();
254  vpImagePoint center_p1, center_p2;
255  double n20_p1, n11_p1, n02_p1, n20_p2, n11_p2, n02_p2;
256 
257  vpMeterPixelConversion::convertEllipse(cam, circle, center_p1, n20_p1, n11_p1, n02_p1);
258  vpMeterPixelConversion::convertEllipse(cameraMatrix, circle, center_p2, n20_p2, n11_p2, n02_p2);
259 
260  if (!vpMath::equal(n20_p1, n20_p2, 1e-6) || !vpMath::equal(n11_p1, n11_p2, 1e-6) ||
261  !vpMath::equal(n02_p1, n02_p2, 1e-6)) {
262  std::cerr << "Error in ellipse from circle meter pixel conversion: visp result (" << n20_p1 << ", " << n11_p1
263  << ", " << n02_p1 << ") "
264  << "differ from OpenCV result (" << n20_p2 << ", " << n11_p2 << ", " << n02_p2 << ")" << std::endl;
265  return EXIT_FAILURE;
266  }
267  if (vpImagePoint::distance(center_p1, center_p2) > 1e-6) {
268  std::cerr << "Error in ellipse from circle meter pixel conversion: visp result (" << center_p1 << ") "
269  << "differ from OpenCV result (" << center_p2 << ")" << std::endl;
270  return EXIT_FAILURE;
271  }
272 
273  std::cout << "* Compare ViSP and OpenCV ellipse from sphere meter pixel conversion without distortion"
274  << std::endl;
275  vpSphere sphere;
276  sphere.setWorldCoordinates(0, 0, 0, 0.1); // X0=0,Y0=0,Z0=0,R=0.1
277  circle.changeFrame(cMo);
278  circle.projection();
279  vpMeterPixelConversion::convertEllipse(cam, sphere, center_p1, n20_p1, n11_p1, n02_p1);
280  vpMeterPixelConversion::convertEllipse(cameraMatrix, sphere, center_p2, n20_p2, n11_p2, n02_p2);
281 
282  if (!vpMath::equal(n20_p1, n20_p2, 1e-6) || !vpMath::equal(n11_p1, n11_p2, 1e-6) ||
283  !vpMath::equal(n02_p1, n02_p2, 1e-6)) {
284  std::cerr << "Error in ellipse from sphere meter pixel conversion: visp result (" << n20_p1 << ", " << n11_p1
285  << ", " << n02_p1 << ") "
286  << "differ from OpenCV result (" << n20_p2 << ", " << n11_p2 << ", " << n02_p2 << ")" << std::endl;
287  return EXIT_FAILURE;
288  }
289  if (vpImagePoint::distance(center_p1, center_p2) > 1e-6) {
290  std::cerr << "Error in ellipse from sphere meter pixel conversion: visp result (" << center_p1 << ") "
291  << "differ from OpenCV result (" << center_p2 << ")" << std::endl;
292  return EXIT_FAILURE;
293  }
294  }
295 #endif
296 
297  std::cout << "Test successful" << std::endl;
298  return EXIT_SUCCESS;
299  } catch (const vpException &e) {
300  std::cout << "Catch an exception: " << e << std::endl;
301  return EXIT_FAILURE;
302  }
303 }
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
void computeFov(const unsigned int &w, const unsigned int &h)
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
void projection() vp_override
Definition: vpCircle.cpp:137
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCircle.cpp:57
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const vp_override
Definition: vpCircle.cpp:246
error that can be emitted by ViSP classes.
Definition: vpException.h:59
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static double rad(double deg)
Definition: vpMath.h:127
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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)
static void convertMoment(const vpCameraParameters &cam, unsigned int order, const vpMatrix &moment_pixel, vpMatrix &moment_meter)
static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m, double &theta_m)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:78
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpSphere.cpp:59