52 #include <visp/vpCameraParameters.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/vpGenericFeature.h>
60 #include <visp/vpHomogeneousMatrix.h>
61 #include <visp/vpImage.h>
62 #include <visp/vpImageIo.h>
63 #include <visp/vpIoTools.h>
64 #include <visp/vpMath.h>
65 #include <visp/vpParseArgv.h>
66 #include <visp/vpRobotCamera.h>
67 #include <visp/vpServo.h>
68 #include <visp/vpSphere.h>
69 #include <visp/vpTime.h>
70 #include <visp/vpVelocityTwistMatrix.h>
71 #include <visp/vpWireFrameSimulator.h>
73 #define GETOPTARGS "dh"
75 #ifdef VISP_HAVE_DISPLAY
85 void usage(
const char *name,
const char *badparam)
88 Demonstration of the wireframe simulator with a simple visual servoing.\n\
90 The visual servoing consists in bringing the camera at a desired position from the object.\n\
92 The visual features used to compute the pose of the camera and thus the control law are special moments computed with the sphere's parameters.\n\
95 %s [-d] [-h]\n", name);
100 Turn off the display.\n\
106 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
120 bool getOptions(
int argc,
const char **argv,
bool &display)
127 case 'd': display =
false;
break;
128 case 'h': usage(argv[0], NULL);
return false;
break;
131 usage(argv[0], optarg);
136 if ((c == 1) || (c == -1)) {
138 usage(argv[0], NULL);
139 std::cerr <<
"ERROR: " << std::endl;
140 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
162 double gx = sphere.
get_x();
163 double gy = sphere.
get_y();
169 if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
174 double sx = gx*h2/(sqrt(h2+1));
175 double sy = gy*h2/(sqrt(h2+1));
176 double sz = sqrt(h2+1);
191 L[0][0] = -1/sphere.
getR();
192 L[1][1] = -1/sphere.
getR();
193 L[2][2] = -1/sphere.
getR();
202 for(
unsigned int i = 0; i < 3; i++)
203 for(
unsigned int j = 0; j < 3; j++)
204 L[i][j+3] = sk[i][j];
209 main(
int argc,
const char ** argv)
211 bool opt_display =
true;
214 if (getOptions(argc, argv, opt_display) ==
false) {
222 #if defined VISP_HAVE_X11
224 #elif defined VISP_HAVE_OPENCV
226 #elif defined VISP_HAVE_GDI
228 #elif defined VISP_HAVE_D3D9
230 #elif defined VISP_HAVE_GTK
239 display[0].
init(Iint, 100, 100,
"The internal view") ;
240 display[1].
init(Iext1, 100, 100,
"The first external view") ;
241 display[2].
init(Iext2, 100, 100,
"The second external view") ;
261 float sampling_time = 0.040f;
289 computeVisualFeatures(sphere, s);
295 computeVisualFeatures(sphere, sd);
298 computeInteractionMatrix(sd,sphere,L);
299 sd.setInteractionMatrix(L);
366 std::cout <<
"Click on a display" << std::endl;
396 computeVisualFeatures(sphere, s);
433 std::cout <<
"|| s - s* || = " << ( task.
getError() ).sumSquare() <<std::endl ;
445 vpERROR_TRACE(
"You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");