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){
Definition of the vpMatrix class.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
static double getAlpha(vpMomentObject &objec)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
Class for generic objects.
void track(const vpHomogeneousMatrix &cMo)
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
Class that defines what is a point.
vpMatrix computeInteractionMatrix()
compute the interaction matrix related to the set of visual features
void kill()
destruction (memory deallocation if required)
vpColVector computeControlLaw()
compute the desired control law
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)
Set the type of the interaction matrix (current, mean, desired, user).
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
This class defines the container for a plane geometrical structure.
Class required to compute the visual servoing control law descbribed in and .
vpFeatureMomentAreaNormalized & getFeatureAn()
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
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...