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
55int 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);
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