Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
tutorial-pose-from-points-image.cpp
1 
2 #include <visp3/gui/vpDisplayGDI.h>
3 #include <visp3/gui/vpDisplayOpenCV.h>
4 #include <visp3/gui/vpDisplayX.h>
5 #include <visp3/blob/vpDot2.h>
6 #include <visp3/io/vpImageIo.h>
7 #include <visp3/core/vpPixelMeterConversion.h>
8 #include <visp3/vision/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) {
22  vpHomogeneousMatrix cMo_dem;
23  vpHomogeneousMatrix cMo_lag;
24  pose.computePose(vpPose::DEMENTHON, cMo_dem);
25  pose.computePose(vpPose::LAGRANGE, cMo_lag);
26  double residual_dem = pose.computeResidual(cMo_dem);
27  double residual_lag = pose.computeResidual(cMo_lag);
28  if (residual_dem < residual_lag)
29  cMo = cMo_dem;
30  else
31  cMo = cMo_lag;
32  }
34 }
35 
36 int main()
37 {
38  try {
40  vpImageIo::read(I, "square.pgm");
41 
42 #if defined(VISP_HAVE_X11)
43  vpDisplayX d(I);
44 #elif defined(VISP_HAVE_GDI)
45  vpDisplayGDI d(I);
46 #elif defined(VISP_HAVE_OPENCV)
47  vpDisplayOpenCV d(I);
48 #endif
49 
50  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
51  std::vector<vpDot2> dot(4);
52  dot[0].initTracking(I, vpImagePoint(193, 157));
53  dot[1].initTracking(I, vpImagePoint(203, 366));
54  dot[2].initTracking(I, vpImagePoint(313, 402));
55  dot[3].initTracking(I, vpImagePoint(304, 133));
56  std::vector<vpPoint> point;
57  point.push_back( vpPoint(-0.06, -0.06, 0) );
58  point.push_back( vpPoint( 0.06, -0.06, 0) );
59  point.push_back( vpPoint( 0.06, 0.06, 0) );
60  point.push_back( vpPoint(-0.06, 0.06, 0) );
62  bool init = true;
63 
64  while(1) {
65  vpImageIo::read(I, "square.pgm");
67  for (unsigned int i=0; i < dot.size(); i ++) {
68  dot[i].setGraphics(true);
69  dot[i].track(I);
70  }
71  computePose(point, dot, cam, init, cMo);
72  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
74 
75  if (init) init = false; // turn off pose initialisation
76 
77  if (vpDisplay::getClick(I, false)) break;
78 
79  vpTime::wait(40);
80  }
81  }
82  catch(const vpException &e) {
83  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
84  }
85 }
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
unsigned int getWidth() const
Definition: vpImage.h:226
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
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)
Class that defines what is a point.
Definition: vpPoint.h:59
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:76
Generic class defining intrinsic camera parameters.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
Definition: vpPose.cpp:372
const char * getMessage(void) const
Definition: vpException.cpp:97
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))
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:205
unsigned int getHeight() const
Definition: vpImage.h:175
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:145
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
Definition: vpPose.cpp:337