Visual Servoing Platform version 3.5.0
vpCalibration.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 * Camera calibration.
33 *
34 * Authors:
35 * Eric Marchand
36 * Francois Chaumette
37 * Anthony Saunier
38 *
39 *****************************************************************************/
40
46#include <visp3/core/vpDebug.h>
47#include <visp3/core/vpPixelMeterConversion.h>
48#include <visp3/vision/vpCalibration.h>
49#include <visp3/vision/vpPose.h>
50
51double vpCalibration::threshold = 1e-10f;
52unsigned int vpCalibration::nbIterMax = 4000;
53double vpCalibration::gain = 0.25;
58{
59 npt = 0;
60
61 residual = residual_dist = 1000.;
62
63 LoX.clear();
64 LoY.clear();
65 LoZ.clear();
66 Lip.clear();
67
68 return 0;
69}
70
75 : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), m_aspect_ratio(-1), npt(0), LoX(), LoY(), LoZ(),
76 Lip(), residual(1000.), residual_dist(1000.)
77
78{
79 init();
80}
85 : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), m_aspect_ratio(-1), npt(0), LoX(), LoY(), LoZ(),
86 Lip(), residual(1000.), residual_dist(1000.)
87{
88 (*this) = c;
89}
90
95
102{
103 npt = twinCalibration.npt;
104 LoX = twinCalibration.LoX;
105 LoY = twinCalibration.LoY;
106 LoZ = twinCalibration.LoZ;
107 Lip = twinCalibration.Lip;
108
109 residual = twinCalibration.residual;
110 cMo = twinCalibration.cMo;
111 residual_dist = twinCalibration.residual_dist;
112 cMo_dist = twinCalibration.cMo_dist;
113
114 cam = twinCalibration.cam;
115 cam_dist = twinCalibration.cam_dist;
116
117 rMe = twinCalibration.rMe;
118
119 eMc = twinCalibration.eMc;
120 eMc_dist = twinCalibration.eMc_dist;
121
122 m_aspect_ratio = twinCalibration.m_aspect_ratio;
123
124 return (*this);
125}
126
131{
132 LoX.clear();
133 LoY.clear();
134 LoZ.clear();
135 Lip.clear();
136 npt = 0;
137
138 return 0;
139}
140
147int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip)
148{
149 LoX.push_back(X);
150 LoY.push_back(Y);
151 LoZ.push_back(Z);
152
153 Lip.push_back(ip);
154
155 npt++;
156
157 return 0;
158}
159
165void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousMatrix &cMo_est)
166{
167 // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y)
168 // )
169 vpPose pose;
170 // the list of point is cleared (if that's not done before)
171 pose.clearPoint();
172 // we set the 3D points coordinates (in meter !) in the object/world frame
173 std::list<double>::const_iterator it_LoX = LoX.begin();
174 std::list<double>::const_iterator it_LoY = LoY.begin();
175 std::list<double>::const_iterator it_LoZ = LoZ.begin();
176 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
177
178 for (unsigned int i = 0; i < npt; i++) {
179 vpPoint P(*it_LoX, *it_LoY, *it_LoZ);
180 double x = 0, y = 0;
181 vpPixelMeterConversion::convertPoint(camera, *it_Lip, x, y);
182 P.set_x(x);
183 P.set_y(y);
184
185 pose.addPoint(P);
186 ++it_LoX;
187 ++it_LoY;
188 ++it_LoZ;
189 ++it_Lip;
190 }
191 vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
192 vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
193
194 // compute the initial pose using Lagrange method followed by a non linear
195 // minimisation method
196 // Pose by Lagrange it provides an initialization of the pose
197 double residual_lagrange = std::numeric_limits<double>::max();
198 double residual_dementhon = std::numeric_limits<double>::max();
199 try {
200 pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
201 residual_lagrange = pose.computeResidual(cMo_lagrange);
202 } catch (const vpException &e) {
203 std::cout << "Pose from Lagrange exception: " << e.getMessage() << std::endl;
204 }
205
206 // compute the initial pose using Dementhon method followed by a non linear
207 // minimisation method
208 // Pose by Dementhon it provides an initialization of the pose
209 try {
210 pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
211 residual_dementhon = pose.computeResidual(cMo_dementhon);
212 } catch (const vpException &e) {
213 std::cout << "Pose from Dementhon exception: " << e.getMessage() << std::endl;
214 }
215
216 // we keep the better initialization
217 if (residual_lagrange < residual_dementhon)
218 cMo_est = cMo_lagrange;
219 else
220 cMo_est = cMo_dementhon;
221
222 // the pose is now refined using the virtual visual servoing approach
223 // Warning: cMo needs to be initialized otherwise it may diverge
224 pose.computePose(vpPose::VIRTUAL_VS, cMo_est);
225}
226
235{
236 double residual_ = 0;
237
238 std::list<double>::const_iterator it_LoX = LoX.begin();
239 std::list<double>::const_iterator it_LoY = LoY.begin();
240 std::list<double>::const_iterator it_LoZ = LoZ.begin();
241 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
242
243 double u0 = camera.get_u0();
244 double v0 = camera.get_v0();
245 double px = camera.get_px();
246 double py = camera.get_py();
247 vpImagePoint ip;
248
249 for (unsigned int i = 0; i < npt; i++) {
250 double oX = *it_LoX;
251 double oY = *it_LoY;
252 double oZ = *it_LoZ;
253
254 double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
255 double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
256 double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
257
258 double x = cX / cZ;
259 double y = cY / cZ;
260
261 ip = *it_Lip;
262 double u = ip.get_u();
263 double v = ip.get_v();
264
265 double xp = u0 + x * px;
266 double yp = v0 + y * py;
267
268 residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
269
270 ++it_LoX;
271 ++it_LoY;
272 ++it_LoZ;
273 ++it_Lip;
274 }
275 this->residual = residual_;
276 return sqrt(residual_ / npt);
277}
286{
287 double residual_ = 0;
288
289 std::list<double>::const_iterator it_LoX = LoX.begin();
290 std::list<double>::const_iterator it_LoY = LoY.begin();
291 std::list<double>::const_iterator it_LoZ = LoZ.begin();
292 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
293
294 double u0 = camera.get_u0();
295 double v0 = camera.get_v0();
296 double px = camera.get_px();
297 double py = camera.get_py();
298 double kud = camera.get_kud();
299 double kdu = camera.get_kdu();
300
301 double inv_px = 1 / px;
302 double inv_py = 1 / px;
303 vpImagePoint ip;
304
305 for (unsigned int i = 0; i < npt; i++) {
306 double oX = *it_LoX;
307 double oY = *it_LoY;
308 double oZ = *it_LoZ;
309
310 double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
311 double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
312 double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
313
314 double x = cX / cZ;
315 double y = cY / cZ;
316
317 ip = *it_Lip;
318 double u = ip.get_u();
319 double v = ip.get_v();
320
321 double r2ud = 1 + kud * (vpMath::sqr(x) + vpMath::sqr(y));
322
323 double xp = u0 + x * px * r2ud;
324 double yp = v0 + y * py * r2ud;
325
326 residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
327
328 double r2du = (vpMath::sqr((u - u0) * inv_px) + vpMath::sqr((v - v0) * inv_py));
329
330 xp = u0 + x * px - kdu * (u - u0) * r2du;
331 yp = v0 + y * py - kdu * (v - v0) * r2du;
332
333 residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
334 ++it_LoX;
335 ++it_LoY;
336 ++it_LoZ;
337 ++it_Lip;
338 }
339 residual_ /= 2;
340
341 this->residual_dist = residual_;
342 return sqrt(residual_ / npt);
343}
344
352void vpCalibration::computeStdDeviation(double &deviation, double &deviation_dist)
353{
354 deviation = computeStdDeviation(cMo, cam);
355 deviation_dist = computeStdDeviation_dist(cMo_dist, cam_dist);
356}
357
370 vpCameraParameters &cam_est, bool verbose)
371{
372 try {
373 computePose(cam_est, cMo_est);
374 switch (method) {
375 case CALIB_LAGRANGE:
377 calibLagrange(cam_est, cMo_est);
378 } break;
379 case CALIB_VIRTUAL_VS:
382 default:
383 break;
384 }
385
386 switch (method) {
387 case CALIB_VIRTUAL_VS:
391 if (verbose) {
392 std::cout << "start calibration without distortion" << std::endl;
393 }
394 calibVVS(cam_est, cMo_est, verbose);
395 } break;
396 case CALIB_LAGRANGE:
397 default:
398 break;
399 }
400 this->cMo = cMo_est;
401 this->cMo_dist = cMo_est;
402
403 // Print camera parameters
404 if (verbose) {
405 // std::cout << "Camera parameters without distortion :" <<
406 // std::endl;
407 cam_est.printParameters();
408 }
409
410 this->cam = cam_est;
411
412 switch (method) {
415 if (verbose) {
416 std::cout << "start calibration with distortion" << std::endl;
417 }
418 calibVVSWithDistortion(cam_est, cMo_est, verbose);
419 } break;
420 case CALIB_LAGRANGE:
421 case CALIB_VIRTUAL_VS:
423 default:
424 break;
425 }
426 // Print camera parameters
427 if (verbose) {
428 // std::cout << "Camera parameters without distortion :" <<
429 // std::endl;
430 this->cam.printParameters();
431 // std::cout << "Camera parameters with distortion :" <<
432 // std::endl;
433 cam_est.printParameters();
434 }
435
436 this->cam_dist = cam_est;
437
438 this->cMo_dist = cMo_est;
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 } catch (...) {
449 throw;
450 }
451}
452
467int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std::vector<vpCalibration> &table_cal,
468 vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose)
469{
470 try {
471 unsigned int nbPose = (unsigned int)table_cal.size();
472 for (unsigned int i = 0; i < nbPose; i++) {
473 if (table_cal[i].get_npt() > 3)
474 table_cal[i].computePose(cam_est, table_cal[i].cMo);
475 }
476 switch (method) {
477 case CALIB_LAGRANGE: {
478 if (nbPose > 1) {
479 std::cout << "this calibration method is not available in" << std::endl
480 << "vpCalibration::computeCalibrationMulti()" << std::endl;
481 return -1;
482 } else {
483 table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
484 table_cal[0].cam = cam_est;
485 table_cal[0].cam_dist = cam_est;
486 table_cal[0].cMo_dist = table_cal[0].cMo;
487 }
488 break;
489 }
492 if (nbPose > 1) {
493 std::cout << "this calibration method is not available in" << std::endl
494 << "vpCalibration::computeCalibrationMulti()" << std::endl
495 << "with several images." << std::endl;
496 return -1;
497 } else {
498 table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
499 table_cal[0].cam = cam_est;
500 table_cal[0].cam_dist = cam_est;
501 table_cal[0].cMo_dist = table_cal[0].cMo;
502 }
503 calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose, table_cal[0].m_aspect_ratio);
504 break;
505 }
506 case CALIB_VIRTUAL_VS:
508 calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose, table_cal[0].m_aspect_ratio);
509 break;
510 }
511 }
512 // Print camera parameters
513 if (verbose) {
514 // std::cout << "Camera parameters without distortion :" <<
515 // std::endl;
516 cam_est.printParameters();
517 }
518
519 switch (method) {
520 case CALIB_LAGRANGE:
522 case CALIB_VIRTUAL_VS:
523 verbose = false;
524 break;
527 if (verbose)
528 std::cout << "Compute camera parameters with distortion" << std::endl;
529
530 calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose, table_cal[0].m_aspect_ratio);
531 } break;
532 }
533 // Print camera parameters
534 if (verbose) {
535 // std::cout << "Camera parameters without distortion :" <<
536 // std::endl;
537 table_cal[0].cam.printParameters();
538 // std::cout << "Camera parameters with distortion:" << std::endl;
539 cam_est.printParameters();
540 std::cout << std::endl;
541 }
542
543 if (cam_est.get_px() < 0 || cam_est.get_py() < 0 || cam_est.get_u0() < 0 || cam_est.get_v0() < 0) {
544 if (verbose) {
545 std::cout << "Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
546 }
547 return EXIT_FAILURE;
548 }
549
550 return EXIT_SUCCESS;
551 } catch (...) {
552 throw;
553 }
554}
555
563int vpCalibration::writeData(const char *filename)
564{
565 std::ofstream f(filename);
566 vpImagePoint ip;
567
568 std::list<double>::const_iterator it_LoX = LoX.begin();
569 std::list<double>::const_iterator it_LoY = LoY.begin();
570 std::list<double>::const_iterator it_LoZ = LoZ.begin();
571 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
572
573 f.precision(10);
574 f.setf(std::ios::fixed, std::ios::floatfield);
575 f << LoX.size() << std::endl;
576
577 for (unsigned int i = 0; i < LoX.size(); i++) {
578
579 double oX = *it_LoX;
580 double oY = *it_LoY;
581 double oZ = *it_LoZ;
582
583 ip = *it_Lip;
584 double u = ip.get_u();
585 double v = ip.get_v();
586
587 f << oX << " " << oY << " " << oZ << " " << u << " " << v << std::endl;
588
589 ++it_LoX;
590 ++it_LoY;
591 ++it_LoZ;
592 ++it_Lip;
593 }
594
595 f.close();
596 return 0;
597}
598
605int vpCalibration::readData(const char *filename)
606{
607 vpImagePoint ip;
608 std::ifstream f;
609 f.open(filename);
610 if (!f.fail()) {
611 unsigned int n;
612 f >> n;
613 std::cout << "There are " << n << " point on the calibration grid " << std::endl;
614
615 clearPoint();
616
617 if (n > 100000)
618 throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
619
620 for (unsigned int i = 0; i < n; i++) {
621 double x, y, z, u, v;
622 f >> x >> y >> z >> u >> v;
623 std::cout << x << " " << y << " " << z << " " << u << " " << v << std::endl;
624 ip.set_u(u);
625 ip.set_v(v);
626 addPoint(x, y, z, ip);
627 }
628
629 f.close();
630 return 0;
631 } else {
632 return -1;
633 }
634}
651int vpCalibration::readGrid(const char *filename, unsigned int &n, std::list<double> &oX, std::list<double> &oY,
652 std::list<double> &oZ, bool verbose)
653{
654 try {
655 std::ifstream f;
656 f.open(filename);
657 if (!f.fail()) {
658
659 f >> n;
660 if (verbose)
661 std::cout << "There are " << n << " points on the calibration grid " << std::endl;
662 int no_pt;
663 double x, y, z;
664
665 // clear the list
666 oX.clear();
667 oY.clear();
668 oZ.clear();
669
670 if (n > 100000)
671 throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
672
673 for (unsigned int i = 0; i < n; i++) {
674 f >> no_pt >> x >> y >> z;
675 if (verbose) {
676 std::cout << no_pt << std::endl;
677 std::cout << x << " " << y << " " << z << std::endl;
678 }
679 oX.push_back(x);
680 oY.push_back(y);
681 oZ.push_back(z);
682 }
683
684 f.close();
685 } else {
686 return -1;
687 }
688 } catch (...) {
689 return -1;
690 }
691 return 0;
692}
693
704int vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
705{
706
707 for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++it) {
708 vpImagePoint ip = *it;
709 if (subsampling_factor > 1.) {
710 ip.set_u(ip.get_u() / subsampling_factor);
711 ip.set_v(ip.get_v() / subsampling_factor);
712 }
713 vpDisplay::displayCross(I, ip, 12, color, thickness);
714 }
715 return 0;
716}
717
728int vpCalibration::displayGrid(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
729{
730 double u0_dist = cam_dist.get_u0() / subsampling_factor;
731 double v0_dist = cam_dist.get_v0() / subsampling_factor;
732 double px_dist = cam_dist.get_px() / subsampling_factor;
733 double py_dist = cam_dist.get_py() / subsampling_factor;
734 double kud_dist = cam_dist.get_kud();
735 // double kdu_dist = cam_dist.get_kdu() ;
736
737 // double u0 = cam.get_u0() ;
738 // double v0 = cam.get_v0() ;
739 // double px = cam.get_px() ;
740 // double py = cam.get_py() ;
741
742 std::list<double>::const_iterator it_LoX = LoX.begin();
743 std::list<double>::const_iterator it_LoY = LoY.begin();
744 std::list<double>::const_iterator it_LoZ = LoZ.begin();
745
746 for (unsigned int i = 0; i < npt; i++) {
747 double oX = *it_LoX;
748 double oY = *it_LoY;
749 double oZ = *it_LoZ;
750
751 // double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
752 // double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
753 // double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
754
755 // double x = cX/cZ ;
756 // double y = cY/cZ ;
757
758 // double xp = u0 + x*px ;
759 // double yp = v0 + y*py ;
760
761 // vpDisplay::displayCross(I,(int)vpMath::round(yp),
762 // (int)vpMath::round(xp),
763 // 5,col) ;
764
765 double cX = oX * cMo_dist[0][0] + oY * cMo_dist[0][1] + oZ * cMo_dist[0][2] + cMo_dist[0][3];
766 double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + oZ * cMo_dist[1][2] + cMo_dist[1][3];
767 double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + oZ * cMo_dist[2][2] + cMo_dist[2][3];
768
769 double x = cX / cZ;
770 double y = cY / cZ;
771
772 double r2 = 1 + kud_dist * (vpMath::sqr(x) + vpMath::sqr(y));
773
774 vpImagePoint ip;
775 ip.set_u(u0_dist + x * px_dist * r2);
776 ip.set_v(v0_dist + y * py_dist * r2);
777
778 vpDisplay::displayCross(I, ip, 6, color, thickness);
780
781 // std::cout << oX << " " << oY << " " <<oZ << std::endl ;
782 // I.getClick() ;
783 ++it_LoX;
784 ++it_LoY;
785 ++it_LoZ;
786 }
787 return 0;
788}
789
795void vpCalibration::setAspectRatio(double aspect_ratio)
796{
797 if (aspect_ratio > 0.) {
798 m_aspect_ratio = aspect_ratio;
799 }
800}
Tools for perspective camera calibration.
Definition: vpCalibration.h:72
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
get the number of points
double m_aspect_ratio
Fix aspect ratio (px/py)
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
Position of the camera in end-effector frame using camera parameters with distorsion.
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
vpCalibration & operator=(const vpCalibration &twinCalibration)
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Definition: vpCalibration.h:93
vpCameraParameters cam_dist
projection model with distortion
int writeData(const char *filename)
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
void setAspectRatio(double aspect_ratio)
vpHomogeneousMatrix eMc
Position of the camera in end-effector frame using camera parameters without distorsion.
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:95
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
virtual ~vpCalibration()
int clearPoint()
Suppress all the point in the array of point.
int displayGrid(vpImage< unsigned char > &I, vpColor color=vpColor::yellow, unsigned int thickness=1, int subsampling_factor=1)
@ CALIB_LAGRANGE_VIRTUAL_VS
Definition: vpCalibration.h:85
@ CALIB_LAGRANGE_VIRTUAL_VS_DIST
Definition: vpCalibration.h:88
static int readGrid(const char *filename, unsigned int &n, std::list< double > &oX, std::list< double > &oY, std::list< double > &oZ, bool verbose=false)
vpCameraParameters cam
projection model without distortion
Definition: vpCalibration.h:98
int readData(const char *filename)
Generic class defining intrinsic camera parameters.
double get_kdu() const
double get_kud() const
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
const char * getMessage() const
Definition: vpException.cpp:90
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
double get_u() const
Definition: vpImagePoint.h:262
void set_u(double u)
Definition: vpImagePoint.h:225
void set_v(double v)
Definition: vpImagePoint.h:236
double get_v() const
Definition: vpImagePoint.h:273
static double sqr(double x)
Definition: vpMath.h:116
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:82
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:149
@ DEMENTHON
Definition: vpPose.h:86
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:336
void clearPoint()
Definition: vpPose.cpp:134
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374