48 #include <visp/vpDebug.h>
49 #include <visp/vpConfig.h>
51 #include <visp/vpHomogeneousMatrix.h>
52 #include <visp/vpMomentObject.h>
53 #include <visp/vpMomentDatabase.h>
54 #include <visp/vpMomentCommon.h>
55 #include <visp/vpFeatureMomentCommon.h>
56 #include <visp/vpDisplayX.h>
57 #include <visp/vpDisplayGTK.h>
58 #include <visp/vpDisplayGDI.h>
59 #include <visp/vpCameraParameters.h>
60 #include <visp/vpIoTools.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpHomogeneousMatrix.h>
63 #include <visp/vpServo.h>
64 #include <visp/vpDebug.h>
65 #include <visp/vpFeatureBuilder.h>
66 #include <visp/vpFeaturePoint.h>
67 #include <visp/vpSimulatorAfma6.h>
68 #include <visp/vpPlane.h>
81 void execute(
unsigned int nbIter);
84 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C);
87 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
91 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
92 std::cout <<
"You should install pthread third-party library." << std::endl;
95 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
98 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
99 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
115 #if defined VISP_HAVE_X11
117 #elif defined VISP_HAVE_OPENCV
119 #elif defined VISP_HAVE_GDI
121 #elif defined VISP_HAVE_D3D9
123 #elif defined VISP_HAVE_GTK
153 vector<vpPoint> src_pts;
154 vector<vpPoint> dst_pts;
156 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
157 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
160 for (
int i = 0 ; i < nbpoints ; i++){
164 src_pts.push_back(p);
169 for (
int i = 0 ; i < nbpoints ; i++){
173 dst_pts.push_back(p);
182 double A;
double B;
double C;
183 double Ad;
double Bd;
double Cd;
190 _planeToABC(pl,A,B,C);
194 _planeToABC(pl,Ad,Bd,Cd);
229 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
230 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
232 vector<vpPoint> cur_pts;
234 for (
int i = 0 ; i < nbpoints ; i++){
238 cur_pts.push_back(p);
250 displayInt.
init(Iint,700,0,
"Visual servoing with moments") ;
259 void execute(
unsigned int nbIter){
265 vpTRACE(
"Display task information " ) ;
269 robot.getInternalView(Iint);
274 while(iter++<nbIter ){
277 cMo = robot.get_cMo();
283 _planeToABC(pl,A,B,C);
293 robot.getInternalView(Iint);
305 _error = ( task.
getError() ).sumSquare();
310 vpTRACE(
"\n\nClick in the internal view window to end...");
315 delete featureMoments;
316 delete featureMomentsDes;
341 void _planeToABC(
vpPlane& pl,
double& A,
double& B,
double& C){
342 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
343 std::cout <<
"Invalid position:" << std::endl;
344 std::cout << cMo << std::endl;
345 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
359 removeJointLimits(robot);
371 double error(){
return _error;}