Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
tutorial-matching-keypoint-homography.cpp
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/vision/vpHomography.h>
#include <visp3/vision/vpKeyPoint.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/io/vpVideoReader.h>
int main(int argc, const char **argv)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
int method = 0;
if (argc > 1)
method = atoi(argv[1]);
if (method == 0)
std::cout << "Uses Ransac to estimate the homography" << std::endl;
else
std::cout << "Uses a robust scheme to estimate the homography" << std::endl;
vpVideoReader reader;
reader.setFileName("video-postcard.mpeg");
reader.acquire(I);
const std::string detectorName = "ORB";
const std::string extractorName = "ORB";
//Hamming distance must be used with ORB
const std::string matcherName = "BruteForce-Hamming";
vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType);
keypoint.buildReference(I);
Idisp.resize(I.getHeight(), 2*I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
vpDisplayOpenCV d(Idisp, 0, 0, "Homography from matched keypoints") ;
vpImagePoint corner_ref[4];
corner_ref[0].set_ij(115, 64);
corner_ref[1].set_ij( 83, 253);
corner_ref[2].set_ij(282, 307);
corner_ref[3].set_ij(330, 72);
for (unsigned int i=0; i<4; i++) {
vpDisplay::displayCross(Idisp, corner_ref[i], 12, vpColor::red);
}
vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
vpHomography curHref;
while ( ! reader.end() )
{
reader.acquire(I);
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
unsigned int nbMatch = keypoint.matchPoint(I);
std::cout << "Nb matches: " << nbMatch << std::endl;
std::vector<vpImagePoint> iPref(nbMatch), iPcur(nbMatch); // Coordinates in pixels (for display only)
std::vector<double> mPref_x(nbMatch), mPref_y(nbMatch);
std::vector<double> mPcur_x(nbMatch), mPcur_y(nbMatch);
std::vector<bool> inliers(nbMatch);
for (unsigned int i = 0; i < nbMatch; i++) {
keypoint.getMatchedPoints(i, iPref[i], iPcur[i]);
vpPixelMeterConversion::convertPoint(cam, iPref[i], mPref_x[i], mPref_y[i]);
vpPixelMeterConversion::convertPoint(cam, iPcur[i], mPcur_x[i], mPcur_y[i]);
}
try{
double residual;
if (method == 0)
vpHomography::ransac(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual,
(unsigned int)(mPref_x.size()*0.25), 2.0/cam.get_px(), true);
else
vpHomography::robust(mPref_x, mPref_y, mPcur_x, mPcur_y, curHref, inliers, residual,
0.4, 4, true);
} catch(...)
{
std::cout << "Cannot compute homography from matches..." << std::endl;
}
vpImagePoint corner_cur[4];
for (int i=0; i< 4; i++) {
corner_cur[i] = vpHomography::project(cam, curHref, corner_ref[i]);
}
vpImagePoint offset(0, I.getWidth());
for (int i=0; i< 4; i++) {
corner_cur[i] + offset,
corner_cur[(i+1)%4] + offset,
}
for (unsigned int i = 0; i < nbMatch; i++) {
if(inliers[i] == true)
vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::green);
else
vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::red);
}
if (vpDisplay::getClick(Idisp, false))
break;
}
#else
(void)argc; (void)argv;
#endif
return 0;
}