50 #include <visp/vpCalibration.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpPose.h>
53 #include <visp/vpPixelMeterConversion.h>
55 double vpCalibration::threshold = 1e-10f;
56 unsigned int vpCalibration::nbIterMax = 4000;
57 double vpCalibration::gain = 0.25;
94 npt = twinCalibration.npt ;
95 LoX = twinCalibration.LoX ;
96 LoY = twinCalibration.LoY ;
97 LoZ = twinCalibration.LoZ ;
98 Lip = twinCalibration.Lip ;
100 residual = twinCalibration.residual;
101 cMo = twinCalibration.
cMo;
102 residual_dist = twinCalibration.residual_dist;
105 cam = twinCalibration.
cam ;
108 rMe = twinCalibration.
rMe;
110 eMc = twinCalibration.
eMc;
161 std::list<double>::const_iterator it_LoX = LoX.begin();
162 std::list<double>::const_iterator it_LoY = LoY.begin();
163 std::list<double>::const_iterator it_LoZ = LoZ.begin();
164 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
166 for (
unsigned int i =0 ; i < npt ; i++)
197 if (residual_lagrange < residual_dementhon)
219 double residual = 0 ;
221 std::list<double>::const_iterator it_LoX = LoX.begin();
222 std::list<double>::const_iterator it_LoY = LoY.begin();
223 std::list<double>::const_iterator it_LoZ = LoZ.begin();
224 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
226 double u0 = cam.
get_u0() ;
227 double v0 = cam.
get_v0() ;
228 double px = cam.
get_px() ;
229 double py = cam.
get_py() ;
232 for (
unsigned int i =0 ; i < npt ; i++)
239 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
240 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
241 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
247 double u = ip.
get_u() ;
248 double v = ip.
get_v() ;
250 double xp = u0 + x*px;
251 double yp = v0 + y*py;
260 this->residual = residual ;
261 return sqrt(residual/npt) ;
274 double residual = 0 ;
276 std::list<double>::const_iterator it_LoX = LoX.begin();
277 std::list<double>::const_iterator it_LoY = LoY.begin();
278 std::list<double>::const_iterator it_LoZ = LoZ.begin();
279 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
281 double u0 = cam.
get_u0() ;
282 double v0 = cam.
get_v0() ;
283 double px = cam.
get_px() ;
284 double py = cam.
get_py() ;
288 double inv_px = 1/px;
289 double inv_py = 1/px;
292 for (
unsigned int i =0 ; i < npt ; i++)
299 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
300 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
301 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
307 double u = ip.
get_u() ;
308 double v = ip.
get_v() ;
312 double xp = u0 + x*px*r2ud;
313 double yp = v0 + y*py*r2ud;
319 xp = u0 + x*px - kdu*(u-u0)*r2du;
320 yp = v0 + y*py - kdu*(v-v0)*r2du;
330 this->residual_dist = residual;
331 return sqrt(residual/npt) ;
365 computePose(cam,cMo);
371 calibLagrange(cam, cMo);
388 if (verbose){std::cout <<
"start calibration without distortion"<< std::endl;}
389 calibVVS(cam, cMo, verbose);
412 if (verbose){std::cout <<
"start calibration with distortion"<< std::endl;}
413 calibVVSWithDistortion(cam, cMo, verbose);
459 for(
unsigned int i=0;i<nbPose;i++){
461 table_cal[i].computePose(cam,table_cal[i].cMo);
466 std::cout <<
"this calibration method is not available in" << std::endl
467 <<
"vpCalibration::computeCalibrationMulti()" << std::endl;
471 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
480 std::cout <<
"this calibration method is not available in" << std::endl
481 <<
"vpCalibration::computeCalibrationMulti()" << std::endl
482 <<
"with several images." << std::endl;
486 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
494 calibVVSMulti(nbPose, table_cal, cam, verbose);
515 std::cout <<
"Compute camera parameters with distortion"<<std::endl;
517 calibVVSWithDistortionMulti(nbPose, table_cal, cam, verbose);
527 std::cout<<std::endl;
564 for(
unsigned int i=0;i<nbPose;i++){
565 table_cMo[i] = table_cal[i].
cMo;
566 table_cMo_dist[i] = table_cal[i].
cMo_dist;
567 table_rMe[i] = table_cal[i].
rMe;
573 delete [] table_cMo_dist;
579 vpERROR_TRACE(
"Three images are needed to compute Tsai calibration !\n");
598 std::ofstream f(filename) ;
601 std::list<double>::const_iterator it_LoX = LoX.begin();
602 std::list<double>::const_iterator it_LoY = LoY.begin();
603 std::list<double>::const_iterator it_LoZ = LoZ.begin();
604 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
607 f.setf(std::ios::fixed,std::ios::floatfield);
608 f << LoX.size() << std::endl ;
610 for (
unsigned int i =0 ; i < LoX.size() ; i++)
618 double u = ip.
get_u() ;
619 double v = ip.
get_v() ;
621 f << oX <<
" " << oY <<
" " << oZ <<
" " << u <<
" " << v <<std::endl ;
648 std::cout <<
"There are "<< n <<
" point on the calibration grid " << std::endl ;
651 for (
int i=0 ; i < n ; i++)
653 double x, y, z, u, v ;
654 f >> x >> y >> z >> u >> v ;
655 std::cout << x <<
" "<<y <<
" "<<z<<
" "<<u<<
" "<<v <<std::endl ;
685 std::list<double> &oX, std::list<double> &oY, std::list<double> &oZ,
bool verbose)
694 std::cout <<
"There are "<< n <<
" points on the calibration grid " << std::endl ;
703 for (
unsigned int i=0 ; i < n ; i++)
705 f >> no_pt >> x >> y >> z ;
707 std::cout << no_pt <<std::endl ;
708 std::cout << x <<
" "<< y <<
" "<< z <<std::endl ;
721 catch(...){
return -1;}
733 unsigned int thickness)
736 for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++ it) {
750 unsigned int thickness)
764 std::list<double>::const_iterator it_LoX = LoX.begin();
765 std::list<double>::const_iterator it_LoY = LoY.begin();
766 std::list<double>::const_iterator it_LoZ = LoZ.begin();
768 for (
unsigned int i =0 ; i < npt ; i++)
775 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
776 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
777 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
789 cY = oX*cMo_dist[1][0]+oY*cMo_dist[1][1]+oZ*cMo_dist[1][2]+cMo_dist[1][3];
790 cZ = oX*cMo_dist[2][0]+oY*cMo_dist[2][1]+oZ*cMo_dist[2][2]+cMo_dist[2][3];
798 ip.
set_u( u0_dist + x*px_dist*r2 );
799 ip.set_v( v0_dist + y*py_dist*r2 );
813 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
843 std::cout <<
"There are "<< n <<
" points on the calibration grid " << std::endl ;
850 for (
unsigned int i=0 ; i < n ; i++)
852 f >> no_pt >> x >> y >> z ;
854 std::cout << no_pt <<std::endl ;
855 std::cout << x <<
" "<< y <<
" "<< z <<std::endl ;
868 catch(...){
return -1;}
872 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS
vpCalibration()
Constructor.
static int computeCalibrationTsai(unsigned int nbPose, vpCalibration table_cal[], vpHomogeneousMatrix &eMc, vpHomogeneousMatrix &eMc_dist)
Compute the multi-image calibration of effector-camera from R. Tsai and R. LorenzTsai.
int displayGrid(vpImage< unsigned char > &I, vpColor color=vpColor::yellow, unsigned int thickness=1)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Provide simple list management.
int readData(const char *filename)
Class to define colors available for display functionnalities.
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
void set_x(const double x)
Set the point x coordinate in the image plane.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
Tools for perspective camera calibration.
virtual ~vpCalibration()
Destructor.
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, bool verbose=false)
double computeStdDeviation_dist(vpHomogeneousMatrix &cMo, vpCameraParameters &cam)
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Class that defines what is a point.
void operator=(vpCalibration &twinCalibration)
= operator
int displayData(vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1)
static void calibrationTsai(unsigned int nbPose, vpHomogeneousMatrix cMo[], vpHomogeneousMatrix rMe[], vpHomogeneousMatrix &eMc)
calibration method of effector-camera from R. Tsai and R. Lorenz
int clearPoint()
Suppress all the point in the array of point.
void set_u(const double u)
static double sqr(double x)
void front(void)
Position the current element on the first element of the list.
vpHomogeneousMatrix cMo_dist
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void set_v(const double v)
Class used for pose computation from N points (pose from point only).
Generic class defining intrinsic camera parameters.
void addRight(const type &el)
add a new element in the list, at the right of the current one
vpHomogeneousMatrix eMc_dist
void set_y(const double y)
Set the point y coordinate in the image plane.
static int computeCalibrationMulti(vpCalibrationMethodType method, unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam, bool verbose=false)
unsigned int get_npt() const
get the number of points
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
vpCameraParameters cam_dist
projection model with distortion
int writeData(const char *filename)
vpHomogeneousMatrix eMc
position of the camera in relation to the effector
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void computeStdDeviation(double &deviation, double &deviation_dist)
vpCameraParameters cam
projection model without distortion
void addPoint(const vpPoint &P)
Add a new point in this array.
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
static int readGrid(const char *filename, unsigned int &n, std::list< double > &oX, std::list< double > &oY, std::list< double > &oZ, bool verbose=false)
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
void clearPoint()
suppress all the point in the array of point