48 #include <visp/vpPoint.h>
51 #include <visp/vpMomentObject.h>
52 #include <visp/vpMomentCommon.h>
53 #include <visp/vpFeatureMomentCommon.h>
54 #include <visp/vpServo.h>
55 #include <visp/vpRobotCamera.h>
56 #include <visp/vpPlane.h>
57 #include <visp/vpException.h>
70 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
71 std::cout <<
"Invalid position:" << std::endl;
72 std::cout << cMo << std::endl;
73 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
84 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
85 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
86 double A,B,C,Ad,Bd,Cd;
89 std::vector<vpPoint> vec_p,vec_p_d;
95 cMoToABC(cdMo,Ad,Bd,Cd);
97 for (
int i = 0 ; i < nbpoints ; i++){
103 vec_p_d.push_back(p);
108 cur.fromVector(vec_p);
123 mdb_dst.updateAll(dst);
125 fmdb_dst.updateAll(Ad,Bd,Cd);
133 task.
addFeature(fmdb_cur.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
134 task.
addFeature(fmdb_cur.getFeatureAn(),fmdb_dst.getFeatureAn());
137 task.
addFeature(fmdb_cur.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),
139 task.
addFeature(fmdb_cur.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
146 float sampling_time = 0.010f;
154 for (
int i = 0 ; i < nbpoints ; i++){
162 cur.fromVector(vec_p);
164 mdb_cur.updateAll(cur);
166 fmdb_cur.updateAll(A,B,C);
173 }
while(( task.
getError() ).sumSquare()>0.005);
174 std::cout <<
"final error=" << ( task.
getError() ).sumSquare() << std::endl;
178 std::cout <<
"Catch an exception: " << e << std::endl;
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
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 track(const vpHomogeneousMatrix &cMo)
virtual vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Compute the error between two visual features from a subset of the possible features.
static double measureTimeMs()
static int wait(double t0, double t)
Class that defines what is a point.
class that defines what is a visual feature
virtual void setSamplingTime(const double &delta_t)
Functionality computation for in-plane rotation moment feature : computes the interaction matrix asso...
vpColVector getError() const
static unsigned int selectC4()
vpColVector computeControlLaw()
void changeFrame(const vpHomogeneousMatrix &cMo)
Class that defines the simplest robot: a free flying camera.
static std::vector< double > getMu3(vpMomentObject &object)
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class defines the container for a plane geometrical structure.
static unsigned int selectC6()
void setServo(const vpServoType &servo_type)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
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...