ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-ibvs-4pts-plotter.cpp
1 
2 #include <visp/vpFeatureBuilder.h>
3 #include <visp/vpServo.h>
4 #include <visp/vpSimulatorCamera.h>
5 #include <visp/vpPlot.h>
6 
7 int main()
8 {
9  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
10  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
11 
12  vpPoint point[4] ;
13  point[0].setWorldCoordinates(-0.1,-0.1, 0);
14  point[1].setWorldCoordinates( 0.1,-0.1, 0);
15  point[2].setWorldCoordinates( 0.1, 0.1, 0);
16  point[3].setWorldCoordinates(-0.1, 0.1, 0);
17 
18  vpServo task ;
21  task.setLambda(0.5);
22 
23  vpFeaturePoint p[4], pd[4] ;
24  for (int i = 0 ; i < 4 ; i++) {
25  point[i].track(cdMo);
26  vpFeatureBuilder::create(pd[i], point[i]);
27  point[i].track(cMo);
28  vpFeatureBuilder::create(p[i], point[i]);
29  task.addFeature(p[i], pd[i]);
30  }
31 
32  vpHomogeneousMatrix wMc, wMo;
33  vpSimulatorCamera robot;
34  robot.setSamplingTime(0.040);
35  robot.getPosition(wMc);
36  wMo = wMc * cMo;
37 
38 #ifdef VISP_HAVE_DISPLAY
39  vpPlot plotter(2, 250*2, 500, 100, 200, "Real time curves plotter");
40  plotter.setTitle(0, "Visual features error");
41  plotter.setTitle(1, "Camera velocities");
42 
43  plotter.initGraph(0, 8);
44  plotter.initGraph(1, 6);
45 
46  plotter.setLegend(0, 0, "x1");
47  plotter.setLegend(0, 1, "y1");
48  plotter.setLegend(0, 2, "x2");
49  plotter.setLegend(0, 3, "y2");
50  plotter.setLegend(0, 4, "x3");
51  plotter.setLegend(0, 5, "y3");
52  plotter.setLegend(0, 6, "x4");
53  plotter.setLegend(0, 7, "y4");
54 
55  plotter.setLegend(1, 0, "v_x");
56  plotter.setLegend(1, 1, "v_y");
57  plotter.setLegend(1, 2, "v_z");
58  plotter.setLegend(1, 3, "w_x");
59  plotter.setLegend(1, 4, "w_y");
60  plotter.setLegend(1, 5, "w_z");
61 #endif
62 
63  for (unsigned int iter=0; iter < 150; iter ++) {
64  robot.getPosition(wMc);
65  cMo = wMc.inverse() * wMo;
66  for (int i = 0 ; i < 4 ; i++) {
67  point[i].track(cMo);
68  vpFeatureBuilder::create(p[i], point[i]);
69  }
70  vpColVector v = task.computeControlLaw();
72 
73 #ifdef VISP_HAVE_DISPLAY
74  plotter.plot(0, iter, task.getError());
75  plotter.plot(1, iter, v);
76 #endif
77  }
78 
79  task.kill();
80 
81 #ifdef VISP_HAVE_DISPLAY
82  plotter.saveData(0, "error.dat");
83  plotter.saveData(1, "vc.dat");
84 
85  vpDisplay::getClick(plotter.I);
86 #endif
87 }
88 
89