ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-pose-from-points-tracking.cpp
1 
2 #include <visp/vp1394CMUGrabber.h>
3 #include <visp/vp1394TwoGrabber.h>
4 #include <visp/vpDisplayGDI.h>
5 #include <visp/vpDisplayX.h>
6 #include <visp/vpDot2.h>
7 #include <visp/vpPixelMeterConversion.h>
8 #include <visp/vpPose.h>
9 
10 void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
11  const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
12 {
13  vpPose pose; double x=0, y=0;
14  for (unsigned int i=0; i < point.size(); i ++) {
15  vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
16  point[i].set_x(x);
17  point[i].set_y(y);
18  pose.addPoint(point[i]);
19  }
20 
21  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
22  else pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
23 }
24 
25 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
26 void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
27 {
28  if (init) {
30  for(unsigned int i=0; i<dot.size(); i++) {
31  dot[i].setGraphics(true);
32  dot[i].setGraphicsThickness(2);
33  dot[i].initTracking(I);
35  }
36  }
37  else {
38  for(unsigned int i=0; i<dot.size(); i++) {
39  dot[i].track(I);
40  }
41  }
42 }
43 #endif
44 
45 int main()
46 {
47 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394))
49 
50 #if defined(VISP_HAVE_DC1394_2)
52 #elif defined(VISP_HAVE_CMU1394)
54 #endif
55  g.open(I);
56 
57  // Parameters of our camera
58  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
59 
60  // The pose container
62 
63  std::vector<vpDot2> dot(4);
64  std::vector<vpPoint> point(4);
65  double L = 0.06;
66  point[0].setWorldCoordinates(-L, -L, 0);
67  point[1].setWorldCoordinates( L, -L, 0);
68  point[2].setWorldCoordinates( L, L, 0);
69  point[3].setWorldCoordinates(-L, L, 0);
70 
71  bool init = true;
72 #if defined(VISP_HAVE_X11)
73  vpDisplayX d(I);
74 #elif defined(VISP_HAVE_GDI)
75  vpDisplayGDI d(I);
76 #endif
77 
78  while(1){
79  // Image Acquisition
80  g.acquire(I);
82  track(I, dot, init);
83  computePose(point, dot, cam, init, cMo);
84  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
86  if (init) init = false; // turn off the initialisation specific stuff
87 
88  if (vpDisplay::getClick(I, false))
89  break;
90  }
91 #endif
92 }