51 #include <visp/vpCameraParameters.h>
52 #include <visp/vpCylinder.h>
53 #include <visp/vpDisplayOpenCV.h>
54 #include <visp/vpDisplayX.h>
55 #include <visp/vpDisplayGTK.h>
56 #include <visp/vpDisplayGDI.h>
57 #include <visp/vpDisplayD3D.h>
58 #include <visp/vpFeatureBuilder.h>
59 #include <visp/vpHomogeneousMatrix.h>
60 #include <visp/vpImage.h>
61 #include <visp/vpImageIo.h>
62 #include <visp/vpIoTools.h>
63 #include <visp/vpMath.h>
64 #include <visp/vpParseArgv.h>
65 #include <visp/vpRobotCamera.h>
66 #include <visp/vpServo.h>
67 #include <visp/vpTime.h>
68 #include <visp/vpVelocityTwistMatrix.h>
69 #include <visp/vpWireFrameSimulator.h>
71 #define GETOPTARGS "dh"
73 #ifdef VISP_HAVE_DISPLAY
83 void usage(
const char *name,
const char *badparam)
86 Demonstration of the wireframe simulator with a simple visual servoing.\n\
88 The visual servoing consists in bringing the camera at a desired position\n\
91 The visual features used to compute the pose of the camera and \n\
92 thus the control law are two lines. These features are computed thanks \n\
93 to the equation of a cylinder.\n\
95 This demonstration explains also how to move the object around a world \n\
96 reference frame. Here, the movment is a rotation around the x and y axis \n\
97 at a given distance from the world frame. In fact the object trajectory \n\
98 is on a sphere whose center is the origin of the world frame.\n\
101 %s [-d] [-h]\n", name);
106 Turn off the display.\n\
112 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
127 bool getOptions(
int argc,
const char **argv,
bool &display)
134 case 'd': display =
false;
break;
135 case 'h': usage(argv[0], NULL);
return false;
break;
138 usage(argv[0], optarg);
143 if ((c == 1) || (c == -1)) {
145 usage(argv[0], NULL);
146 std::cerr <<
"ERROR: " << std::endl;
147 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
156 main(
int argc,
const char ** argv)
158 bool opt_display =
true;
161 if (getOptions(argc, argv, opt_display) ==
false) {
168 #if defined VISP_HAVE_X11
170 #elif defined VISP_HAVE_OPENCV
172 #elif defined VISP_HAVE_GDI
174 #elif defined VISP_HAVE_D3D9
176 #elif defined VISP_HAVE_GTK
185 display[0].
init(Iint, 100, 100,
"The internal view") ;
186 display[1].
init(Iext, 100, 100,
"The first external view") ;
203 float sampling_time = 0.040f;
229 cylinder.track(cdMo);
246 for (
int i = 0 ; i < 2 ; i++)
292 std::cout <<
"Click on a display" << std::endl;
311 double vitesse = 0.3;
330 cylinder.track(cMo) ;
337 if ( iter%tempo < 200 && iter%tempo >= 0)
340 e1[0] = -fabs(vitesse) ;
342 rapport = -vitesse/proj_e1[0];
347 if ( iter%tempo < 300 && iter%tempo >= 200)
350 e2[1] = -fabs(vitesse) ;
352 rapport = -vitesse/proj_e2[1];
357 if ( iter%tempo < 500 && iter%tempo >= 300)
360 e1[0] = -fabs(vitesse) ;
362 rapport = vitesse/proj_e1[0];
367 if ( iter%tempo < 600 && iter%tempo >= 500)
370 e2[1] = -fabs(vitesse) ;
372 rapport = vitesse/proj_e2[1];
404 std::cout <<
"|| s - s* || = " << (task.
getError() ).sumSquare() <<std::endl ;
416 vpERROR_TRACE(
"You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");