Visual Servoing Platform  version 3.1.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  oP[i].project(aMo);
30  xa[i] = oP[i].get_x();
31  ya[i] = oP[i].get_y();
32  oP[i].project(bMo);
33  xb[i] = oP[i].get_x();
34  yb[i] = oP[i].get_y();
35  }
37 
39  vpHomography aHb;
40  vpHomography::DLT(xb, yb, xa, ya, aHb, true);
41  std::cout << "Estimated homography using DLT:\n" << aHb / aHb[2][2] << std::endl;
42 
43  vpHomography::HLM(xb, yb, xa, ya, true, aHb);
44  std::cout << "Estimated homography using HLM:\n" << aHb / aHb[2][2] << std::endl;
46 
48  vpRotationMatrix aRb;
50  vpColVector n;
51  aHb.computeDisplacement(aRb, atb, n);
53 
55  std::cout << "\nEstimated displacement:" << std::endl;
56  std::cout << " atb: " << atb.t() << std::endl;
57  vpThetaUVector atub;
58  atub.buildFrom(aRb);
59  std::cout << " athetaub: ";
60  for (unsigned int i = 0; i < 3; i++)
61  std::cout << vpMath::deg(atub[i]) << " ";
62  std::cout << std::endl;
63  std::cout << " n: " << n.t() << std::endl;
65 
67  vpImagePoint iPa, iPb;
69  vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb);
70  vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa);
71 
72  std::cout << "Ground truth: Point 3 in pixels in frame b: " << iPb << std::endl;
73  std::cout << "Ground truth: Point 3 in pixels in frame a: " << iPa << std::endl;
75 
77  // Project the position in pixel of point 3 from the homography
78  std::cout << "Estimation from homography: Point 3 in pixels in frame a: " << vpHomography::project(cam, aHb, iPb)
79  << std::endl;
81 }
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 ...
vpRowVector t() const
vpRowVector t() const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Class that defines what is a point.
Definition: vpPoint.h:58
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:174
Generic class defining intrinsic camera parameters.
static double rad(double deg)
Definition: vpMath.h:102
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:95
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.