Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
52int 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);
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:87
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
Definition vpCircle.cpp:246
void setWorldCoordinates(const vpColVector &oP)
Definition vpCircle.cpp:57
void projection()
Definition vpCircle.cpp:137
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 ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static double rad(double deg)
Definition vpMath.h:116
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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)
Definition vpSphere.cpp:59