// ------------------------------------------------------
// Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_Harris_SAD( int w, int h )
{
CTicTac tictac;
CImage imL, imR;
CFeatureExtraction fExt;
CFeatureList featsHarris_L, featsHarris_R;
CMatchedFeatureList mHarris;
getTestImage(0,imR);
getTestImage(1,imL);
// Extract features: HARRIS
fExt.options.featsType = featHarris;
TMatchingOptions opt;
const size_t N = 20;
opt.matching_method = TMatchingOptions::mmSAD;
// HARRIS
tictac.Tic();
for (size_t i=0;i<N;i++)
{
fExt.detectFeatures( imL, featsHarris_L, 0, NFEATS );
fExt.detectFeatures( imR, featsHarris_R, 0, NFEATS );
//nMatches =
matchFeatures( featsHarris_L, featsHarris_R, mHarris, opt );
}
const double T = tictac.Tac()/N;
// cout << endl << "L: " << featsHarris_L.size() << " R: " << featsHarris_R.size() << " M: " << mHarris.size() << endl;
return T;
}
// ------------------------------------------------------
// Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_FAST_CC( int w, int h )
{
CTicTac tictac;
CImage imL, imR;
CFeatureExtraction fExt;
CFeatureList featsFAST_L, featsFAST_R;
CMatchedFeatureList mFAST;
getTestImage(0,imR);
getTestImage(1,imL);
// Extract features: HARRIS
fExt.options.featsType = featFAST;
//size_t nMatches;
TMatchingOptions opt;
const size_t N = 20;
// HARRIS
tictac.Tic();
for (size_t i=0;i<N;i++)
{
fExt.detectFeatures( imL, featsFAST_L, 0, NFEATS );
fExt.detectFeatures( imR, featsFAST_R, 0, NFEATS );
//nMatches =
matchFeatures( featsFAST_L, featsFAST_R, mFAST, opt );
}
const double T = tictac.Tac()/N;
// cout << endl << "L: " << featsFAST_L.size() << " R: " << featsFAST_R.size() << " M: " << mFAST.size() << endl;
return T;
}
// ------------------------------------------------------
// TestCapture
// ------------------------------------------------------
void TestExtractMatchProjectAndPaint()
{
CDisplayWindow3D wind;
CFeatureExtraction fExt;
CFeatureList featsHarris_L, featsHarris_R;
CMatchedFeatureList mHarris, mSIFT, mSURF;
CImage imL, imR;
string imgL = MRPT_EXAMPLES_BASE_DIRECTORY + string("feature_extraction/") + string("imgs/imL_p01.jpg"); // Left image
string imgR = MRPT_EXAMPLES_BASE_DIRECTORY + string("feature_extraction/") + string("imgs/imR_p01.jpg"); // Right image
// Load and check images
if (!imL.loadFromFile( imgL ))
{
cerr << "Cannot load " << imgL << endl;
return;
}
cout << "Loaded test image: " << imgL << endl;
if (!imR.loadFromFile( imgR ))
{
cerr << "Cannot load " << imgR << endl;
return;
}
cout << "Loaded test image: " << imgR << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Extract features:
// HARRIS
cout << "Detecting HARRIS features in LEFT image" << endl;
fExt.options.featsType = featKLT;
fExt.detectFeatures( imL, featsHarris_L );
cout << "Detected " << featsHarris_L.size() << endl;
cout << "Detecting HARRIS features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsHarris_R );
cout << "Detected " << featsHarris_R.size() << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Match features:
//size_t nMatches;
TMatchingOptions opt;
// HARRIS
cout << "Matching HARRIS features by CORRELATION" << endl;
//nMatches =
matchFeatures( featsHarris_L, featsHarris_R, mHarris );
cout << "Matches found: " << mHarris.size() << endl;
cout << "***************************************************" << endl;
} // end TestExtractMatchProjectAndPaint
// ------------------------------------------------------
// TestCapture
// ------------------------------------------------------
void TestExtractFeaturesTile()
{
CDisplayWindow wind1,wind2;
CFeatureExtraction fExt;
CFeatureList featsHarris;
CImage img;
string the_img = myDataDir+string("test_image.jpg");
if (!img.loadFromFile(the_img ))
{
cerr << "Cannot load " << the_img << endl;
return;
}
cout << "Loaded test image: " << the_img << endl;
CTicTac tictac;
cout << "Extracting Harris features (tiled)... [f_harris_tiled.txt]";
fExt.options.featsType = featHarris;
fExt.options.harrisOptions.tile_image = true;
tictac.Tic();
fExt.detectFeatures( img, featsHarris );
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
cout << "Detected " << featsHarris.size() << " features in " << endl;
featsHarris.saveToTextFile("f_harris_tiled.txt");
wind1.setWindowTitle("Harris detected features (Tiled image)");
wind1.showTiledImageAndPoints( img, featsHarris );
cout << "Extracting Harris features... [f_harris.txt]";
fExt.options.harrisOptions.tile_image = false;
tictac.Tic();
fExt.detectFeatures( img, featsHarris );
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
featsHarris.saveToTextFile("f_harris.txt");
wind2.setWindowTitle("Harris detected features");
wind2.showTiledImageAndPoints( img, featsHarris );
mrpt::system::pause();
return;
}
double benchmark_detectFeatures(int N, [[maybe_unused]] int h)
{
// Generate a random image
CImage img;
getTestImage(0, img);
CFeatureExtraction fExt;
fExt.profiler.enable();
fExt.options.featsType = FEAT_TYPE;
for (int i = 0; i < N; i++)
{
CFeatureList fs;
fExt.detectFeatures(img, fs);
if (i == (N - 1))
std::cout << "(" << std::setw(4) << fs.size() << " found)\n";
}
return fExt.profiler.getMeanTime("detectFeatures");
}
double benchmark_computeDescriptor(int N, int num_feats)
{
CImage img;
getTestImage(0, img);
CFeatureExtraction fExt;
fExt.profiler.enable();
fExt.options.featsType = featFASTER9;
for (int i = 0; i < N; i++)
{
CFeatureList fs;
fExt.detectFeatures(img, fs, 0 /*id*/, num_feats);
fExt.computeDescriptors(img, fs, DESCRIPTOR_TYPE);
}
return fExt.profiler.getMeanTime("computeDescriptors");
}
// ------------------------------------------------------
// Benchmark: Harris
// ------------------------------------------------------
double feature_extraction_test_Harris( int N, int h )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
CFeatureExtraction fExt;
CFeatureList featsHarris;
fExt.options.featsType = featHarris;
tictac.Tic();
for (int i=0;i<N;i++)
fExt.detectFeatures( img, featsHarris );
const double T = tictac.Tac()/N;
// cout << "Harris: " << featsHarris.size();
return T;
}
double benchmark_detectFeatures_FASTER(int N, int threshold)
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0, img);
CFeatureExtraction fExt;
CFeatureList feats;
fExt.options.featsType = TYP;
fExt.options.FASTOptions.threshold = threshold;
fExt.options.patchSize = 0;
img = img.grayscale();
tictac.Tic();
for (int i = 0; i < N; i++) fExt.detectFeatures(img, feats, 0, MAX_N_FEATS);
const double T = tictac.Tac() / N;
return T;
}
void TestTrackFeatures()
{
CImage im1, im2;
im1.loadFromFile("/Trabajo/Experimentos/[2009] vOdometry Characterization/right1.jpg");
im2.loadFromFile("/Trabajo/Experimentos/[2009] vOdometry Characterization/right2.jpg");
CFeatureExtraction fExt;
CFeatureList feats;
fExt.options.featsType = featKLT;
fExt.detectFeatures( im1, feats );
feats.saveToTextFile("J:/Trabajo/Experimentos/[2009] vOdometry Characterization/before.txt");
CFeatureTracker_KL tracker;
// tracker.extra_params["add_new_features"] = 1; // track, AND ALSO, add new features
// ...
// Do tracking:
tracker.trackFeatures(im1, im2, feats);
feats.saveToTextFile("/Trabajo/Experimentos/[2009] vOdometry Characterization/after.txt");
}
// ------------------------------------------------------
// Benchmark: KLT
// ------------------------------------------------------
double feature_extraction_test_KLT( int N, int h )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
CFeatureExtraction fExt;
CFeatureList featsKLT;
fExt.options.featsType = featKLT;
fExt.options.KLTOptions.threshold = 0.05f;
fExt.options.KLTOptions.radius = 5;
tictac.Tic();
for (int i=0;i<N;i++)
fExt.detectFeatures( img, featsKLT );
const double T = tictac.Tac()/N;
// cout << "SPIN: " << featsKLT.size();
return T;
}
// ------------------------------------------------------
// Benchmark: SIFT (hess)
// ------------------------------------------------------
double feature_extraction_test_SIFT( int N, int h )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
CFeatureExtraction fExt;
CFeatureList featsSIFT;
fExt.options.featsType = featSIFT;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
tictac.Tic();
for (int i=0;i<N;i++)
fExt.detectFeatures( img, featsSIFT );
const double T = tictac.Tac()/N;
// cout << "SIFT: " << featsSIFT.size();
return T;
}
// ------------------------------------------------------
// Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_SURF(int w, int h)
{
CTicTac tictac;
CImage imL, imR;
CFeatureExtraction fExt;
CFeatureList featsSURF_L, featsSURF_R;
CMatchedFeatureList mSURF;
getTestImage(0, imR);
getTestImage(1, imL);
// Extract features: HARRIS
fExt.options.featsType = featSURF;
// size_t nMatches;
TMatchingOptions opt;
const size_t N = 10;
opt.matching_method = TMatchingOptions::mmDescriptorSURF;
// HARRIS
tictac.Tic();
for (size_t i = 0; i < N; i++)
{
fExt.detectFeatures(imL, featsSURF_L, 0, NFEATS);
fExt.detectFeatures(imR, featsSURF_R, 0, NFEATS);
// nMatches =
matchFeatures(featsSURF_L, featsSURF_R, mSURF, opt);
}
const double T = tictac.Tac() / N;
// cout << endl << "L: " << featsSURF_L.size() << " R: " <<
// featsSURF_R.size() << " M: " << mSURF.size() << endl;
return T;
}
double feature_extraction_test_FASTER( int N, int threshold )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
CFeatureExtraction fExt;
CFeatureList feats;
fExt.options.featsType = TYP; // FASTER_N==9 ? featFASTER9 : (FASTER_N==10 ? featFASTER10 : featFASTER12 );
fExt.options.FASTOptions.threshold = threshold; //20;
fExt.options.patchSize = 0;
img.grayscaleInPlace();
tictac.Tic();
for (int i=0;i<N;i++)
fExt.detectFeatures( img, feats,0, MAX_N_FEATS );
const double T = tictac.Tac()/N;
return T;
}
// ------------------------------------------------------
// Benchmark: Spin descriptor
// ------------------------------------------------------
double feature_extraction_test_Spin_desc( int N, int h )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
CFeatureExtraction fExt;
CFeatureList featsHarris;
fExt.options.SpinImagesOptions.radius = 13;
fExt.options.SpinImagesOptions.hist_size_distance = 10;
fExt.options.SpinImagesOptions.hist_size_intensity = 10;
fExt.detectFeatures( img, featsHarris );
tictac.Tic();
for (int i=0;i<N;i++)
fExt.computeDescriptors( img, featsHarris, descSpinImages );
const double T = tictac.Tac()/N;
return T;
}
// ------------------------------------------------------
// Benchmark: FAST
// ------------------------------------------------------
double feature_extraction_test_FAST( int N, int h )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
CFeatureExtraction fExt;
CFeatureList featsFAST;
fExt.options.featsType = featFAST;
fExt.options.FASTOptions.threshold = 20;
fExt.options.patchSize = 0;
img.grayscaleInPlace();
tictac.Tic();
for (int i=0;i<N;i++)
fExt.detectFeatures( img, featsFAST );
const double T = tictac.Tac()/N;
return T;
}
// ------------------------------------------------------
// TestCapture
// ------------------------------------------------------
void TestMatchFeatures()
{
CDisplayWindow wind, wind2;
CFeatureExtraction fExt;
CFeatureList featsHarris_L, featsHarris_R, featsSIFT_L, featsSIFT_R, featsSURF_L, featsSURF_R, featsFAST_L, featsFAST_R;
CMatchedFeatureList mHarris, mSIFT, mSURF, mHarris_SAD, mFAST_CC, mFAST_SAD;
CImage imL, imR;
string imgL = MRPT_EXAMPLES_BASE_DIRECTORY + string("feature_extraction/") + string("imgs/imL_p01.jpg"); // Left image
string imgR = MRPT_EXAMPLES_BASE_DIRECTORY + string("feature_extraction/") + string("imgs/imR_p01.jpg"); // Right image
// string imgL = "../../bin/imgs/640x480_left_rect.jpg"; // Left image
// string imgR = "../../bin/imgs/640x480_right_rect.jpg"; // Right image
// Load and check images
if (!imL.loadFromFile( imgL ))
{
cerr << "Cannot load " << imgL << endl;
return;
}
cout << "Loaded test image: " << imgL << endl;
if (!imR.loadFromFile( imgR ))
{
cerr << "Cannot load " << imgR << endl;
return;
}
cout << "Loaded test image: " << imgR << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Extract features:
// HARRIS
cout << "Detecting HARRIS features in LEFT image" << endl;
fExt.options.featsType = featHarris;
fExt.detectFeatures( imL, featsHarris_L );
cout << "Detected " << featsHarris_L.size() << endl;
cout << "Detecting HARRIS features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsHarris_R );
cout << "Detected " << featsHarris_R.size() << endl;
cout << "***************************************************" << endl;
// SIFT
cout << "Detecting SIFT features in LEFT image" << endl;
fExt.options.featsType = featSIFT;
//fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::OpenCV;
fExt.detectFeatures( imL, featsSIFT_L );
cout << "Detected " << featsSIFT_L.size() << endl;
cout << "Detecting SIFT features in RIGHT image" << endl;
fExt.options.featsType = featSIFT;
//fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::OpenCV;
fExt.detectFeatures( imR, featsSIFT_R );
cout << "Detected " << featsSIFT_R.size() << endl;
cout << "***************************************************" << endl;
// SURF
cout << "Detecting SURF features in LEFT image" << endl;
fExt.options.featsType = featSURF;
fExt.detectFeatures( imL, featsSURF_L );
cout << "Detected " << featsSURF_L.size() << endl;
cout << "Detecting SURF features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsSURF_R );
cout << "Detected " << featsSURF_R.size() << endl;
cout << "***************************************************" << endl;
// FAST
cout << "Detecting FAST features in LEFT image" << endl;
fExt.options.featsType = featFAST;
fExt.detectFeatures( imL, featsFAST_L, 0, 400 );
cout << "Detected " << featsFAST_L.size() << endl;
CDisplayWindow fast1("LEFT");
fast1.showImageAndPoints( imL, featsFAST_L );
cout << "Detecting FAST features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsFAST_R, 0, 400 );
cout << "Detected " << featsFAST_R.size() << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
CDisplayWindow fast2("RIGHT");
fast2.showImageAndPoints( imR, featsFAST_R );
// Match features:
//size_t nMatches;
TMatchingOptions opt;
// HARRIS
CTicTac tictac;
cout << "Matching HARRIS features by CORRELATION" << endl;
tictac.Tic();
//nMatches =
matchFeatures( featsHarris_L, featsHarris_R, mHarris );
double T = tictac.Tac();
cout << "[CC] Matches found: " << mHarris.size() << " in " << T*1000.0f << " ms " << endl;
opt.matching_method = TMatchingOptions::mmSAD;
tictac.Tic();
//nMatches =
matchFeatures( featsHarris_L, featsHarris_R, mHarris_SAD, opt );
T = tictac.Tac();
cout << "[SAD] Matches found: " << mHarris_SAD.size() << " in " << T*1000.0f << " ms " << endl;
cout << "***************************************************" << endl;
wind.showImagesAndMatchedPoints( imL, imR, mHarris_SAD, TColor(0,0,255) );
// SIFT
cout << "Matching SIFT features by DESCRIPTOR" << endl;
opt.matching_method = TMatchingOptions::mmDescriptorSIFT;
//nMatches =
matchFeatures( featsSIFT_L, featsSIFT_R, mSIFT, opt );
cout << "Matches found: " << mSIFT.size() << endl;
cout << "***************************************************" << endl;
// SURF
cout << "Matching SURF features by DESCRIPTOR" << endl;
opt.matching_method = TMatchingOptions::mmDescriptorSURF;
//nMatches =
matchFeatures( featsSURF_L, featsSURF_R, mSURF, opt );
cout << "Matches found: " << mSURF.size() << endl;
cout << "***************************************************" << endl;
// FAST
cout << "Matching FAST features by CC" << endl;
tictac.Tic();
//nMatches =
matchFeatures( featsFAST_L, featsFAST_R, mFAST_CC );
T = tictac.Tac();
cout << "[CC] Matches found: " << mFAST_CC.size() << " in " << T*1000.0f << " ms " << endl;
opt.matching_method = TMatchingOptions::mmSAD;
tictac.Tic();
//nMatches =
matchFeatures( featsFAST_L, featsFAST_R, mFAST_SAD, opt );
T = tictac.Tac();
cout << "[SAD] Matches found: " << mFAST_SAD.size() << " in " << T*1000.0f << " ms " << endl;
cout << "***************************************************" << endl;
wind2.showImagesAndMatchedPoints( imL, imR, mFAST_SAD, TColor(0,255,0) );
mrpt::system::pause();
} // end TestMatchFeatures
// ------------------------------------------------------
// TestExtractFeatures
// ------------------------------------------------------
void TestExtractFeatures()
{
CDisplayWindow wind1,wind2,wind3,wind4,wind5;
CFeatureExtraction fExt;
CFeatureList featsHarris, featsKLT, featsSIFT_Hess, featsSIFT_Lowe, featsSIFT_Vedaldi, featsSURF, featsFAST;
CImage img;
if (!img.loadFromFile(the_img_for_extract_feats ))
{
cerr << "Cannot load " << the_img_for_extract_feats << endl;
return;
}
cout << "Loaded test image: " << endl << the_img_for_extract_feats << endl;
cout << "--------------------------------------------------------------------------" << endl << endl;
CTicTac tictac;
fExt.options.patchSize = 0;
cout << "Detect Harris features... [f_harris.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featHarris;
fExt.detectFeatures( img, featsHarris );
cout << "Detected " << featsHarris.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsHarris.saveToTextFile("f_harris.txt");
wind1.setWindowTitle("Harris detected features");
wind1.showImageAndPoints(img, featsHarris);
cout << "Detect FAST features... [f_fast.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featFAST;
fExt.options.FASTOptions.threshold = 15; //150;
fExt.options.FASTOptions.min_distance = 4;
fExt.options.FASTOptions.use_KLT_response = true;
fExt.detectFeatures( img, featsFAST, 0, 500 /* max num feats */ );
cout << "Detected " << featsFAST.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsFAST.saveToTextFile("f_fast.txt");
wind5.setWindowTitle("FAST detected features");
wind5.showImageAndPoints( img, featsFAST );
cout << "Computing SIFT descriptors only ... [f_harris+sift.txt]" << endl;
tictac.Tic();
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.computeDescriptors( img, featsHarris, descSIFT );
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsHarris.saveToTextFile("f_harris+sift.txt");
cout << "Extracting KLT features... [f_klt.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featKLT;
fExt.options.KLTOptions.threshold = 0.05f;
fExt.options.KLTOptions.radius = 5;
fExt.detectFeatures( img, featsKLT, 0, 10 );
cout << "Detected " << featsKLT.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsKLT.saveToTextFile("f_klt.txt");
wind2.setWindowTitle("KLT detected features");
wind2.showImageAndPoints( img, featsKLT );
cout << "Extracting SIFT features... [f_sift_hess.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featSIFT;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.detectFeatures( img, featsSIFT_Hess );
cout << "Detected " << featsSIFT_Hess.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsSIFT_Hess.saveToTextFile("f_sift_hess.txt");
wind3.setWindowTitle("SIFT Hess detected features");
wind3.showImageAndPoints( img, featsSIFT_Hess );
cout << "Extracting SURF features... [f_surf.txt]" << endl;
tictac.Tic();
fExt.options.featsType = featSURF;
fExt.detectFeatures( img, featsSURF );
cout << "Detected " << featsSURF.size() << " features in ";
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsSURF.saveToTextFile("f_surf.txt");
wind4.setWindowTitle("SURF detected features");
wind4.showImageAndPoints( img, featsSURF );
cout << "Computing spin images descriptors only ... [f_harris+spinimgs.txt]" << endl;
tictac.Tic();
fExt.options.SpinImagesOptions.radius = 13;
fExt.options.SpinImagesOptions.hist_size_distance = 10;
fExt.options.SpinImagesOptions.hist_size_intensity = 10;
fExt.computeDescriptors( img, featsHarris, descSpinImages );
cout << format(" %.03fms",tictac.Tac()*1000) << endl << endl;
featsHarris.saveToTextFile("f_harris+spinimgs.txt");
mrpt::system::pause();
return;
}
// ------------------------------------------------------
// TestExtractMatchProjectAndPaint
// ------------------------------------------------------
void TestExtractMatchProjectAndPaint()
{
CFeatureExtraction fExt;
CFeatureList featsHarris_L, featsHarris_R;
CMatchedFeatureList mHarris, mSIFT, mSURF;
CImage imL, imR;
string imgL = myDataDir + string("imL_p01.jpg"); // Left image
string imgR = myDataDir + string("imR_p01.jpg"); // Right image
// Load and check images
if (!imL.loadFromFile( imgL ))
{
cerr << "Cannot load " << imgL << endl;
return;
}
cout << "Loaded test image: " << imgL << endl;
if (!imR.loadFromFile( imgR ))
{
cerr << "Cannot load " << imgR << endl;
return;
}
cout << "Loaded test image: " << imgR << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Extract features:
// HARRIS
cout << "Detecting HARRIS features in LEFT image" << endl;
fExt.options.featsType = featKLT;
fExt.detectFeatures( imL, featsHarris_L );
cout << "Detected " << featsHarris_L.size() << endl;
cout << "Detecting HARRIS features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsHarris_R );
cout << "Detected " << featsHarris_R.size() << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Match features:
//size_t nMatches;
TMatchingOptions opt;
cout << "Matching HARRIS features by CORRELATION" << endl;
//nMatches =
matchFeatures( featsHarris_L, featsHarris_R, mHarris );
cout << "Matches found: " << mHarris.size() << endl;
cout << "***************************************************" << endl;
// Project features:
mrpt::maps::CLandmarksMap outMap;
TStereoSystemParams stereoOptions; // Default options: Bumblebee + 640x480
cout << "Projecting matched features" << endl;
mrpt::vision::projectMatchedFeatures( mHarris, stereoOptions, outMap );
CDisplayWindow3D win3D("3D Map");
COpenGLScenePtr &scene3D = win3D.get3DSceneAndLock();
CSetOfObjectsPtr map3D = CSetOfObjects::Create();
outMap.getAs3DObject( map3D );
CGridPlaneXYPtr gridXY = CGridPlaneXY::Create(-10,10,-10,10,0,1);
scene3D->insert( gridXY );
scene3D->insert( map3D );
win3D.unlockAccess3DScene();
win3D.repaint();
mrpt::system::pause();
} // end TestExtractMatchProjectAndPaint
// ------------------------------------------------------
// TestMatchingComparative
// ------------------------------------------------------
void TestMatchingComparative()
{
// Take two images
string imgL = myDataDir + string("imL_p01.jpg"); // Left image
string imgR = myDataDir + string("imR_p01.jpg"); // Right image
CImage im1, im2;
im1.loadFromFile( imgL );
im2.loadFromFile( imgR );
size_t imW = im1.getWidth();
size_t imH = im1.getHeight();
CFeatureExtraction fExt;
fExt.options.featsType = featFAST;
fExt.options.patchSize = 21;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
// Find FAST features
CFeatureList list1, list2;
fExt.detectFeatures( im1, list1, 150 );
// Compute SIFT & SURF descriptors
fExt.computeDescriptors( im1, list1, descSIFT );
fExt.computeDescriptors( im1, list1, descSURF );
fExt.detectFeatures( im2, list2, 150 );
// Compute SIFT & SURF descriptors
fExt.computeDescriptors( im2, list2, descSIFT );
fExt.computeDescriptors( im2, list2, descSURF );
CFeatureList::iterator it1, it2;
for( it1 = list1.begin(); it1 != list1.end(); ++it1 )
im1.cross( (*it1)->x, (*it1)->y, TColor::red, '+');
for( it2 = list2.begin(); it2 != list2.end(); ++it2 )
im2.cross( (*it2)->x, (*it2)->y, TColor::red, '+');
CDisplayWindow win, win2;
win.setPos(0,0);
win2.setPos(0,imH*1.5);
CImage joinimage, copyjoinimage, copyInfoImage;
size_t imW2 = 1280;
size_t imH2 = 150;
CImage infoimage( imW2, imH2, CH_RGB );
joinimage.joinImagesHorz( im1, im2 );
infoimage.filledRectangle( 0, 0, imW2, imH2, TColor(150,150,150) );
infoimage.textOut( 20, imH2-53, "SAD", TColor::blue );
infoimage.textOut( 20, imH2-41, "NCC", TColor::blue );
infoimage.textOut( 20, imH2-29, "SIFT", TColor::blue );
infoimage.textOut( 20, imH2-17, "SURF", TColor::blue );
for( it1 = list1.begin(); it1 != list1.end(); ++it1 )
{
copyInfoImage = infoimage;
copyjoinimage = joinimage;
copyjoinimage.line( (*it1)->x, 0, (*it1)->x, imH, TColor::green ); // Horiz
copyjoinimage.line( (*it1)->x+imW, 0, (*it1)->x+imW, imH, TColor::green ); // Horiz
copyjoinimage.line( 0, (*it1)->y, imW+imW, (*it1)->y, TColor::green ); // Epipolar
copyjoinimage.drawCircle( (*it1)->x, (*it1)->y, 4, TColor::green, 2 ); // Keypoint
copyInfoImage.update_patch( (*it1)->patch, 0, 0 );
bool firstMatch = true;
int cnt = 0;
int px = 80;
double minsad = 1.0, maxncc = 0.0;
float minsiftd = 1.0f, minsurfd = 1.0f;
int idxsad = 0, idxncc = 0, idxsiftd = 0, idxsurfd = 0;
for( it2 = list2.begin(); it2 != list2.end(); ++it2 )
{
if( fabs((*it1)->y-(*it2)->y) <= 1.0 && (*it1)->x > (*it2)->x )
{
// Compute matching with SAD and Correlation and SIFT/SURF?
// Use epipolar constraints
// Compute SAD
double sad = mrpt::vision::computeSAD( (*it1)->patch, (*it2)->patch );
if( sad < minsad )
{
minsad = sad;
idxsad = cnt;
}
// Compute Correlation
double ncc;
size_t u, v;
mrpt::vision::openCV_cross_correlation( (*it1)->patch, (*it2)->patch, u, v, ncc );
if( ncc > maxncc )
{
maxncc = ncc;
idxncc = cnt;
}
// Compute distance between descriptors SIFT
float siftd = (*it1)->descriptorSIFTDistanceTo( *(*it2) );
if( siftd < minsiftd )
{
minsiftd = siftd;
idxsiftd = cnt;
}
// Compute distance between descriptors SIFT
float surfd = (*it1)->descriptorSURFDistanceTo( *(*it2) );
if( surfd < minsurfd )
{
minsurfd = surfd;
idxsurfd = cnt;
}
// Plot images + features + each candidate + difference score
if( firstMatch )
{
copyjoinimage.line( (*it1)->x+imW, 0, (*it1)->x+imW, imH, TColor::green ); // Limit line (only the first time)
firstMatch = false;
} // end-if
copyjoinimage.drawCircle( (*it2)->x+imW, (*it2)->y, 4, TColor::blue, 2 ); // Keypoint
double rx0, rx1, ry0, ry1, tx, ty;
rx0 = (*it2)->x+imW-15;
rx1 = (*it2)->x+imW;
tx = (*it2)->x+imW-13;
if( cnt % 2 )
{
ry0 = (*it2)->y-20;
ry1 = (*it2)->y-10;
ty = (*it2)->y-22;
}
else
{
ry0 = (*it2)->y+10;
ry1 = (*it2)->y+20;
ty = (*it2)->y+8;
}
copyjoinimage.filledRectangle( rx0, ry0, rx1, ry1, TColor(150,150,150) );
copyjoinimage.textOut( tx, ty, format("%d", cnt), TColor::blue );
px = 80+cnt*50;
if( px + fExt.options.patchSize > imW2 )
continue;
copyInfoImage.update_patch( (*it2)->patch, px, 30 );
copyInfoImage.textOut( px, imH2-70, format("%d", cnt), TColor::blue );
copyInfoImage.textOut( px, imH2-53, format("%.2f", sad), TColor::blue );
copyInfoImage.textOut( px, imH2-41, format("%.2f", ncc), TColor::blue );
copyInfoImage.textOut( px, imH2-29, format("%.2f", siftd), TColor::blue );
copyInfoImage.textOut( px, imH2-17, format("%.2f", surfd), TColor::blue );
cnt++;
} // end if
} // end for it2
copyInfoImage.textOut( 80+idxsad*50, imH2-53, format("%.2f", minsad), TColor::green );
copyInfoImage.textOut( 80+idxncc*50, imH2-41, format("%.2f", maxncc), TColor::green );
copyInfoImage.textOut( 80+idxsiftd*50, imH2-29, format("%.2f", minsiftd), TColor::green );
copyInfoImage.textOut( 80+idxsurfd*50, imH2-17, format("%.2f", minsurfd), TColor::green );
win.showImage( copyjoinimage );
win2.showImage( copyInfoImage );
mrpt::system::pause();
} // end for it1
// Save to file
// Check number of good features
} // end TestMatchingComparative
// ------------------------------------------------------
// TestMatchFeatures
// ------------------------------------------------------
void TestMatchFeatures( bool showMatches )
{
CFeatureExtraction fExt;
CFeatureList featsHarris_L, featsHarris_R, featsSIFT_L, featsSIFT_R, featsSURF_L, featsSURF_R, featsFAST_L, featsFAST_R;
CMatchedFeatureList mHarris, mSIFT, mSURF, mHarris_SAD, mFAST_CC, mFAST_SAD;
CImage imL, imR;
string imgL = myDataDir + string("imL_p01.jpg"); // Left image
string imgR = myDataDir + string("imR_p01.jpg"); // Right image
// Load and check images
if (!imL.loadFromFile( imgL ))
{
cerr << "Cannot load " << imgL << endl;
return;
}
cout << "Loaded LEFT image: " << endl << imgL << endl;
if (!imR.loadFromFile( imgR ))
{
cerr << "Cannot load " << imgR << endl;
return;
}
cout << "Loaded RIGHT image: " << endl << imgR << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Extract features:
// HARRIS
cout << "Detecting HARRIS features in LEFT image" << endl;
fExt.options.featsType = featHarris;
fExt.detectFeatures( imL, featsHarris_L, 250 );
cout << "Detected " << featsHarris_L.size() << endl;
cout << "Detecting HARRIS features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsHarris_R, 250 );
cout << "Detected " << featsHarris_R.size() << endl;
cout << "***************************************************" << endl;
// SIFT
cout << "Detecting SIFT features in LEFT image" << endl;
fExt.options.featsType = featSIFT;
//fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::OpenCV;
fExt.detectFeatures( imL, featsSIFT_L );
cout << "Detected " << featsSIFT_L.size() << endl;
cout << "Detecting SIFT features in RIGHT image" << endl;
fExt.options.featsType = featSIFT;
//fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
fExt.options.SIFTOptions.implementation = CFeatureExtraction::OpenCV;
fExt.detectFeatures( imR, featsSIFT_R );
cout << "Detected " << featsSIFT_R.size() << endl;
cout << "***************************************************" << endl;
// SURF
cout << "Detecting SURF features in LEFT image" << endl;
fExt.options.featsType = featSURF;
fExt.detectFeatures( imL, featsSURF_L );
cout << "Detected " << featsSURF_L.size() << endl;
cout << "Detecting SURF features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsSURF_R );
cout << "Detected " << featsSURF_R.size() << endl;
cout << "***************************************************" << endl;
// FAST
cout << "Detecting FAST features in LEFT image" << endl;
fExt.options.featsType = featFAST;
fExt.detectFeatures( imL, featsFAST_L, 0, 250 );
cout << "Detected " << featsFAST_L.size() << endl;
cout << "Detecting FAST features in RIGHT image" << endl;
fExt.detectFeatures( imR, featsFAST_R, 0, 250 );
cout << "Detected " << featsFAST_R.size() << endl;
cout << "***************************************************" << endl;
cout << "***************************************************" << endl;
// Match features:
size_t nMatches;
TMatchingOptions opt;
// // HARRIS
CTicTac tictac;
double T = 0.0;
cout << "Matching HARRIS features" << endl;
tictac.Tic();
nMatches = matchFeatures( featsHarris_L, featsHarris_R, mHarris );
T = tictac.Tac();
cout << "[NCC] Matches found: " << mHarris.size() << " in " << T*1000.0f << " ms " << endl;
opt.matching_method = TMatchingOptions::mmSAD;
tictac.Tic();
nMatches = matchFeatures( featsHarris_L, featsHarris_R, mHarris_SAD, opt );
T = tictac.Tac();
cout << "[SAD] Matches found: " << mHarris_SAD.size() << " in " << T*1000.0f << " ms " << endl;
cout << "***************************************************" << endl;
// SIFT
cout << "Matching SIFT features by DESCRIPTOR" << endl;
opt.matching_method = TMatchingOptions::mmDescriptorSIFT;
tictac.Tic();
nMatches = matchFeatures( featsSIFT_L, featsSIFT_R, mSIFT, opt );
T = tictac.Tac();
cout << "Matches found: " << mSIFT.size() << " in " << T*1000.0f << " ms " << endl;
cout << "***************************************************" << endl;
// SURF
cout << "Matching SURF features by DESCRIPTOR" << endl;
opt.matching_method = TMatchingOptions::mmDescriptorSURF;
tictac.Tic();
nMatches = matchFeatures( featsSURF_L, featsSURF_R, mSURF, opt );
T = tictac.Tac();
cout << "Matches found: " << mSURF.size() << " in " << T*1000.0f << " ms " << endl;
cout << "***************************************************" << endl;
// FAST
cout << "Matching FAST features" << endl;
tictac.Tic();
nMatches = matchFeatures( featsFAST_L, featsFAST_R, mFAST_CC );
T = tictac.Tac();
cout << "[NCC] Matches found: " << mFAST_CC.size() << " in " << T*1000.0f << " ms " << endl;
opt.matching_method = TMatchingOptions::mmSAD;
tictac.Tic();
nMatches = matchFeatures( featsFAST_L, featsFAST_R, mFAST_SAD, opt );
T = tictac.Tac();
cout << "[SAD] Matches found: " << mFAST_SAD.size() << " in " << T*1000.0f << " ms " << endl;
cout << "***************************************************" << endl;
if( showMatches )
{
CDisplayWindow winHarrisSAD, winHarrisNCC, winFASTSAD, winFASTNCC, winSIFT, winSURF;
winHarrisSAD.setWindowTitle("Matches with Harris + SAD");
winHarrisNCC.setWindowTitle("Matches with Harris + NCC");
winFASTSAD.setWindowTitle("Matches with FAST + SAD");
winFASTNCC.setWindowTitle("Matches with FAST + NCC");
winSIFT.setWindowTitle("Matches with SIFT");
winSURF.setWindowTitle("Matches with SURF");
winHarrisNCC.showImagesAndMatchedPoints( imL, imR, mHarris, TColor(0,0,255) );
winHarrisSAD.showImagesAndMatchedPoints( imL, imR, mHarris_SAD, TColor(0,0,255) );
winSIFT.showImagesAndMatchedPoints( imL, imR, mSIFT, TColor(0,255,0) );
winSURF.showImagesAndMatchedPoints( imL, imR, mSURF, TColor(0,255,0) );
winFASTSAD.showImagesAndMatchedPoints( imL, imR, mFAST_SAD, TColor(0,255,0) );
winFASTNCC.showImagesAndMatchedPoints( imL, imR, mFAST_CC, TColor(0,255,0) );
mrpt::system::pause();
}
} // end TestMatchFeatures
/************************************************************************************************
* Start Function function *
************************************************************************************************/
string PlaceRecognition::startPlaceRecognition(CFeatureExtraction fext)
{
ofstream training_file;
CTicTac feature_time;
feature_time.Tic();
/// stores the labels for the i'th image instance for training and testing
/// images
int training_labels[len_training];
int testing_labels[len_testing];
/// The training model is built here all features are extracted in this
/// part, takes 30 seconds for 900+900 images
if (!trained_flag)
{
for (int i = 0; i < len_training; i++)
{
training[i].loadFromFile(training_paths.at(i));
fext.detectFeatures(training[i], feats_training[i], 0, numFeats);
fext.computeDescriptors(
training[i], feats_training[i], desc_to_compute);
}
for (int i = 0; i < len_testing; i++)
{
testing[i].loadFromFile(testing_paths.at(i));
fext.detectFeatures(testing[i], feats_testing[i], 0, numFeats);
fext.computeDescriptors(
testing[i], feats_testing[i], desc_to_compute);
}
trained_flag = true;
feats_testing_org = feats_testing;
} /// end of if feature extraction flag (trained flag)
CTicTac label_time;
label_time.Tic();
computeLabels(training_paths, training_count, training_labels);
computeLabels(testing_paths, testing_count, testing_labels);
int len_train_words;
len_train_words = 0;
// int len_test_words = 0;
for (int i = 0; i < len_training; i++)
len_train_words += feats_training[i].size();
if (!training_file_written_flag)
{
training_words2 = new vector<float>[len_train_words];
training_words1 = new vector<uint8_t>[len_train_words];
}
int training_word_labels[len_train_words];
CTicTac training_time;
training_time.Tic();
training_file.open("training_images_features.txt");
if (!training_file_written_flag)
{
training_file.clear();
int kount = 0;
for (int i = 0; i < len_training; i++)
{
training_file << feats_training[i].size();
for (unsigned int j = 0; j < feats_training[i].size(); j++, kount++)
{
if (descriptor_selected == 0)
{
vector<uint8_t> temp_feat;
temp_feat =
feats_training[i].getByID(j).get()->descriptors.SIFT;
training_words1[kount] =
feats_training[i].getByID(j).get()->descriptors.SIFT;
training_word_labels[kount] = training_labels[i];
for (unsigned int k = 0; k < temp_feat.size(); k++)
training_file << (int)temp_feat.at(k) << " ";
}
else if (descriptor_selected == 1)
{
vector<float> temp_feat;
temp_feat =
feats_training[i].getByID(j).get()->descriptors.SURF;
training_words2[kount] =
feats_training[i].getByID(j).get()->descriptors.SURF;
training_word_labels[kount] = training_labels[i];
for (unsigned int k = 0; k < temp_feat.size(); k++)
{
training_file << temp_feat.at(k) << " ";
}
}
else if (descriptor_selected == 2)
{
vector<float> temp_feat;
temp_feat =
feats_training[i].getByID(j).get()->descriptors.SpinImg;
training_words2[kount] =
feats_training[i].getByID(j).get()->descriptors.SpinImg;
training_word_labels[kount] = training_labels[i];
for (unsigned int k = 0; k < temp_feat.size(); k++)
{
training_file << temp_feat.at(k) << " ";
}
}
else if (descriptor_selected == 3)
;
// //!< Polar image descriptor
else if (descriptor_selected == 4)
;
// //!< Log-Polar image descriptor
else if (descriptor_selected == 5)
{
vector<uint8_t> temp_feat;
temp_feat =
feats_training[i].getByID(j).get()->descriptors.ORB;
training_words1[kount] =
feats_training[i].getByID(j).get()->descriptors.ORB;
training_word_labels[kount] = training_labels[i];
for (unsigned int k = 0; k < temp_feat.size(); k++)
{
int temp_var; //= (int) temp_feat.at(k);
temp_var = (int)training_words1[kount].at(k);
training_file << temp_var << " ";
}
}
else if (descriptor_selected == 6)
{
vector<uint8_t> temp_feat;
temp_feat =
feats_training[i].getByID(j).get()->descriptors.BLD;
training_words1[kount] =
feats_training[i].getByID(j).get()->descriptors.BLD;
training_word_labels[kount] = training_labels[i];
for (unsigned int k = 0; k < temp_feat.size(); k++)
training_file << (int)temp_feat.at(k) << " ";
}
else if (descriptor_selected == 7)
{
vector<uint8_t> temp_feat;
temp_feat =
feats_training[i].getByID(j).get()->descriptors.LATCH;
training_words1[kount] =
feats_training[i].getByID(j).get()->descriptors.LATCH;
training_word_labels[kount] = training_labels[i];
for (unsigned int k = 0; k < temp_feat.size(); k++)
training_file << (int)temp_feat.at(k) << " ";
}
training_file << " #" << training_labels[i] << " $"
<< training_word_labels[kount] << endl;
} // end of inner for loop for number of key-points
} // end of outer for loop for number of images
training_file.close();
// testing_file.close();
this->training_words_org = training_words2;
this->training_words_org2 = training_words1;
this->training_word_labels_org = training_word_labels;
training_word_labels_org = new int[kount];
for (int i = 0; i < kount; i++)
{
training_word_labels_org[i] = training_word_labels[i];
}
this->total_vocab_size_org = len_train_words;
this->training_file_written_flag = true;
} // end of writting training features to a file
CTicTac testing_time;
testing_time.Tic();
/// now extracting features for Place Recognition for testing dataset
int predicted_classes[len_testing];
CTicTac time_prediction;
time_prediction.Tic();
int predicted_Label = 1;
if (descriptor_selected == 1)
predicted_Label = predictLabel(
feats_testing_org, training_words_org, training_word_labels_org,
total_vocab_size_org, current_index_test_image);
else
predicted_Label = predictLabel2(
feats_testing_org, training_words_org2, training_word_labels_org,
total_vocab_size_org, current_index_test_image);
current_index_test_image++;
/// use a bag of words kind of framework here
predicted_classes[current_index_test_image] = predicted_Label;
if (predicted_classes[current_index_test_image] ==
testing_labels[current_index_test_image])
correct++;
else
incorrect++;
stringstream output;
output << endl
<< endl
<< "PLACE RECOGNITION RESULTS " << endl
<< endl
<< "actual label : "
<< findPlaceName(
testing_labels[current_index_test_image % len_testing])
<< ".\n"
<< endl
<< " predicted label : " << findPlaceName(predicted_Label) << ".\n"
<< endl
<< " correct = " << correct << " incorrect = " << incorrect << ".\n"
<< " Current Accuracy: "
<< 100.00 * (double)correct / (double)(incorrect + correct) << " % "
<< endl
<< " image " << current_index_test_image << " of " << len_testing
<< endl;
return output.str();
}
