41 #include <visp/vpMomentObject.h>
42 #include <visp/vpMomentDatabase.h>
43 #include <visp/vpMomentCommon.h>
44 #include <visp/vpFeatureMomentCommon.h>
45 #include <visp/vpHomogeneousMatrix.h>
46 #include <visp/vpServo.h>
47 #include <visp/vpDebug.h>
48 #include <visp/vpPlane.h>
58 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
59 int test(
double x,
double y,
double z,
double alpha);
77 for(
double i=-0.2;i<0.2;i+=0.1){
78 for(
double j=-0.2;j<0.2;j+=0.1){
80 for(
double l=0.5;l<1.5;l+=0.1){
93 int test(
double x,
double y,
double z,
double alpha){
103 if(fabs(mat[0][0]-(-1)) > numeric_limits<double>::epsilon()*1e10)
return -1;
104 if(fabs(mat[0][1]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
105 if(fabs(mat[0][2]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
107 if(fabs(mat[1][0]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
108 if(fabs(mat[1][1]-(-1)) > numeric_limits<double>::epsilon()*1e10)
return -1;
109 if(fabs(mat[1][2]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
111 if(fabs(mat[2][0]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
112 if(fabs(mat[2][1]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
113 if(fabs(mat[2][2]-(-1)) > numeric_limits<double>::epsilon()*1e10)
return -1;
114 if(fabs(mat[2][5]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
116 if(fabs(mat[3][0]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
117 if(fabs(mat[3][1]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
118 if(fabs(mat[3][2]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
119 if(fabs(mat[3][5]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
121 if(fabs(mat[4][0]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
122 if(fabs(mat[4][1]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
123 if(fabs(mat[4][2]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
124 if(fabs(mat[4][5]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
126 if(fabs(mat[5][0]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
127 if(fabs(mat[5][1]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
128 if(fabs(mat[5][2]-(0)) > numeric_limits<double>::epsilon()*1e10)
return -1;
129 if(fabs(mat[5][5]-(-1)) > numeric_limits<double>::epsilon()*1e10)
return -1;
139 vector<vpPoint> src_pts;
140 vector<vpPoint> dst_pts;
142 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
143 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
146 for (
int i = 0 ; i < nbpoints ; i++){
150 src_pts.push_back(p);
155 for (
int i = 0 ; i < nbpoints ; i++){
159 dst_pts.push_back(p);
181 double A;
double B;
double C;
182 double Ad;
double Bd;
double Cd;
189 _planeToABC(pl,A,B,C);
193 _planeToABC(pl,Ad,Bd,Cd);
231 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){