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>
66 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
67 std::cout <<
"Invalid position:" << std::endl;
68 std::cout << cMo << std::endl;
69 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
79 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
80 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
81 double A,B,C,Ad,Bd,Cd;
84 std::vector<vpPoint> vec_p,vec_p_d;
90 cMoToABC(cdMo,Ad,Bd,Cd);
92 for (
int i = 0 ; i < nbpoints ; i++){
103 cur.fromVector(vec_p);
119 mdb_dst.updateAll(dst);
121 fmdb_dst.updateAll(Ad,Bd,Cd);
129 task.
addFeature(fmdb_cur.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
130 task.
addFeature(fmdb_cur.getFeatureAn(),fmdb_dst.getFeatureAn());
133 task.
addFeature(fmdb_cur.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),
135 task.
addFeature(fmdb_cur.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
142 float sampling_time = 0.010f;
150 for (
int i = 0 ; i < nbpoints ; i++){
158 cur.fromVector(vec_p);
160 mdb_cur.updateAll(cur);
162 fmdb_cur.updateAll(A,B,C);
169 }
while(( task.
getError() ).sumSquare()>0.005);
170 std::cout <<
"final error=" << ( task.
getError() ).sumSquare() << std::endl;