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.
void extract(vpRotationMatrix &R) const
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)
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.