ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-ibvs-4pts-image-tracking.cpp
1 
2 #include <visp/vpDisplayX.h>
3 #include <visp/vpDisplayGDI.h>
4 #include <visp/vpFeatureBuilder.h>
5 #include <visp/vpImageIo.h>
6 #include <visp/vpImageSimulator.h>
7 #include <visp/vpServo.h>
8 #include <visp/vpServoDisplay.h>
9 #include <visp/vpSimulatorCamera.h>
10 
17 {
18 public:
24  vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam)
25  {
26  // The target is a square 20cm by 2cm square
27  // Initialise the 3D coordinates of the target corners
28  for (int i = 0; i < 4; i++) X_[i].resize(3);
29  // Top left Top right Bottom right Bottom left
30  X_[0][0] = -0.1; X_[1][0] = 0.1; X_[2][0] = 0.1; X_[3][0] = -0.1;
31  X_[0][1] = -0.1; X_[1][1] = -0.1; X_[2][1] = 0.1; X_[3][1] = 0.1;
32  X_[0][2] = 0; X_[1][2] = 0; X_[2][2] = 0; X_[3][2] = 0;
33 
34  vpImageIo::read(target_, filename);
35 
36  // Initialize the image simulator
37  cam_ = cam;
39  sim_.init(target_, X_);
40  }
41 
51  {
52  sim_.setCleanPreviousImage(true);
53  sim_.setCameraPosition(cMo);
54  sim_.getImage(I, cam_);
55  }
56 
57 private:
58  vpColVector X_[4]; // 3D coordinates of the target corners
59  vpImageSimulator sim_;
60  vpImage<unsigned char> target_; // image of the target
61  vpCameraParameters cam_;
62 };
63 
64 
65 void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot)
66 {
67  static std::vector<vpImagePoint> traj[4];
68  for (unsigned int i=0; i<4; i++) {
69  traj[i].push_back(dot[i].getCog());
70  }
71  for (unsigned int i=0; i<4; i++) {
72  for (unsigned int j=1; j<traj[i].size(); j++) {
73  vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green);
74  }
75  }
76 }
77 
78 
79 int main()
80 {
81 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
82  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
83  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
84 
85  vpImage<unsigned char> I(480, 640, 255);
86  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
87 
88  std::vector<vpPoint> point(4) ;
89  point[0].setWorldCoordinates(-0.1,-0.1, 0);
90  point[1].setWorldCoordinates( 0.1,-0.1, 0);
91  point[2].setWorldCoordinates( 0.1, 0.1, 0);
92  point[3].setWorldCoordinates(-0.1, 0.1, 0);
93 
94  vpServo task ;
97  task.setLambda(0.5);
98 
99  vpVirtualGrabber g("./target_square.pgm", cam);
100  g.acquire(I, cMo);
101 
102 #if defined(VISP_HAVE_X11)
103  vpDisplayX d(I, 0, 0, "Current camera view");
104 #elif defined(VISP_HAVE_GDI)
105  vpDisplayGDI d(I, 0, 0, "Current camera view");
106 #else
107  std::cout << "No image viewer is available..." << std::endl;
108 #endif
109 
112  "Click in the 4 dots to initialise the tracking and start the servo",
113  vpColor::red);
114  vpDisplay::flush(I);
115 
116  vpFeaturePoint p[4], pd[4];
117  std::vector<vpDot2> dot(4);
118 
119  for (int i = 0 ; i < 4 ; i++) {
120  point[i].track(cdMo);
121  vpFeatureBuilder::create(pd[i], point[i]);
122 
123  dot[i].setGraphics(true);
124  dot[i].initTracking(I);
125  vpDisplay::flush(I);
126  vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
127 
128  task.addFeature(p[i], pd[i]);
129  }
130 
131  vpHomogeneousMatrix wMc, wMo;
132  vpSimulatorCamera robot;
133  robot.setSamplingTime(0.040);
134  robot.getPosition(wMc);
135  wMo = wMc * cMo;
136 
137  for (; ; ) {
138  robot.getPosition(wMc);
139  cMo = wMc.inverse() * wMo;
140 
141  g.acquire(I, cMo);
142 
144 
145  for (int i = 0 ; i < 4 ; i++) {
146  dot[i].track(I);
147  vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
148 
149  vpColVector cP;
150  point[i].changeFrame(cMo, cP) ;
151  p[i].set_Z(cP[2]);
152  }
153 
154  vpColVector v = task.computeControlLaw();
155 
156  display_trajectory(I, dot);
159 
160  vpDisplay::flush(I);
161  if (vpDisplay::getClick(I, false))
162  break;
163 
164  vpTime::wait( robot.getSamplingTime() * 1000);
165  }
166  task.kill();
167 #endif
168 }
169