Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpCalibration.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 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 * Camera calibration.
32 */
33
39#include <visp3/core/vpDebug.h>
40#include <visp3/core/vpPixelMeterConversion.h>
41#include <visp3/vision/vpCalibration.h>
42#include <visp3/vision/vpPose.h>
43
44double vpCalibration::threshold = 1e-10f;
45unsigned int vpCalibration::nbIterMax = 4000;
46double vpCalibration::gain = 0.25;
47
49{
50 npt = 0;
51
52 residual = residual_dist = 1000.;
53
54 LoX.clear();
55 LoY.clear();
56 LoZ.clear();
57 Lip.clear();
58
59 return 0;
60}
61
63 : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), m_aspect_ratio(-1), npt(0), LoX(), LoY(), LoZ(),
64 Lip(), residual(1000.), residual_dist(1000.)
65
66{
67 init();
68}
69
71 : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), m_aspect_ratio(-1), npt(0), LoX(), LoY(), LoZ(),
72 Lip(), residual(1000.), residual_dist(1000.)
73{
74 (*this) = c;
75}
76
78
80{
81 npt = twinCalibration.npt;
82 LoX = twinCalibration.LoX;
83 LoY = twinCalibration.LoY;
84 LoZ = twinCalibration.LoZ;
85 Lip = twinCalibration.Lip;
86
87 residual = twinCalibration.residual;
88 cMo = twinCalibration.cMo;
89 residual_dist = twinCalibration.residual_dist;
90 cMo_dist = twinCalibration.cMo_dist;
91
92 cam = twinCalibration.cam;
93 cam_dist = twinCalibration.cam_dist;
94
95 rMe = twinCalibration.rMe;
96
97 eMc = twinCalibration.eMc;
98 eMc_dist = twinCalibration.eMc_dist;
99
100 m_aspect_ratio = twinCalibration.m_aspect_ratio;
101
102 return (*this);
103}
104
106{
107 LoX.clear();
108 LoY.clear();
109 LoZ.clear();
110 Lip.clear();
111 npt = 0;
112
113 return 0;
114}
115
116int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip)
117{
118 LoX.push_back(X);
119 LoY.push_back(Y);
120 LoZ.push_back(Z);
121
122 Lip.push_back(ip);
123
124 npt++;
125
126 return 0;
127}
128
134void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousMatrix &cMo_est)
135{
136 // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y)
137 // )
138 vpPose pose;
139 // the list of point is cleared (if that's not done before)
140 pose.clearPoint();
141 // we set the 3D points coordinates (in meter !) in the object/world frame
142 std::list<double>::const_iterator it_LoX = LoX.begin();
143 std::list<double>::const_iterator it_LoY = LoY.begin();
144 std::list<double>::const_iterator it_LoZ = LoZ.begin();
145 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
146
147 for (unsigned int i = 0; i < npt; i++) {
148 vpPoint P(*it_LoX, *it_LoY, *it_LoZ);
149 double x = 0, y = 0;
150 vpPixelMeterConversion::convertPoint(camera, *it_Lip, x, y);
151 P.set_x(x);
152 P.set_y(y);
153
154 pose.addPoint(P);
155 ++it_LoX;
156 ++it_LoY;
157 ++it_LoZ;
158 ++it_Lip;
159 }
160
161 // the pose is now refined using the virtual visual servoing approach
162 // Warning: cMo needs to be initialized otherwise it may diverge
164}
165
167{
168 double residual_ = 0;
169
170 std::list<double>::const_iterator it_LoX = LoX.begin();
171 std::list<double>::const_iterator it_LoY = LoY.begin();
172 std::list<double>::const_iterator it_LoZ = LoZ.begin();
173 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
174
175 double u0 = camera.get_u0();
176 double v0 = camera.get_v0();
177 double px = camera.get_px();
178 double py = camera.get_py();
179 vpImagePoint ip;
180
181 for (unsigned int i = 0; i < npt; i++) {
182 double oX = *it_LoX;
183 double oY = *it_LoY;
184 double oZ = *it_LoZ;
185
186 double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
187 double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
188 double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
189
190 double x = cX / cZ;
191 double y = cY / cZ;
192
193 ip = *it_Lip;
194 double u = ip.get_u();
195 double v = ip.get_v();
196
197 double xp = u0 + x * px;
198 double yp = v0 + y * py;
199
200 residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
201
202 ++it_LoX;
203 ++it_LoY;
204 ++it_LoZ;
205 ++it_Lip;
206 }
207 this->residual = residual_;
208 return sqrt(residual_ / npt);
209}
210
212{
213 double residual_ = 0;
214
215 std::list<double>::const_iterator it_LoX = LoX.begin();
216 std::list<double>::const_iterator it_LoY = LoY.begin();
217 std::list<double>::const_iterator it_LoZ = LoZ.begin();
218 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
219
220 double u0 = camera.get_u0();
221 double v0 = camera.get_v0();
222 double px = camera.get_px();
223 double py = camera.get_py();
224 double kud = camera.get_kud();
225 double kdu = camera.get_kdu();
226
227 double inv_px = 1 / px;
228 double inv_py = 1 / px;
229 vpImagePoint ip;
230
231 for (unsigned int i = 0; i < npt; i++) {
232 double oX = *it_LoX;
233 double oY = *it_LoY;
234 double oZ = *it_LoZ;
235
236 double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
237 double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
238 double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
239
240 double x = cX / cZ;
241 double y = cY / cZ;
242
243 ip = *it_Lip;
244 double u = ip.get_u();
245 double v = ip.get_v();
246
247 double r2ud = 1 + kud * (vpMath::sqr(x) + vpMath::sqr(y));
248
249 double xp = u0 + x * px * r2ud;
250 double yp = v0 + y * py * r2ud;
251
252 residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
253
254 double r2du = (vpMath::sqr((u - u0) * inv_px) + vpMath::sqr((v - v0) * inv_py));
255
256 xp = u0 + x * px - kdu * (u - u0) * r2du;
257 yp = v0 + y * py - kdu * (v - v0) * r2du;
258
259 residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
260 ++it_LoX;
261 ++it_LoY;
262 ++it_LoZ;
263 ++it_Lip;
264 }
265 residual_ /= 2;
266
267 this->residual_dist = residual_;
268 return sqrt(residual_ / npt);
269}
270
271void vpCalibration::computeStdDeviation(double &deviation, double &deviation_dist)
272{
273 deviation = computeStdDeviation(cMo, cam);
274 deviation_dist = computeStdDeviation_dist(cMo_dist, cam_dist);
275}
276
278 vpCameraParameters &cam_est, bool verbose)
279{
280 try {
281 computePose(cam_est, cMo_est);
282 switch (method) {
283 case CALIB_LAGRANGE:
285 calibLagrange(cam_est, cMo_est);
286 } break;
287 case CALIB_VIRTUAL_VS:
290 default:
291 break;
292 }
293
294 switch (method) {
295 case CALIB_VIRTUAL_VS:
299 if (verbose) {
300 std::cout << "start calibration without distortion" << std::endl;
301 }
302 calibVVS(cam_est, cMo_est, verbose);
303 } break;
304 case CALIB_LAGRANGE:
305 default:
306 break;
307 }
308 this->cMo = cMo_est;
309 this->cMo_dist = cMo_est;
310
311 // Print camera parameters
312 if (verbose) {
313 // std::cout << "Camera parameters without distortion :" <<
314 // std::endl;
315 cam_est.printParameters();
316 }
317
318 this->cam = cam_est;
319
320 switch (method) {
323 if (verbose) {
324 std::cout << "start calibration with distortion" << std::endl;
325 }
326 calibVVSWithDistortion(cam_est, cMo_est, verbose);
327 } break;
328 case CALIB_LAGRANGE:
329 case CALIB_VIRTUAL_VS:
331 default:
332 break;
333 }
334 // Print camera parameters
335 if (verbose) {
336 // std::cout << "Camera parameters without distortion :" <<
337 // std::endl;
338 this->cam.printParameters();
339 // std::cout << "Camera parameters with distortion :" <<
340 // std::endl;
341 cam_est.printParameters();
342 }
343
344 this->cam_dist = cam_est;
345
346 this->cMo_dist = cMo_est;
347
348 if (cam_est.get_px() < 0 || cam_est.get_py() < 0 || cam_est.get_u0() < 0 || cam_est.get_v0() < 0) {
349 if (verbose) {
350 std::cout << "Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
351 }
352 return EXIT_FAILURE;
353 }
354
355 return EXIT_SUCCESS;
356 }
357 catch (...) {
358 throw;
359 }
360}
361
362int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std::vector<vpCalibration> &table_cal,
363 vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose)
364{
365 try {
366 unsigned int nbPose = (unsigned int)table_cal.size();
367 for (unsigned int i = 0; i < nbPose; i++) {
368 if (table_cal[i].get_npt() > 3)
369 table_cal[i].computePose(cam_est, table_cal[i].cMo);
370 }
371 switch (method) {
372 case CALIB_LAGRANGE: {
373 if (nbPose > 1) {
374 std::cout << "this calibration method is not available in" << std::endl
375 << "vpCalibration::computeCalibrationMulti()" << std::endl;
376 return -1;
377 }
378 else {
379 table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
380 table_cal[0].cam = cam_est;
381 table_cal[0].cam_dist = cam_est;
382 table_cal[0].cMo_dist = table_cal[0].cMo;
383 }
384 break;
385 }
388 if (nbPose > 1) {
389 std::cout << "this calibration method is not available in" << std::endl
390 << "vpCalibration::computeCalibrationMulti()" << std::endl
391 << "with several images." << std::endl;
392 return -1;
393 }
394 else {
395 table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
396 table_cal[0].cam = cam_est;
397 table_cal[0].cam_dist = cam_est;
398 table_cal[0].cMo_dist = table_cal[0].cMo;
399 }
400 calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose, table_cal[0].m_aspect_ratio);
401 break;
402 }
403 case CALIB_VIRTUAL_VS:
405 calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose, table_cal[0].m_aspect_ratio);
406 break;
407 }
408 }
409 // Print camera parameters
410 if (verbose) {
411 // std::cout << "Camera parameters without distortion :" <<
412 // std::endl;
413 cam_est.printParameters();
414 }
415
416 switch (method) {
417 case CALIB_LAGRANGE:
419 case CALIB_VIRTUAL_VS:
420 verbose = false;
421 break;
424 if (verbose)
425 std::cout << "Compute camera parameters with distortion" << std::endl;
426
427 calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose, table_cal[0].m_aspect_ratio);
428 } break;
429 }
430 // Print camera parameters
431 if (verbose) {
432 // std::cout << "Camera parameters without distortion :" <<
433 // std::endl;
434 table_cal[0].cam.printParameters();
435 // std::cout << "Camera parameters with distortion:" << std::endl;
436 cam_est.printParameters();
437 std::cout << std::endl;
438 }
439
440 if (cam_est.get_px() < 0 || cam_est.get_py() < 0 || cam_est.get_u0() < 0 || cam_est.get_v0() < 0) {
441 if (verbose) {
442 std::cout << "Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
443 }
444 return EXIT_FAILURE;
445 }
446
447 return EXIT_SUCCESS;
448 }
449 catch (...) {
450 throw;
451 }
452}
453
454int vpCalibration::writeData(const std::string &filename)
455{
456 std::ofstream f(filename.c_str());
457 vpImagePoint ip;
458
459 std::list<double>::const_iterator it_LoX = LoX.begin();
460 std::list<double>::const_iterator it_LoY = LoY.begin();
461 std::list<double>::const_iterator it_LoZ = LoZ.begin();
462 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
463
464 f.precision(10);
465 f.setf(std::ios::fixed, std::ios::floatfield);
466 f << LoX.size() << std::endl;
467
468 for (unsigned int i = 0; i < LoX.size(); i++) {
469
470 double oX = *it_LoX;
471 double oY = *it_LoY;
472 double oZ = *it_LoZ;
473
474 ip = *it_Lip;
475 double u = ip.get_u();
476 double v = ip.get_v();
477
478 f << oX << " " << oY << " " << oZ << " " << u << " " << v << std::endl;
479
480 ++it_LoX;
481 ++it_LoY;
482 ++it_LoZ;
483 ++it_Lip;
484 }
485
486 f.close();
487 return 0;
488}
489
490int vpCalibration::readData(const std::string &filename)
491{
492 vpImagePoint ip;
493 std::ifstream f;
494 f.open(filename.c_str());
495 if (!f.fail()) {
496 unsigned int n;
497 f >> n;
498 std::cout << "There are " << n << " point on the calibration grid " << std::endl;
499
500 clearPoint();
501
502 if (n > 100000)
503 throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
504
505 for (unsigned int i = 0; i < n; i++) {
506 double x, y, z, u, v;
507 f >> x >> y >> z >> u >> v;
508 std::cout << x << " " << y << " " << z << " " << u << " " << v << std::endl;
509 ip.set_u(u);
510 ip.set_v(v);
511 addPoint(x, y, z, ip);
512 }
513
514 f.close();
515 return 0;
516 }
517 else {
518 return -1;
519 }
520}
521
522int vpCalibration::readGrid(const std::string &filename, unsigned int &n, std::list<double> &oX, std::list<double> &oY,
523 std::list<double> &oZ, bool verbose)
524{
525 try {
526 std::ifstream f;
527 f.open(filename.c_str());
528 if (!f.fail()) {
529
530 f >> n;
531 if (verbose)
532 std::cout << "There are " << n << " points on the calibration grid " << std::endl;
533 int no_pt;
534 double x, y, z;
535
536 // clear the list
537 oX.clear();
538 oY.clear();
539 oZ.clear();
540
541 if (n > 100000)
542 throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
543
544 for (unsigned int i = 0; i < n; i++) {
545 f >> no_pt >> x >> y >> z;
546 if (verbose) {
547 std::cout << no_pt << std::endl;
548 std::cout << x << " " << y << " " << z << std::endl;
549 }
550 oX.push_back(x);
551 oY.push_back(y);
552 oZ.push_back(z);
553 }
554
555 f.close();
556 }
557 else {
558 return -1;
559 }
560 }
561 catch (...) {
562 return -1;
563 }
564 return 0;
565}
566
567int vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
568{
569
570 for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++it) {
571 vpImagePoint ip = *it;
572 if (subsampling_factor > 1.) {
573 ip.set_u(ip.get_u() / subsampling_factor);
574 ip.set_v(ip.get_v() / subsampling_factor);
575 }
576 vpDisplay::displayCross(I, ip, 12, color, thickness);
577 }
578 return 0;
579}
580
581int vpCalibration::displayGrid(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
582{
583 double u0_dist = cam_dist.get_u0() / subsampling_factor;
584 double v0_dist = cam_dist.get_v0() / subsampling_factor;
585 double px_dist = cam_dist.get_px() / subsampling_factor;
586 double py_dist = cam_dist.get_py() / subsampling_factor;
587 double kud_dist = cam_dist.get_kud();
588 // double kdu_dist = cam_dist.get_kdu() ;
589
590 // double u0 = cam.get_u0() ;
591 // double v0 = cam.get_v0() ;
592 // double px = cam.get_px() ;
593 // double py = cam.get_py() ;
594
595 std::list<double>::const_iterator it_LoX = LoX.begin();
596 std::list<double>::const_iterator it_LoY = LoY.begin();
597 std::list<double>::const_iterator it_LoZ = LoZ.begin();
598
599 for (unsigned int i = 0; i < npt; i++) {
600 double oX = *it_LoX;
601 double oY = *it_LoY;
602 double oZ = *it_LoZ;
603
604 double cX = oX * cMo_dist[0][0] + oY * cMo_dist[0][1] + oZ * cMo_dist[0][2] + cMo_dist[0][3];
605 double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + oZ * cMo_dist[1][2] + cMo_dist[1][3];
606 double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + oZ * cMo_dist[2][2] + cMo_dist[2][3];
607
608 double x = cX / cZ;
609 double y = cY / cZ;
610
611 double r2 = 1 + kud_dist * (vpMath::sqr(x) + vpMath::sqr(y));
612
613 vpImagePoint ip;
614 ip.set_u(u0_dist + x * px_dist * r2);
615 ip.set_v(v0_dist + y * py_dist * r2);
616
617 vpDisplay::displayCross(I, ip, 6, color, thickness);
619
620 // std::cout << oX << " " << oY << " " <<oZ << std::endl ;
621 // I.getClick() ;
622 ++it_LoX;
623 ++it_LoY;
624 ++it_LoZ;
625 }
626 return 0;
627}
628
629void vpCalibration::setAspectRatio(double aspect_ratio)
630{
631 if (aspect_ratio > 0.) {
632 m_aspect_ratio = aspect_ratio;
633 }
634}
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Tools for perspective camera calibration.
void computeStdDeviation(double &deviation, double &deviation_dist)
int displayData(vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1, int subsampling_factor=1)
unsigned int get_npt() const
double m_aspect_ratio
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
vpHomogeneousMatrix eMc_dist
vpHomogeneousMatrix rMe
int writeData(const std::string &filename)
vpCalibration & operator=(const vpCalibration &twinCalibration)
vpHomogeneousMatrix cMo
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera)
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose=false)
vpCameraParameters cam_dist
static int readGrid(const std::string &filename, unsigned int &n, std::list< double > &oX, std::list< double > &oY, std::list< double > &oZ, bool verbose=false)
void setAspectRatio(double aspect_ratio)
vpHomogeneousMatrix eMc
vpHomogeneousMatrix cMo_dist
virtual ~vpCalibration()
int displayGrid(vpImage< unsigned char > &I, vpColor color=vpColor::yellow, unsigned int thickness=1, int subsampling_factor=1)
@ CALIB_LAGRANGE_VIRTUAL_VS_DIST
int readData(const std::string &filename)
vpCameraParameters cam
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
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 ...
double get_u() const
void set_u(double u)
void set_v(double v)
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double sqr(double x)
Definition vpMath.h:124
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
void clearPoint()
Definition vpPose.cpp:125
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469