ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
tutorial-pose-from-points-image.cpp
1 
2 #include <visp/vpDisplayGDI.h>
3 #include <visp/vpDisplayX.h>
4 #include <visp/vpDot2.h>
5 #include <visp/vpImageIo.h>
6 #include <visp/vpPixelMeterConversion.h>
7 #include <visp/vpPose.h>
8 
9 void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
10  const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
11 {
12  vpPose pose; double x=0, y=0;
13  for (unsigned int i=0; i < point.size(); i ++) {
14  vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
15  point[i].set_x(x);
16  point[i].set_y(y);
17  pose.addPoint(point[i]);
18  }
19 
20  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
21  else pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
22 }
23 
24 int main()
25 {
27  vpImageIo::read(I, "square.pgm");
28 
29 #if defined(VISP_HAVE_X11)
30  vpDisplayX d(I);
31 #elif defined(VISP_HAVE_GDI)
32  vpDisplayGDI d(I);
33 #endif
34 
35  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
36  std::vector<vpDot2> dot(4);
37  dot[0].initTracking(I, vpImagePoint(193, 157));
38  dot[1].initTracking(I, vpImagePoint(203, 366));
39  dot[2].initTracking(I, vpImagePoint(313, 402));
40  dot[3].initTracking(I, vpImagePoint(304, 133));
41  std::vector<vpPoint> point(4);
42  point[0].setWorldCoordinates(-0.06, -0.06, 0);
43  point[1].setWorldCoordinates( 0.06, -0.06, 0);
44  point[2].setWorldCoordinates( 0.06, 0.06, 0);
45  point[3].setWorldCoordinates(-0.06, 0.06, 0);
47  bool init = true;
48 
49  while(1){
50  vpImageIo::read(I, "square.pgm");
52  for (unsigned int i=0; i < dot.size(); i ++) {
53  dot[i].setGraphics(true);
54  dot[i].track(I);
55  }
56  computePose(point, dot, cam, init, cMo);
57  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
59 
60  if (init) init = false; // turn off pose initialisation
61 
62  if (vpDisplay::getClick(I, false)) break;
63 
64  vpTime::wait(40);
65  }
66 }