47 #include <visp/vpDebug.h>
48 #include <visp/vpConfig.h>
50 #include <visp/vpHomogeneousMatrix.h>
51 #include <visp/vpMomentObject.h>
52 #include <visp/vpMomentDatabase.h>
53 #include <visp/vpMomentCommon.h>
54 #include <visp/vpFeatureMomentCommon.h>
55 #include <visp/vpDisplayX.h>
56 #include <visp/vpDisplayGTK.h>
57 #include <visp/vpDisplayGDI.h>
58 #include <visp/vpCameraParameters.h>
59 #include <visp/vpIoTools.h>
60 #include <visp/vpMath.h>
61 #include <visp/vpHomogeneousMatrix.h>
62 #include <visp/vpServo.h>
63 #include <visp/vpDebug.h>
64 #include <visp/vpFeatureBuilder.h>
65 #include <visp/vpFeaturePoint.h>
66 #include <visp/vpSimulatorAfma6.h>
67 #include <visp/vpPlane.h>
80 void execute(
unsigned int nbIter);
83 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
86 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
90 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
91 std::cout <<
"You should install pthread third-party library." << std::endl;
94 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
97 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
98 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
114 #if defined VISP_HAVE_X11
116 #elif defined VISP_HAVE_OPENCV
118 #elif defined VISP_HAVE_GDI
120 #elif defined VISP_HAVE_D3D9
122 #elif defined VISP_HAVE_GTK
151 vector<vpPoint> src_pts;
152 vector<vpPoint> dst_pts;
154 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
155 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
158 for (
int i = 0 ; i < nbpoints ; i++){
162 src_pts.push_back(p);
167 for (
int i = 0 ; i < nbpoints ; i++){
171 dst_pts.push_back(p);
179 double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
180 double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
182 vector<vpPoint> cur_pts;
184 for (
int i = 0 ; i < nbpoints ; i++){
188 cur_pts.push_back(p);
198 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
209 double A;
double B;
double C;
210 double Ad;
double Bd;
double Cd;
217 _planeToABC(pl,A,B,C);
221 _planeToABC(pl,Ad,Bd,Cd);
253 void execute(
unsigned int nbIter){
259 vpTRACE(
"Display task information " ) ;
263 robot.getInternalView(Iint);
268 while(iter++<nbIter ){
272 cMo = robot.get_cMo();
278 _planeToABC(pl,A,B,C);
288 robot.getInternalView(Iint);
299 _error = ( task.
getError() ).sumSquare();
304 vpTRACE(
"\n\nClick in the internal view window to end...");
309 delete featureMoments;
310 delete featureMomentsDes;
314 double error(){
return _error;}
336 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
337 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
338 std::cout <<
"Invalid position:" << std::endl;
339 std::cout << cMo << std::endl;
340 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
354 removeJointLimits(robot);