ViSP  2.9.0
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 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
13 void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init);
14 #endif
15 
16 void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
17  const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
18 {
19  vpPose pose; double x=0, y=0;
20  for (unsigned int i=0; i < point.size(); i ++) {
21  vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
22  point[i].set_x(x);
23  point[i].set_y(y);
24  pose.addPoint(point[i]);
25  }
26 
27  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
28  else pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
29 }
30 
31 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
32 void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
33 {
34  if (init) {
36  for(unsigned int i=0; i<dot.size(); i++) {
37  dot[i].setGraphics(true);
38  dot[i].setGraphicsThickness(2);
39  dot[i].initTracking(I);
41  }
42  }
43  else {
44  for(unsigned int i=0; i<dot.size(); i++) {
45  dot[i].track(I);
46  }
47  }
48 }
49 #endif
50 
51 int main()
52 {
53 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394))
54  try { vpImage<unsigned char> I;
55 
56 #if defined(VISP_HAVE_DC1394_2)
58 #elif defined(VISP_HAVE_CMU1394)
60 #endif
61  g.open(I);
62 
63  // Parameters of our camera
64  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
65 
66  // The pose container
68 
69  std::vector<vpDot2> dot(4);
70  std::vector<vpPoint> point(4);
71  double L = 0.06;
72  point[0].setWorldCoordinates(-L, -L, 0);
73  point[1].setWorldCoordinates( L, -L, 0);
74  point[2].setWorldCoordinates( L, L, 0);
75  point[3].setWorldCoordinates(-L, L, 0);
76 
77  bool init = true;
78 #if defined(VISP_HAVE_X11)
79  vpDisplayX d(I);
80 #elif defined(VISP_HAVE_GDI)
81  vpDisplayGDI d(I);
82 #endif
83 
84  while(1){
85  // Image Acquisition
86  g.acquire(I);
88  track(I, dot, init);
89  computePose(point, dot, cam, init, cMo);
90  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
92  if (init) init = false; // turn off the initialisation specific stuff
93 
94  if (vpDisplay::getClick(I, false))
95  break;
96  }
97  }
98  catch(vpException e) {
99  std::cout << "Catch an exception: " << e << std::endl;
100  }
101 #endif
102 }
unsigned int getWidth() const
Definition: vpImage.h:159
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void open(vpImage< unsigned char > &I)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Define the X11 console to display images.
Definition: vpDisplayX.h:152
static const vpColor none
Definition: vpColor.h:179
error that can be emited by ViSP classes.
Definition: vpException.h:76
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1994
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void acquire(vpImage< unsigned char > &I)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:78
Generic class defining intrinsic camera parameters.
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:371
unsigned int getHeight() const
Definition: vpImage.h:150
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:386
virtual bool getClick(bool blocking=true)=0
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:155