37 #include <visp3/core/vpMomentObject.h> 38 #include <visp3/core/vpMomentDatabase.h> 39 #include <visp3/core/vpMomentCommon.h> 40 #include <visp3/visual_features/vpFeatureMomentCommon.h> 41 #include <visp3/core/vpHomogeneousMatrix.h> 42 #include <visp3/vs/vpServo.h> 43 #include <visp3/core/vpDebug.h> 44 #include <visp3/core/vpPlane.h> 55 void planeToABC(
const vpPlane& pl,
double& A,
double& B,
double& C);
56 int test(
double x,
double y,
double z,
double alpha);
63 for(
double i=-0.2;i<0.2;i+=0.1){
64 for(
double j=-0.2;j<0.2;j+=0.1){
66 for(
double l=0.5;l<1.5;l+=0.1){
77 std::cout <<
"Catch an exception: " << e << std::endl;
82 int test(
double x,
double y,
double z,
double alpha){
93 initScene(cMo, cdMo, src, dst);
95 vpMatrix mat = execute(cMo, cdMo, src, dst);
97 if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
98 if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
99 if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
101 if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
102 if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
103 if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
105 if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
106 if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
107 if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
108 if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
110 if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
111 if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
112 if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
113 if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
115 if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
116 if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
117 if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
118 if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
120 if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
121 if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
122 if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
123 if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10)
return -1;
131 std::vector<vpPoint> src_pts;
132 std::vector<vpPoint> dst_pts;
134 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
135 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
138 for (
int i = 0 ; i < nbpoints ; i++){
141 src_pts.push_back(p);
146 for (
int i = 0 ; i < nbpoints ; i++){
149 dst_pts.push_back(p);
164 double A;
double B;
double C;
165 double Ad;
double Bd;
double Cd;
172 planeToABC(pl,A,B,C);
176 planeToABC(pl,Ad,Bd,Cd);
214 void planeToABC(
const vpPlane& pl,
double& A,
double& B,
double& C)
Implementation of a matrix and operations on matrices.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
void setABCD(const double a, const double b, const double c, const double d)
Class for generic objects.
Class that defines what is a point.
vpMatrix computeInteractionMatrix()
vpColVector computeControlLaw()
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
void extract(vpRotationMatrix &R) const
vpServoIteractionMatrixType
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
void setType(vpObjectType input_type)
This class defines the container for a plane geometrical structure.
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Class that consider the case of a translation vector.