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