Skip to content
Snippets Groups Projects
Commit f4f75253 authored by Sergi Pujol's avatar Sergi Pujol
Browse files

format modification with clang file

parent e5730d54
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
This commit is part of merge request !4. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
Showing
with 361 additions and 355 deletions
src/corner_falko_2d.cpp
+ 77
− 76
View file @ f4f75253
......@@ -7,114 +7,115 @@
#include "corner_falko_2d.h"
namespace laserscanutils{
CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius, double _radius) :BSCExtractor( _circularSectorNumber, _radialRingNumber, _useKeypointRadius ,_radius ) {
// FALKO Extractor Parameters
setMinExtractionRange(0.1);
setMaxExtractionRange(25);
enableSubbeam(true);
setNMSRadius(0.1);
setNeighB(0.01);
setBRatio(4);
setGridSectors(16);
// Matcher Extractor Parameters
setDistanceThreshold(0.1);
}
CornerFalko2d::~CornerFalko2d()
{
namespace laserscanutils {
CornerFalko2d::CornerFalko2d(int _circularSectorNumber, int _radialRingNumber,
bool _useKeypointRadius, double _radius)
: BSCExtractor(_circularSectorNumber, _radialRingNumber, _useKeypointRadius,
_radius) {
// FALKO Extractor Parameters
setMinExtractionRange(0.1);
setMaxExtractionRange(25);
enableSubbeam(true);
setNMSRadius(0.1);
setNeighB(0.01);
setBRatio(4);
setGridSectors(16);
// Matcher Extractor Parameters
setDistanceThreshold(0.1);
}
void CornerFalko2d::AddNewReferenceScene (falkolib::LaserScan scanFALKO){
CornerFalko2d::~CornerFalko2d() {}
// Extract keypoints
lastKeypointSet.clear();
extract(scanFALKO, lastKeypointSet);
void CornerFalko2d::AddNewReferenceScene(falkolib::LaserScan scanFALKO) {
//Compute descriptors
lastDescriptorSet.clear();
compute(scanFALKO, lastKeypointSet, lastDescriptorSet);
// Extract keypoints
lastKeypointSet.clear();
extract(scanFALKO, lastKeypointSet);
keypointSets.push_back(lastKeypointSet);
descriptorSets.push_back(lastDescriptorSet);
// Compute descriptors
lastDescriptorSet.clear();
compute(scanFALKO, lastKeypointSet, lastDescriptorSet);
scansExtracted=scansExtracted+1;
keypointSets.push_back(lastKeypointSet);
descriptorSets.push_back(lastDescriptorSet);
scansExtracted = scansExtracted + 1;
}
void CornerFalko2d::findLoopClosure(LaserScan scan,LaserScanParams scanParams){
falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams);
void CornerFalko2d::findLoopClosure(LaserScan scan,
LaserScanParams scanParams) {
//Compute descriptors
std::vector<falkolib::FALKO> keypointSet2;
extract(scanFALKO, keypointSet2);
falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams);
//Compute descriptors
std::vector <falkolib::BSC> descriptorSet2;
compute(scanFALKO, keypointSet2, descriptorSet2);
// Compute descriptors
std::vector<falkolib::FALKO> keypointSet2;
extract(scanFALKO, keypointSet2);
//Matching
int rows = keypointSets.size();
// Compute descriptors
std::vector<falkolib::BSC> descriptorSet2;
compute(scanFALKO, keypointSet2, descriptorSet2);
numberKeypointsMatch=0;
numberSceneMatch=0;
matchingPosition=-1;
for (int i=0; i<rows;i++){
std::vector<std::pair<int, int> > assoNN;
// Matching
int rows = keypointSets.size();
int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN);
numberKeypointsMatch = 0;
numberSceneMatch = 0;
matchingPosition = -1;
for (int i = 0; i < rows; i++) {
std::vector<std::pair<int, int>> assoNN;
if (NewMatchingNumber > numberKeypointsMatch && NewMatchingNumber>keypointsNumberTh) {
numberKeypointsMatch=NewMatchingNumber;
matchingPosition=i;
numberSceneMatch=numberSceneMatch+1;
int NewMatchingNumber = match(keypointSets[i], keypointSet2, assoNN);
}
if (NewMatchingNumber > numberKeypointsMatch &&
NewMatchingNumber > keypointsNumberTh) {
numberKeypointsMatch = NewMatchingNumber;
matchingPosition = i;
numberSceneMatch = numberSceneMatch + 1;
}
if (numberSceneMatch>1){
numberSceneMatch=0;
matchingPosition=-1;
}
}
if (numberSceneMatch > 1) {
numberSceneMatch = 0;
matchingPosition = -1;
}
}
falkolib::LaserScan CornerFalko2d::convert2LaserScanFALKO(LaserScan scan,LaserScanParams scanParams){
falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size());
std::vector<double> doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end());
scanFALKO.fromRanges(doubleRanges);
return scanFALKO;
falkolib::LaserScan
CornerFalko2d::convert2LaserScanFALKO(LaserScan scan,
LaserScanParams scanParams) {
falkolib::LaserScan scanFALKO(scanParams.angle_min_, scanParams.angle_max_,
scan.ranges_raw_.size());
std::vector<double> doubleRanges(scan.ranges_raw_.begin(),
scan.ranges_raw_.end());
scanFALKO.fromRanges(doubleRanges);
return scanFALKO;
}
int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,LaserScanParams scanParams, int scanInterval){
int CornerFalko2d::evaluateNewReferenceScene(LaserScan scan,
LaserScanParams scanParams,
int scanInterval) {
scanNumber=scanNumber+1;
scanNumber = scanNumber + 1;
int NewSceneAdded=0;
int NewSceneAdded = 0;
if (scanNumber % scanInterval == 0 || scanNumber == 1){
if (scanNumber % scanInterval == 0 || scanNumber == 1) {
findLoopClosure(scan, scanParams);
findLoopClosure(scan, scanParams);
if (numberKeypointsMatch < refSceneAddingTh) {
if (numberKeypointsMatch < refSceneAddingTh) {
falkolib::LaserScan scanFALKO= convert2LaserScanFALKO(scan, scanParams);
falkolib::LaserScan scanFALKO = convert2LaserScanFALKO(scan, scanParams);
AddNewReferenceScene(scanFALKO);
AddNewReferenceScene(scanFALKO);
NewSceneAdded=1;
}
NewSceneAdded = 1;
}
}
return NewSceneAdded;
return NewSceneAdded;
}
} // laserscanutils namespace
src/corner_falko_2d.h
+ 74
− 66
View file @ f4f75253
......@@ -8,8 +8,8 @@
#ifndef CORNER_FALKO_2D_H_
#define CORNER_FALKO_2D_H_
#include <iostream>
#include <fstream>
#include <iostream>
/**************************
* laser_scan_utils includes *
......@@ -24,86 +24,94 @@
/**************************
* Falko includes *
**************************/
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/BSC.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/FALKOExtractor.h>
#include <falkolib/Feature/BSCExtractor.h>
#include <falkolib/Feature/CGHExtractor.h>
#include <falkolib/Matching/NNMatcher.h>
#include <falkolib/Matching/AHTMatcher.h>
#include <falkolib/Matching/NNMatcher.h>
namespace laserscanutils
{
namespace laserscanutils {
/** \brief A 2D corner extractor and loop closure computing class
**/
class CornerFalko2d: public falkolib::FALKOExtractor, public falkolib::BSCExtractor<falkolib::FALKO>, public falkolib::NNMatcher<falkolib::FALKO>
{
class CornerFalko2d : public falkolib::FALKOExtractor,
public falkolib::BSCExtractor<falkolib::FALKO>,
public falkolib::NNMatcher<falkolib::FALKO> {
private:
/** \brief Get and stores a scene to use as trained/Reference set of keypoints.
**/
void AddNewReferenceScene (falkolib::LaserScan scanFALKO);
/** \brief Get and stores a scene to use as trained/Reference set of
*keypoints.
**/
void AddNewReferenceScene(falkolib::LaserScan scanFALKO);
public:
/**
* @brief Constructor
* @param _circularSectorNumber number of grid circular sector (BSCExtractor)
* @param _radialRingNumber number of grid radial number (BSCExtractor)
* @param _useKeypointRadius if true, the selected neighborhood points search radius is keypoint one (BSCExtractor)
* @param _radius neighborhood points search radius (BSCExtractor)
*/
CornerFalko2d(int _circularSectorNumber=16, int _radialRingNumber=8, bool _useKeypointRadius = true, double _radius = 0.1) ;
/** \brief Destructor
**/
~CornerFalko2d();
/** \brief compare new scans against the training set in order to find loop closures
**/
void findLoopClosure(LaserScan scan,LaserScanParams scanParams);
/** @brief set a threshold for the minimum number of matched keypoints to consider a matching correct*/
void setKeypointsNumberTh(int _th) {
keypointsNumberTh = _th;
}
/** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object
**/
falkolib::LaserScan convert2LaserScanFALKO(LaserScan scan,LaserScanParams scanParams);
/** \brief Evaluates if New scene is a good candidate to be a Reference Scene. If it is detected as good,
* the scene is added and the functions returns 1. If it is not a good candidate, the function returns 0
* and the scene is not added.
**/
int evaluateNewReferenceScene(LaserScan scan,LaserScanParams scanParams, int scanInterval);
std::vector<std::vector<falkolib::FALKO>> keypointSets;
std::vector<falkolib::FALKO> lastKeypointSet;
std::vector<std::vector<falkolib::BSC>> descriptorSets;
std::vector<falkolib::BSC>lastDescriptorSet;
// Number of Scene matched when performing the loop closure
int numberKeypointsMatch=0;
//Number of keypoints that are matched between 2 scenes when performing the loop closure
int numberSceneMatch=0;
int scanNumber=0;
int scansExtracted=0;
int matchingPosition=-1;
int keypointsNumberTh=2;
// Max number of matched keypoints between 2 scenes for the candidate scene be considered a good New reference scene
int refSceneAddingTh =4;
/**
* @brief Constructor
* @param _circularSectorNumber number of grid circular sector (BSCExtractor)
* @param _radialRingNumber number of grid radial number (BSCExtractor)
* @param _useKeypointRadius if true, the selected neighborhood points search
* radius is keypoint one (BSCExtractor)
* @param _radius neighborhood points search radius (BSCExtractor)
*/
CornerFalko2d(int _circularSectorNumber = 16, int _radialRingNumber = 8,
bool _useKeypointRadius = true, double _radius = 0.1);
/** \brief Destructor
**/
~CornerFalko2d();
/** \brief compare new scans against the training set in order to find loop
*closures
**/
void findLoopClosure(LaserScan scan, LaserScanParams scanParams);
/** @brief set a threshold for the minimum number of matched keypoints to
* consider a matching correct*/
void setKeypointsNumberTh(int _th) { keypointsNumberTh = _th; }
/** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan
*object
**/
falkolib::LaserScan convert2LaserScanFALKO(LaserScan scan,
LaserScanParams scanParams);
/** \brief Evaluates if New scene is a good candidate to be a Reference Scene.
*If it is detected as good,
* the scene is added and the functions returns 1. If it is not a good
*candidate, the function returns 0
* and the scene is not added.
**/
int evaluateNewReferenceScene(LaserScan scan, LaserScanParams scanParams,
int scanInterval);
std::vector<std::vector<falkolib::FALKO>> keypointSets;
std::vector<falkolib::FALKO> lastKeypointSet;
std::vector<std::vector<falkolib::BSC>> descriptorSets;
std::vector<falkolib::BSC> lastDescriptorSet;
// Number of Scene matched when performing the loop closure
int numberKeypointsMatch = 0;
// Number of keypoints that are matched between 2 scenes when performing the
// loop closure
int numberSceneMatch = 0;
int scanNumber = 0;
int scansExtracted = 0;
int matchingPosition = -1;
int keypointsNumberTh = 2;
// Max number of matched keypoints between 2 scenes for the candidate scene be
// considered a good New reference scene
int refSceneAddingTh = 4;
};
} /* namespace laserscanutils */
......
src/loop_closure_base.cpp
+ 8
− 9
View file @ f4f75253
......@@ -5,14 +5,15 @@
* \author: spujol
*/
# include "loop_closure_base.h"
#include "loop_closure_base.h"
namespace laserscanutils{
namespace laserscanutils {
loopClosureBase2d::loopClosureBase2d(){}
loopClosureBase2d::~loopClosureBase2d(){}
loopClosureBase2d::loopClosureBase2d() {}
loopClosureBase2d::~loopClosureBase2d() {}
/*
std::shared_ptr<cornerScene> loopClosureBase2d::extractScene(LaserScan &scan,LaserScanParams &scanParams){
std::shared_ptr<cornerScene> loopClosureBase2d::extractScene(LaserScan
&scan,LaserScanParams &scanParams){
auto NewScene=std::make_shared<cornerScene>();
......@@ -20,8 +21,6 @@ namespace laserscanutils{
}
*/
//void findLoopClosure(std::list<cornerScene>& scenes, const cornerScene newScene){}
// void findLoopClosure(std::list<cornerScene>& scenes, const cornerScene
// newScene){}
}
src/loop_closure_base.h
+ 23
− 23
View file @ f4f75253
......@@ -8,8 +8,8 @@
#ifndef LOOP_CLOSURE_BASE_2D_H_
#define LOOP_CLOSURE_BASE_2D_H_
#include <iostream>
#include <fstream>
#include <iostream>
#include <memory>
/**************************
......@@ -22,43 +22,43 @@
/**************************
* Falko includes *
**************************/
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/BSC.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/FALKOExtractor.h>
#include <falkolib/Feature/BSCExtractor.h>
#include <falkolib/Feature/CGHExtractor.h>
#include <falkolib/Matching/NNMatcher.h>
#include <falkolib/Matching/AHTMatcher.h>
#include <falkolib/Matching/NNMatcher.h>
namespace laserscanutils
{
namespace laserscanutils {
/** \brief A 2base class for loop closure using falko library
/** \brief A 2base class for loop closure using falko library
*/
class loopClosureBase2d{
class loopClosureBase2d {
private:
public:
/** \brief Constructor
**/
loopClosureBase2d();
/** \brief Destructor
**/
~loopClosureBase2d();
/** \brief Constructor
**/
loopClosureBase2d();
/** \brief compare new scans against the trained set in order to find loop closures
**/
//virtual void findLoopClosure(std::list<cornerScene>& scenes, const cornerScene newScene){}
//virtual void findLoopClosure(){}
/** \brief Destructor
**/
~loopClosureBase2d();
/** \brief update the scene struct with keypoints and descriptors
**/
virtual std::shared_ptr<sceneFalko<falkolib::BSC>> extractScene(LaserScan &scan,LaserScanParams &scanParams){}
/** \brief compare new scans against the trained set in order to find loop
*closures
**/
// virtual void findLoopClosure(std::list<cornerScene>& scenes, const
// cornerScene newScene){}
// virtual void findLoopClosure(){}
/** \brief update the scene struct with keypoints and descriptors
**/
virtual std::shared_ptr<sceneFalko<falkolib::BSC>>
extractScene(LaserScan &scan, LaserScanParams &scanParams) {}
};
} /* namespace laserscanutils */
......
src/loop_closure_falko.cpp
+ 46
− 39
View file @ f4f75253
......@@ -5,61 +5,69 @@
* \author: spujol
*/
# include "loop_closure_falko.h"
#include "loop_closure_falko.h"
namespace laserscanutils{
namespace laserscanutils {
//CONSTRUCTOR
template <typename D,typename Extr, typename M>
loopClosureFalko<D,Extr, M>::loopClosureFalko(int _circularSectorNumber, int _radialRingNumber) :
loopClosureBase2d(),
falkolib::FALKOExtractor(),
extractor_(_circularSectorNumber, _radialRingNumber),
matcher_()
{
// FALKO Extractor Parameters
setMinExtractionRange(0.1);
setMaxExtractionRange(25);
enableSubbeam(true);
setNMSRadius(0.1);
setNeighB(0.01);
setBRatio(4);
setGridSectors(16);
// Matcher Extractor Parameters
matcher_.setDistanceThreshold(0.1);
// CONSTRUCTOR
template <typename D, typename Extr, typename M>
loopClosureFalko<D, Extr, M>::loopClosureFalko(int _circularSectorNumber,
int _radialRingNumber)
: loopClosureBase2d(), falkolib::FALKOExtractor(),
extractor_(_circularSectorNumber, _radialRingNumber), matcher_() {
// FALKO Extractor Parameters
setMinExtractionRange(0.1);
setMaxExtractionRange(25);
enableSubbeam(true);
setNMSRadius(0.1);
setNeighB(0.01);
setBRatio(4);
setGridSectors(16);
}
// Matcher Extractor Parameters
matcher_.setDistanceThreshold(0.1);
}
/*
// DESTRUCTOR
template <typename D,typename Extr, typename M>
loopClosureFalko<D,Extr,M>::~loopClosureFalko(){}
*/
template <typename D,typename Extr, typename M>
std::shared_ptr<falkolib::LaserScan> loopClosureFalko<D,Extr, M>::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){
auto scanFALKO=std::make_shared<falkolib::LaserScan>(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size());
std::vector<double> doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end());
((*scanFALKO).fromRanges)(doubleRanges);
return scanFALKO;
}
template <typename D, typename Extr, typename M>
std::shared_ptr<falkolib::LaserScan>
loopClosureFalko<D, Extr, M>::convert2LaserScanFALKO(
LaserScan &scan, LaserScanParams &scanParams) {
auto scanFALKO = std::make_shared<falkolib::LaserScan>(
scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size());
std::vector<double> doubleRanges(scan.ranges_raw_.begin(),
scan.ranges_raw_.end());
((*scanFALKO).fromRanges)(doubleRanges);
return scanFALKO;
}
/*
template <typename D,typename Extr, typename M>
typename loopClosureFalko<D,Extr, M>::sceneFalkoBSCPtr loopClosureFalko<D,Extr, M>::extractScene(LaserScan &scan,LaserScanParams &scanParams){
typename loopClosureFalko<D,Extr, M>::sceneFalkoBSCPtr
loopClosureFalko<D,Extr, M>::extractScene(LaserScan &scan,LaserScanParams
&scanParams){
auto newScene=std::make_shared<sceneFalko<D>>();
auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan, scanParams);
auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan,
scanParams);
// Extract keypoints
extract((*scanFALKO), (newScene->keypointsList));
// Compute descriptors
extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList);
extractor_.compute(*scanFALKO,newScene->keypointsList,
newScene->descriptorsList);
return newScene;
}
*/
/*
template <typename D,typename Extr, typename M>
typename loopClosureFalko<D,Extr, M>::matchLoopClosurePtr matchScene(std::shared_ptr<sceneFalko<D>> scene1,std::shared_ptr<sceneFalko<D>> scene2){
typename loopClosureFalko<D,Extr, M>::matchLoopClosurePtr
matchScene(std::shared_ptr<sceneFalko<D>>
scene1,std::shared_ptr<sceneFalko<D>> scene2){
std::vector<std::pair<int, int> > assoNN;
int matching_number = match(scene1->keypointsList,scene2->keypointsList, assoNN);
int matching_number = match(scene1->keypointsList,scene2->keypointsList,
assoNN);
auto new_match=std::make_shared<matchLoopClosure<D>>();
new_match-> keypointsNumberMatch = matching_number;
int keypoints_number_th=5;
......@@ -74,10 +82,9 @@ namespace laserscanutils{
return new_match;
}
*/
//void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene newScene){}
// void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene
// newScene){}
//Explicitly compile all the templates
template class loopClosureFalko <bsc, bscExtractor, NNMatcher>;
// Explicitly compile all the templates
template class loopClosureFalko<bsc, bscExtractor, NNMatcher>;
}
src/loop_closure_falko.h
+ 76
− 69
View file @ f4f75253
......@@ -8,111 +8,118 @@
#ifndef LOOP_CLOSURE_FALKO_H_
#define LOOP_CLOSURE_FALKO_H_
#include <iostream>
#include <fstream>
#include <memory>
#include <iostream>
#include <iterator>
#include <list>
#include <memory>
/**************************
* laser_scan_utils includes *
**************************/
#include "laser_scan.h"
#include "scene_falko.h"
#include "loop_closure_base.h"
#include "match_loop_closure.h"
#include "scene_falko.h"
/**************************
* Falko includes *
**************************/
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/BSC.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/FALKOExtractor.h>
#include <falkolib/Feature/BSCExtractor.h>
#include <falkolib/Feature/CGHExtractor.h>
#include <falkolib/Matching/NNMatcher.h>
#include <falkolib/Matching/AHTMatcher.h>
#include <falkolib/Matching/NNMatcher.h>
namespace laserscanutils
{
namespace laserscanutils {
typedef falkolib::BSC bsc;
typedef falkolib::BSCExtractor<falkolib::FALKO> bscExtractor;
typedef falkolib::NNMatcher<falkolib::FALKO> NNMatcher;
/** \brief A base class for loop closure using falko library
**/
/** \brief A base class for loop closure using falko library
**/
template <typename D, typename Extr, typename M>
class loopClosureFalko : public loopClosureBase2d, public falkolib::FALKOExtractor{
class loopClosureFalko : public loopClosureBase2d,
public falkolib::FALKOExtractor {
private:
public:
typedef std::shared_ptr<sceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<matchLoopClosure<D>> matchLoopClosurePtr;
typedef std::shared_ptr<sceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<matchLoopClosure<D>> matchLoopClosurePtr;
Extr extractor_;
M matcher_;
/** \brief Constructor
**/
loopClosureFalko(int _circularSectorNumber=16, int _radialRingNumber=8);
/** \brief Destructor
**/
~loopClosureFalko(){}
/** \brief Create and update the scene struct with keypoints and descriptors
**/
sceneFalkoBSCPtr extractScene(LaserScan &scan,LaserScanParams &scanParams) {
auto newScene=std::make_shared<sceneFalko<D>>();
auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan, scanParams);
// Extract keypoints
extract((*scanFALKO), (newScene->keypointsList));
// Compute descriptors
extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList);
return newScene;
}
Extr extractor_;
M matcher_;
/** \brief Constructor
**/
loopClosureFalko(int _circularSectorNumber = 16, int _radialRingNumber = 8);
/** \brief Destructor
**/
~loopClosureFalko() {}
/** \brief Convert scans from laserscanutils::LaserScan to falkolib::LaserScan object
**/
std::shared_ptr<falkolib::LaserScan> convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams);
/** \brief Create and update a matchLoopClosure struct with the info that is produced when matching two given scenes
**/
matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1 ,sceneFalkoBSCPtr scene2){
std::vector<std::pair<int, int> > assoNN;
int matching_number = matcher_.match(scene1->keypointsList,scene2->keypointsList, assoNN);
auto new_match=std::make_shared<matchLoopClosure<D>>();
new_match-> keypointsNumberMatch = matching_number;
int keypoints_number_th=5;
if (matching_number>keypoints_number_th){
new_match-> match = true;
} else {
new_match-> match = false;
}
new_match -> sceneTuple = std::make_tuple(scene1,scene2);
return new_match;
/** \brief Create and update the scene struct with keypoints and descriptors
**/
sceneFalkoBSCPtr extractScene(LaserScan &scan, LaserScanParams &scanParams) {
auto newScene = std::make_shared<sceneFalko<D>>();
auto scanFALKO =
loopClosureFalko<D, Extr, M>::convert2LaserScanFALKO(scan, scanParams);
// Extract keypoints
extract((*scanFALKO), (newScene->keypointsList));
// Compute descriptors
extractor_.compute(*scanFALKO, newScene->keypointsList,
newScene->descriptorsList);
return newScene;
}
/** \brief Convert scans from laserscanutils::LaserScan to
*falkolib::LaserScan object
**/
std::shared_ptr<falkolib::LaserScan>
convert2LaserScanFALKO(LaserScan &scan, LaserScanParams &scanParams);
/** \brief Create and update a matchLoopClosure struct with the info that is
*produced when matching two given scenes
**/
matchLoopClosurePtr matchScene(sceneFalkoBSCPtr scene1,
sceneFalkoBSCPtr scene2) {
std::vector<std::pair<int, int>> assoNN;
int matching_number =
matcher_.match(scene1->keypointsList, scene2->keypointsList, assoNN);
auto new_match = std::make_shared<matchLoopClosure<D>>();
new_match->keypointsNumberMatch = matching_number;
int keypoints_number_th = 5;
if (matching_number > keypoints_number_th) {
new_match->match = true;
} else {
new_match->match = false;
}
new_match->sceneTuple = std::make_tuple(scene1, scene2);
return new_match;
}
/** \brief compare new scans against the trained set in order to find loop closures
**/
std::list<matchLoopClosurePtr> findLoopClosure(std::list<sceneFalkoBSCPtr> &l_scenes, const sceneFalkoBSCPtr &new_scene){
int number_ref_sc = l_scenes.size();
std::list<matchLoopClosurePtr> matchings;
for (int i=0; i<number_ref_sc; i++ ){
auto l_front = l_scenes.begin();
std::advance(l_front, i);
auto new_match = matchScene(*l_front,new_scene);
matchings.push_back(new_match);
}
return matchings;
/** \brief compare new scans against the trained set in order to find loop
*closures
**/
std::list<matchLoopClosurePtr>
findLoopClosure(std::list<sceneFalkoBSCPtr> &l_scenes,
const sceneFalkoBSCPtr &new_scene) {
int number_ref_sc = l_scenes.size();
std::list<matchLoopClosurePtr> matchings;
for (int i = 0; i < number_ref_sc; i++) {
auto l_front = l_scenes.begin();
std::advance(l_front, i);
auto new_match = matchScene(*l_front, new_scene);
matchings.push_back(new_match);
}
return matchings;
}
};
} /* namespace laserscanutils */
......
src/match_loop_closure.h
+ 11
− 14
View file @ f4f75253
......@@ -8,8 +8,8 @@
#ifndef MATCH_LOOP_CLOSURE_H_
#define MATCH_LOOP_CLOSURE_H_
#include <iostream>
#include <fstream>
#include <iostream>
#include <memory>
#include <tuple>
......@@ -18,23 +18,20 @@
**************************/
#include "scene_falko.h"
namespace laserscanutils
{
namespace laserscanutils {
template <typename D>
struct matchLoopClosure{
//std::shared_ptr<sceneFalko<D>> scene1;
//std::shared_ptr<sceneFalko<D>> scene2;
// tuple that stores the pointers of two matched scenes
std::tuple<std::shared_ptr<sceneFalko<D>>, std::shared_ptr<sceneFalko<D>>> sceneTuple;
bool match;
int keypointsNumberMatch;
double score;
template <typename D> struct matchLoopClosure {
// std::shared_ptr<sceneFalko<D>> scene1;
// std::shared_ptr<sceneFalko<D>> scene2;
// tuple that stores the pointers of two matched scenes
std::tuple<std::shared_ptr<sceneFalko<D>>, std::shared_ptr<sceneFalko<D>>>
sceneTuple;
bool match;
int keypointsNumberMatch;
double score;
};
} /* namespace laserscanutils */
#endif /* MATCH_LOOP_CLOSURE_H_ */
src/scene_base.h
+ 5
− 8
View file @ f4f75253
......@@ -8,19 +8,16 @@
#ifndef SCENE_BASE_H_
#define SCENE_BASE_H_
#include <iostream>
#include <fstream>
#include <iostream>
namespace laserscanutils {
namespace laserscanutils
{
struct sceneBase{
//std::vector<falkolib::FALKO> keypointsList;
//std::vector<D> descriptorsList;
struct sceneBase {
// std::vector<falkolib::FALKO> keypointsList;
// std::vector<D> descriptorsList;
};
} /* namespace laserscanutils */
#endif /* SCENE_BASE_H_ */
src/scene_falko.h
+ 8
− 14
View file @ f4f75253
......@@ -8,41 +8,35 @@
#ifndef SCENE_FALKO_H_
#define SCENE_FALKO_H_
#include <iostream>
#include <fstream>
#include <iostream>
/**************************
* LaserScanUtils includes *
**************************/
#include "scene_base.h"
/**************************
* Falko includes *
**************************/
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/BSC.h>
#include <falkolib/Feature/CGH.h>
#include <falkolib/Feature/FALKO.h>
#include <falkolib/Feature/FALKOExtractor.h>
#include <falkolib/Feature/BSCExtractor.h>
#include <falkolib/Feature/CGHExtractor.h>
#include <falkolib/Matching/NNMatcher.h>
#include <falkolib/Matching/AHTMatcher.h>
#include <falkolib/Matching/NNMatcher.h>
namespace laserscanutils
{
namespace laserscanutils {
template <typename D>
struct sceneFalko : public sceneBase{
std::vector<falkolib::FALKO> keypointsList;
std::vector<D> descriptorsList;
template <typename D> struct sceneFalko : public sceneBase {
std::vector<falkolib::FALKO> keypointsList;
std::vector<D> descriptorsList;
};
} /* namespace laserscanutils */
#endif /* SCENE_FALKO_H_ */
test/gtest_loop_closure_falko.cpp
+ 33
− 37
View file @ f4f75253
#include "gtest/utils_gtest.h"
#include "testData2.h"
#include "../src/loop_closure_base.h"
#include "../src/loop_closure_falko.h"
#include "testData2.h"
#include "gtest/utils_gtest.h"
using namespace laserscanutils;
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
{
int scanSize = 1440;
LaserScan scan;
LaserScanParams laserParams;
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) {
int scanSize = 1440;
LaserScan scan;
LaserScanParams laserParams;
laserParams.angle_min_=0;
laserParams.angle_max_=2.0 * M_PI;
laserParams.angle_min_ = 0;
laserParams.angle_max_ = 2.0 * M_PI;
for (int i=0; i<scanSize; i++) {
scan.ranges_raw_.push_back(testRanges1[i]);
}
for (int i = 0; i < scanSize; i++) {
scan.ranges_raw_.push_back(testRanges1[i]);
}
loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko;
auto new_scene=LCFalko.extractScene(scan, laserParams);
auto new_scene = LCFalko.extractScene(scan, laserParams);
int detectedKeypoints =((*new_scene).keypointsList).size();
int detectedKeypoints = ((*new_scene).keypointsList).size();
int detectedDescriptors =new_scene->descriptorsList.size();
int detectedDescriptors = new_scene->descriptorsList.size();
ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedDescriptors, 18);
//Test matcher
// Test matcher
auto new_match =LCFalko.matchScene(new_scene,new_scene);
auto new_match = LCFalko.matchScene(new_scene, new_scene);
int keypoints_matched = new_match->keypointsNumberMatch;
ASSERT_EQ(keypoints_matched, 18);
//TEST findLoopClosure
// TEST findLoopClosure
std::list<std::shared_ptr<sceneFalko<bsc>>> ref_scenes;
......@@ -48,9 +45,9 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ref_scenes.push_back(new_scene);
auto matchings = LCFalko.findLoopClosure(ref_scenes,new_scene);
auto matchings = LCFalko.findLoopClosure(ref_scenes, new_scene);
int matchings_number =matchings.size();
int matchings_number = matchings.size();
ASSERT_EQ(matchings_number, 2);
......@@ -60,36 +57,35 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ASSERT_EQ(there_is_match, true);
// PRINTF("All good at TestTest::DummyTestExample !\n");
// PRINTF("All good at TestTest::DummyTestExample !\n");
}
TEST(loop_closure_falko2, convert2laserScanFalko)
{
int scanSize = 1440;
TEST(loop_closure_falko2, convert2laserScanFalko) {
int scanSize = 1440;
LaserScan scan;
LaserScanParams laserParams;
LaserScan scan;
LaserScanParams laserParams;
laserParams.angle_min_=0;
laserParams.angle_max_=2.0 * M_PI;
laserParams.angle_min_ = 0;
laserParams.angle_max_ = 2.0 * M_PI;
for (int i=0; i<scanSize; i++) {
scan.ranges_raw_.push_back(testRanges1[i]);
}
for (int i = 0; i < scanSize; i++) {
scan.ranges_raw_.push_back(testRanges1[i]);
}
loopClosureFalko<bsc, bscExtractor, NNMatcher> LCFalko;
std::shared_ptr<falkolib::LaserScan> scanFALKO=LCFalko.convert2LaserScanFALKO(scan, laserParams);
std::shared_ptr<falkolib::LaserScan> scanFALKO =
LCFalko.convert2LaserScanFALKO(scan, laserParams);
int firstPoint = ((*scanFALKO).ranges)[0];
ASSERT_EQ(firstPoint, 250);
// PRINTF("All good at TestTest::DummyTestExample !\n");
// PRINTF("All good at TestTest::DummyTestExample !\n");
}
int main(int argc, char **argv)
{
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment