Visual Servoing Platform  version 3.0.0
tutorial-pose-from-points-tracking.cpp
1 
2 #include <visp3/core/vpConfig.h>
3 #ifdef VISP_HAVE_MODULE_SENSOR
4 #include <visp3/sensor/vp1394CMUGrabber.h>
5 #include <visp3/sensor/vp1394TwoGrabber.h>
6 #endif
7 #include <visp3/gui/vpDisplayGDI.h>
8 #include <visp3/gui/vpDisplayOpenCV.h>
9 #include <visp3/gui/vpDisplayX.h>
10 #include <visp3/blob/vpDot2.h>
11 #include <visp3/core/vpPixelMeterConversion.h>
12 #include <visp3/vision/vpPose.h>
13 
14 void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
15  const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo);
16 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
17 void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init);
18 #endif
19 
20 void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
21  const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
22 {
23  vpPose pose; double x=0, y=0;
24  for (unsigned int i=0; i < point.size(); i ++) {
25  vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
26  point[i].set_x(x);
27  point[i].set_y(y);
28  pose.addPoint(point[i]);
29  }
30 
31  if (init == true) {
32  vpHomogeneousMatrix cMo_dem;
33  vpHomogeneousMatrix cMo_lag;
34  pose.computePose(vpPose::DEMENTHON, cMo_dem);
35  pose.computePose(vpPose::LAGRANGE, cMo_lag);
36  double residual_dem = pose.computeResidual(cMo_dem);
37  double residual_lag = pose.computeResidual(cMo_lag);
38  if (residual_dem < residual_lag)
39  cMo = cMo_dem;
40  else
41  cMo = cMo_lag;
42  }
44 }
45 
46 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
47 void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
48 {
49  if (init) {
51  for(unsigned int i=0; i<dot.size(); i++) {
52  dot[i].setGraphics(true);
53  dot[i].setGraphicsThickness(2);
54  dot[i].initTracking(I);
56  }
57  }
58  else {
59  for(unsigned int i=0; i<dot.size(); i++) {
60  dot[i].track(I);
61  }
62  }
63 }
64 #endif
65 
66 int main()
67 {
68 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
69  try { vpImage<unsigned char> I;
70 
71 #if defined(VISP_HAVE_DC1394)
73  g.open(I);
74 #elif defined(VISP_HAVE_CMU1394)
76  g.open(I);
77 #elif defined(VISP_HAVE_OPENCV)
78  cv::VideoCapture g(0); // open the default camera
79  if(!g.isOpened()) { // check if we succeeded
80  std::cout << "Failed to open the camera" << std::endl;
81  return -1;
82  }
83  cv::Mat frame;
84  g >> frame; // get a new frame from camera
85  vpImageConvert::convert(frame, I);
86 #endif
87 
88  // Parameters of our camera
89  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
90 
91  // The pose container
93 
94  std::vector<vpDot2> dot(4);
95  std::vector<vpPoint> point;
96  double L = 0.06;
97  point.push_back( vpPoint(-L, -L, 0) );
98  point.push_back( vpPoint( L, -L, 0) );
99  point.push_back( vpPoint( L, L, 0) );
100  point.push_back( vpPoint(-L, L, 0) );
101 
102  bool init = true;
103 #if defined(VISP_HAVE_X11)
104  vpDisplayX d(I);
105 #elif defined(VISP_HAVE_GDI)
106  vpDisplayGDI d(I);
107 #elif defined(VISP_HAVE_OPENCV)
108  vpDisplayOpenCV d(I);
109 #endif
110 
111  while(1){
112  // Image Acquisition
113 #if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
114  g.acquire(I);
115 #elif defined(VISP_HAVE_OPENCV)
116  g >> frame;
117  vpImageConvert::convert(frame, I);
118 #endif
119 
121  track(I, dot, init);
122  computePose(point, dot, cam, init, cMo);
123  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
124  vpDisplay::flush(I);
125  if (init) init = false; // turn off the initialisation specific stuff
126 
127  if (vpDisplay::getClick(I, false))
128  break;
129  }
130  }
131  catch(vpException e) {
132  std::cout << "Catch an exception: " << e << std::endl;
133  }
134 #endif
135 }
unsigned int getWidth() const
Definition: vpImage.h:161
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void open(vpImage< unsigned char > &I)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Define the X11 console to display images.
Definition: vpDisplayX.h:148
static const vpColor none
Definition: vpColor.h:175
error that can be emited by ViSP classes.
Definition: vpException.h:73
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:2233
Class that defines what is a point.
Definition: vpPoint.h:59
void open(vpImage< unsigned char > &I)
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void acquire(vpImage< unsigned char > &I)
bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
compute the pose for a given method
Definition: vpPose.cpp:382
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
The vpDisplayOpenCV allows to display image using the opencv library.
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:74
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=vpColor::none, unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0, 0))
Definition: vpDisplay.cpp:373
unsigned int getHeight() const
Definition: vpImage.h:152
Class for firewire ieee1394 video devices using libdc1394-2.x api.
virtual bool getClick(bool blocking=true)=0
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:151
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
Definition: vpPose.cpp:340