Visual Servoing Platform  version 3.0.0
tutorial-matching-keypoint-homography.cpp
1 #include <visp3/gui/vpDisplayOpenCV.h>
3 #include <visp3/vision/vpHomography.h>
4 #include <visp3/vision/vpKeyPoint.h>
5 #include <visp3/core/vpPixelMeterConversion.h>
6 #include <visp3/io/vpVideoReader.h>
7 
8 int main(int argc, const char **argv)
9 {
10 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
11  int method = 0;
13 
14  if (argc > 1)
15  method = atoi(argv[1]);
16 
17  if (method == 0)
18  std::cout << "Uses Ransac to estimate the homography" << std::endl;
19  else
20  std::cout << "Uses a robust scheme to estimate the homography" << std::endl;
22 
24 
25  vpVideoReader reader;
26  reader.setFileName("video-postcard.mpeg");
27  reader.acquire(I);
28 
29  const std::string detectorName = "ORB";
30  const std::string extractorName = "ORB";
31  //Hamming distance must be used with ORB
32  const std::string matcherName = "BruteForce-Hamming";
34  vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType);
35  keypoint.buildReference(I);
36 
38  Idisp.resize(I.getHeight(), 2*I.getWidth());
39  Idisp.insert(I, vpImagePoint(0, 0));
40  Idisp.insert(I, vpImagePoint(0, I.getWidth()));
41 
42  vpDisplayOpenCV d(Idisp, 0, 0, "Homography from matched keypoints") ;
43  vpDisplay::display(Idisp);
44  vpDisplay::flush(Idisp);
45 
47  vpImagePoint corner_ref[4];
48  corner_ref[0].set_ij(115, 64);
49  corner_ref[1].set_ij( 83, 253);
50  corner_ref[2].set_ij(282, 307);
51  corner_ref[3].set_ij(330, 72);
54  for (unsigned int i=0; i<4; i++) {
55  vpDisplay::displayCross(Idisp, corner_ref[i], 12, vpColor::red);
56  }
57  vpDisplay::flush(Idisp);
59 
61  vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
63 
64  vpHomography curHref;
65 
66  while ( ! reader.end() )
67  {
68  reader.acquire(I);
69  Idisp.insert(I, vpImagePoint(0, I.getWidth()));
70  vpDisplay::display(Idisp);
72 
74  unsigned int nbMatch = keypoint.matchPoint(I);
75  std::cout << "Nb matches: " << nbMatch << std::endl;
77 
78  std::vector<vpImagePoint> iPref(nbMatch), iPcur(nbMatch); // Coordinates in pixels (for display only)
80  std::vector<double> mPref_x(nbMatch), mPref_y(nbMatch);
81  std::vector<double> mPcur_x(nbMatch), mPcur_y(nbMatch);
82  std::vector<bool> inliers(nbMatch);
84 
85  for (unsigned int i = 0; i < nbMatch; i++) {
86  keypoint.getMatchedPoints(i, iPref[i], iPcur[i]);
88  vpPixelMeterConversion::convertPoint(cam, iPref[i], mPref_x[i], mPref_y[i]);
89  vpPixelMeterConversion::convertPoint(cam, iPcur[i], mPcur_x[i], mPcur_y[i]);
91  }
92 
94  try{
95  double residual;
96  if (method == 0)
97  vpHomography::ransac(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual,
98  (unsigned int)(mPref_x.size()*0.25), 2.0/cam.get_px(), true);
99  else
100  vpHomography::robust(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual,
101  0.4, 4, true);
102  } catch(...)
103  {
104  std::cout << "Cannot compute homography from matches..." << std::endl;
105  }
106 
108 
110  vpImagePoint corner_cur[4];
111  for (int i=0; i< 4; i++) {
112  corner_cur[i] = vpHomography::project(cam, curHref, corner_ref[i]);
113  }
115 
117  vpImagePoint offset(0, I.getWidth());
118  for (int i=0; i< 4; i++) {
120  corner_cur[i] + offset,
121  corner_cur[(i+1)%4] + offset,
122  vpColor::blue, 3);
123  }
125 
127  for (unsigned int i = 0; i < nbMatch; i++) {
128  if(inliers[i] == true)
129  vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::green);
130  else
131  vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::red);
132  }
134 
135  vpDisplay::flush(Idisp);
136 
137  if (vpDisplay::getClick(Idisp, false))
138  break;
139  }
140 
141  vpDisplay::getClick(Idisp);
142 #else
143  (void)argc; (void)argv;
144 #endif
145  return 0;
146 }
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
unsigned int getWidth() const
Definition: vpImage.h:161
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
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 const vpColor green
Definition: vpColor.h:166
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
static const vpColor red
Definition: vpColor.h:163
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
The vpDisplayOpenCV allows to display image using the opencv library.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
static bool ransac(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, std::vector< bool > &inliers, double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization=true)
void acquire(vpImage< vpRGBa > &I)
void resize(const unsigned int h, const unsigned int w)
set the size of the image without initializing it.
Definition: vpImage.h:616
void setFileName(const char *filename)
double get_px() const
static void robust(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, std::vector< bool > &inlier, double &residual, double weights_threshold=0.4, unsigned int niter=4, bool normalization=true)
Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV libr...
Definition: vpKeyPoint.h:213
void insert(const vpImage< Type > &src, const vpImagePoint topLeft)
Definition: vpImage.h:935
vpFilterMatchingType
Definition: vpKeyPoint.h:218
unsigned int getHeight() const
Definition: vpImage.h:152
virtual bool getClick(bool blocking=true)=0
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static const vpColor white
Definition: vpColor.h:158
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:176
static const vpColor blue
Definition: vpColor.h:169