Visual Servoing Platform  version 3.0.0
tutorial-homography-from-points.cpp
1 
4 #include <visp3/vision/vpHomography.h>
6 #include <visp3/core/vpMeterPixelConversion.h>
7 
8 int main()
9 {
11  double L = 0.1;
12  std::vector<vpPoint> oP;
13  oP.push_back( vpPoint( -L,-L, 0) );
14  oP.push_back( vpPoint(2*L,-L, 0) );
15  oP.push_back( vpPoint( L, 3*L, 0) );
16  oP.push_back( vpPoint( -L, 4*L, 0) );
18 
20  vpHomogeneousMatrix bMo(0.1, 0, 1, 0, vpMath::rad(15), 0);
21  vpHomogeneousMatrix aMb(0.2, -0.1, 0.1, vpMath::rad(-3), vpMath::rad(20), vpMath::rad(5));
22  vpHomogeneousMatrix aMo = aMb*bMo;
24 
26  std::vector<vpPoint> aP(4), bP(4);
27  std::vector<double> xa(4), ya(4), xb(4), yb(4);
28  for(unsigned int i=0 ; i < 4; i++)
29  {
30  oP[i].project(aMo);
31  xa[i] = oP[i].get_x();
32  ya[i] = oP[i].get_y();
33  oP[i].project(bMo);
34  xb[i] = oP[i].get_x();
35  yb[i] = oP[i].get_y();
36  }
38 
40  vpHomography aHb ;
41  vpHomography::DLT(xb, yb, xa, ya, aHb, true);
42  std::cout << "Estimated homography using DLT:\n" << aHb/aHb[2][2] << std::endl;
43 
44  vpHomography::HLM(xb, yb, xa, ya, true, aHb);
45  std::cout << "Estimated homography using HLM:\n" << aHb/aHb[2][2] << std::endl;
47 
49  vpRotationMatrix aRb;
51  vpColVector n;
52  aHb.computeDisplacement(aRb, atb, n);
54 
56  std::cout << "\nEstimated displacement:" << std::endl;
57  std::cout << " atb: " << atb.t() << std::endl;
58  vpThetaUVector atub;
59  atub.buildFrom(aRb);
60  std::cout << " athetaub: ";
61  for(unsigned int i=0; i<3; i++)
62  std::cout << vpMath::deg(atub[i]) << " ";
63  std::cout << std::endl;
64  std::cout << " n: " << n.t() << std::endl;
66 
68  vpImagePoint iPa, iPb;
70  vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb);
71  vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa);
72 
73  std::cout << "Ground truth: Point 3 in pixels in frame b: " << iPb << std::endl;
74  std::cout << "Ground truth: Point 3 in pixels in frame a: " << iPa << std::endl;
76 
78  // Project the position in pixel of point 3 from the homography
79  std::cout << "Estimation from homography: Point 3 in pixels in frame a: "
80  << vpHomography::project(cam, aHb, iPb) << std::endl;
82 }
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Class that defines what is a point.
Definition: vpPoint.h:59
static void HLM(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, bool isplanar, vpHomography &aHb)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
vpRowVector t() const
Generic class defining intrinsic camera parameters.
vpRowVector t() const
static double rad(double deg)
Definition: vpMath.h:104
static void DLT(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, bool normalization=true)
static double deg(double rad)
Definition: vpMath.h:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.