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

renaming use_descriptors

parent 20a993d6
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
Hide whitespace changes
Inline Side-by-side
src/loop_closure_falko.h
+ 141
− 138
View file @ 6f7311ce
......@@ -42,152 +42,155 @@ namespace laserscanutils {
typedef falkolib::BSCExtractor<falkolib::FALKO> bscExtractor;
typedef falkolib::CGHExtractor<falkolib::FALKO> cghExtractor;
template <typename T, typename D> using nn_matcher = falkolib::NNMatcher<T, D>;
template <typename T, typename D>
using aht_matcher = falkolib::AHTMatcher<T, D>;
struct ParameterLoopClosureFalko {
// Keypoints extractor Default
double min_extraction_range_ = 0.1;
double max_extraction_range_ = 25;
bool enable_subbeam_ = true;
double nms_radius_ = 0.1;
double neigh_b_ = 0.01;
double b_ratio_ = 4;
int grid_sectors_ = 16;
// Descriptors parameters Default
int circularSectorNumber_ = 16;
int radialRingNumber_ = 8;
// matcher threshold Default
double matcher_distance_th_ = 0.2;
int keypoints_number_th_ = 5;
int match_type_ = 1; // match_type=1-> uses keypoints and descriptors for
// matching. match_type=2-> uses only keypoints for
// matching
double matcher_ddesc_th_ = 0.2;
// aht matcher
double xRes_ = 0.1;
double yRes_ = 0.1;
double thetaRes_ = 0.04;
double xAbsMax_ = 5;
double yAbsMax_ = 5;
double thetaAbsMax_ = 1.57;
template <typename T, typename D> using nn_matcher = falkolib::NNMatcher<T, D>;
template <typename T, typename D> using aht_matcher = falkolib::AHTMatcher<T, D>;
struct ParameterLoopClosureFalko
{
// Keypoints extractor Default
double min_extraction_range_ = 0.1;
double max_extraction_range_ = 25;
bool enable_subbeam_ = true;
double nms_radius_ = 0.1;
double neigh_b_ = 0.01;
double b_ratio_ = 4;
int grid_sectors_ = 16;
// Descriptors parameters Default
int circularSectorNumber_ = 16;
int radialRingNumber_ = 8;
// matcher threshold Default
double matcher_distance_th_ = 0.2;
int keypoints_number_th_ = 5;
bool use_descriptors_ = 1; // match_type=1-> uses keypoints and descriptors
// match_type=0-> uses only keypoints
// matching
double matcher_ddesc_th_ = 0.2;
// aht matcher
double xRes_ = 0.1;
double yRes_ = 0.1;
double thetaRes_ = 0.04;
double xAbsMax_ = 5;
double yAbsMax_ = 5;
double thetaAbsMax_ = 1.57;
};
/** \brief A base class for loop closure using falko library
**/
template <typename D, typename Extr, template <typename, typename> typename M>
class LoopClosureFalko : public LoopClosureBase2d,
public falkolib::FALKOExtractor {
private:
public:
typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
Extr extractor_;
M<falkolib::FALKO, D> matcher_;
// M<D, D> matcher_desc_;
/** \brief Constructor for nn_matcher
**/
LoopClosureFalko(ParameterLoopClosureFalko _param)
: LoopClosureBase2d(), falkolib::FALKOExtractor(),
extractor_(_param.circularSectorNumber_, _param.radialRingNumber_),
matcher_() {
// FALKO Extractor Parameters
setMinExtractionRange(_param.min_extraction_range_);
setMaxExtractionRange(_param.max_extraction_range_);
enableSubbeam(_param.enable_subbeam_);
setNMSRadius(_param.nms_radius_);
setNeighB(_param.neigh_b_);
setBRatio(_param.b_ratio_);
setGridSectors(_param.grid_sectors_);
// Matcher Extractor Parameters
matcher_.setDistanceThreshold(_param.matcher_distance_th_);
matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_);
keypoints_number_th_ = _param.keypoints_number_th_;
match_type_ = _param.match_type_;
};
/** \brief Destructor
**/
~LoopClosureFalko() {}
/** \brief Create and update the scene struct with keypoints and descriptors
**/
sceneBasePtr extractScene(const LaserScan &_scan,
const LaserScanParams &_scan_params) override {
auto new_scene = std::make_shared<SceneFalko<D>>();
auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
// Extract keypoints
extract(*scan_falko, (new_scene->keypoints_list_));
// Compute descriptors
extractor_.compute(*scan_falko, new_scene->keypoints_list_,
new_scene->descriptors_list_);
return new_scene;
}
/** \brief Convert scans from laserscanutils::LaserScan to
*falkolib::LaserScan object
**/
laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan,
const LaserScanParams &_scan_params) {
auto scan_falko = std::make_shared<falkolib::LaserScan>(
_scan_params.angle_min_, _scan_params.angle_max_,
_scan.ranges_raw_.size());
std::vector<double> double_ranges(_scan.ranges_raw_.begin(),
_scan.ranges_raw_.end());
scan_falko->fromRanges(double_ranges);
return scan_falko;
}
/** \brief Create and update a matchLoopClosure struct with the info that is
*produced when matching two given scenes
**/
MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1,
sceneBasePtr _scene_2) override {
std::vector<std::pair<int, int>> asso_nn;
auto scene_1_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_1);
auto scene_2_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_2);
int matching_number = 0;
if (match_type_ == 1) {
matching_number = matcher_.match(scene_1_falko->keypoints_list_,
scene_2_falko->keypoints_list_, asso_nn);
} else if (match_type_ == 2) {
matching_number = matcher_.match(
scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_,
scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_,
asso_nn);
class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor
{
private:
public:
typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
Extr extractor_;
M<falkolib::FALKO, D> matcher_;
// M<D, D> matcher_desc_;
/** \brief Constructor for nn_matcher
**/
LoopClosureFalko(ParameterLoopClosureFalko _param)
: LoopClosureBase2d()
, falkolib::FALKOExtractor()
, extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
, matcher_()
{
// FALKO Extractor Parameters
setMinExtractionRange(_param.min_extraction_range_);
setMaxExtractionRange(_param.max_extraction_range_);
enableSubbeam(_param.enable_subbeam_);
setNMSRadius(_param.nms_radius_);
setNeighB(_param.neigh_b_);
setBRatio(_param.b_ratio_);
setGridSectors(_param.grid_sectors_);
// Matcher Extractor Parameters
matcher_.setDistanceThreshold(_param.matcher_distance_th_);
matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_);
keypoints_number_th_ = _param.keypoints_number_th_;
use_descriptors_ = _param.use_descriptors_;
};
/** \brief Destructor
**/
~LoopClosureFalko() {}
/** \brief Create and update the scene struct with keypoints and descriptors
**/
sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override
{
auto new_scene = std::make_shared<SceneFalko<D>>();
auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
// Extract keypoints
extract(*scan_falko, (new_scene->keypoints_list_));
// Compute descriptors
extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_);
return new_scene;
}
auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number;
if (matching_number > keypoints_number_th_) {
new_match->match = computeTransform(scene_1_falko->keypoints_list_,
scene_2_falko->keypoints_list_,
asso_nn, new_match->transform);
} else {
new_match->match = false;
/** \brief Convert scans from laserscanutils::LaserScan to
*falkolib::LaserScan object
**/
laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params)
{
auto scan_falko = std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, _scan_params.angle_max_,
_scan.ranges_raw_.size());
std::vector<double> double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end());
scan_falko->fromRanges(double_ranges);
return scan_falko;
}
/** \brief Create and update a matchLoopClosure struct with the info that is
*produced when matching two given scenes
**/
MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override
{
std::vector<std::pair<int, int>> asso_nn;
auto scene_1_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_1);
auto scene_2_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_2);
int matching_number = 0;
if (use_descriptors_ == 1)
{
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn);
}
else if (use_descriptors_ == 0)
{
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_,
scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn);
}
auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number;
if (matching_number > keypoints_number_th_)
{
new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_,
asso_nn, new_match->transform);
}
else
{
new_match->match = false;
}
new_match->scene_1 = _scene_1;
new_match->scene_2 = _scene_2;
new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(),
scene_2_falko->keypoints_list_.size());
//}
return new_match;
}
new_match->scene_1 = _scene_1;
new_match->scene_2 = _scene_2;
new_match->score = (double)matching_number /
(double)std::min(scene_1_falko->keypoints_list_.size(),
scene_2_falko->keypoints_list_.size());
//}
return new_match;
}
int keypoints_number_th_;
int match_type_;
int keypoints_number_th_;
bool use_descriptors_;
};
} /* namespace laserscanutils */
......
test/gtest_loop_closure_falko.cpp
+ 51
− 58
View file @ 6f7311ce
......@@ -5,64 +5,57 @@
using namespace laserscanutils;
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) {
// Initialization
int scan_size = 1440;
LaserScan scan, scan2;
LaserScanParams laser_params;
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
for (int i = 0; i < scan_size; i++) {
scan.ranges_raw_.push_back(testRanges1[i]);
scan2.ranges_raw_.push_back(testRanges2[i]);
}
ParameterLoopClosureFalko param;
LoopClosureFalko<bsc, bscExtractor, nn_matcher> loop_cl_falko(param);
// Test convert2LaserScanFALKO
std::shared_ptr<falkolib::LaserScan> scan_falko =
loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
int firstPoint = scan_falko->ranges[0];
ASSERT_EQ(firstPoint, 250);
// Test extractScene
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(
loop_cl_falko.extractScene(scan, laser_params));
auto new_scene2 = std::static_pointer_cast<SceneFalko<bsc>>(
loop_cl_falko.extractScene(scan2, laser_params));
int detectedKeypoints = new_scene->keypoints_list_.size();
int detectedDescriptors = new_scene->descriptors_list_.size();
ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedDescriptors, 18);
// Test matcheScene
auto new_match = loop_cl_falko.matchScene(new_scene, new_scene);
ASSERT_EQ(new_match->keypoints_number_match, 18);
// TesT findLoopClosure
//auto ref_scenes = std::make_shared<SceneFalkoList<bsc>>();
//ref_scenes->scenes_.emplace_back(new_scene);
//ref_scenes->scenes_.emplace_back(new_scene2);
std::list<std::shared_ptr<SceneBase>> ref_scenes;
ref_scenes.emplace_back(new_scene);
ref_scenes.emplace_back(new_scene2);
auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene);
ASSERT_EQ(matchings.size(), 2);
auto best_match = matchings.rbegin()->second;
ASSERT_EQ(best_match->match, true);
ASSERT_EQ(best_match->scene_1, new_scene);
ASSERT_EQ(best_match->scene_2, new_scene);
ASSERT_EQ(best_match->score, 1);
// PRINTF("All good at TestTest::DummyTestExample !\n");
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
{
// Initialization
int scan_size = 1440;
LaserScan scan, scan2;
LaserScanParams laser_params;
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
for (int i = 0; i < scan_size; i++)
{
scan.ranges_raw_.push_back(testRanges1[i]);
scan2.ranges_raw_.push_back(testRanges2[i]);
}
ParameterLoopClosureFalko param;
LoopClosureFalko<bsc, bscExtractor, nn_matcher> loop_cl_falko(param);
// Test convert2LaserScanFALKO
std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
int firstPoint = scan_falko->ranges[0];
ASSERT_EQ(firstPoint, 250);
// Test extractScene
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan, laser_params));
auto new_scene2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan2, laser_params));
int detectedKeypoints = new_scene->keypoints_list_.size();
int detectedDescriptors = new_scene->descriptors_list_.size();
ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedDescriptors, 18);
// Test matcheScene
auto new_match = loop_cl_falko.matchScene(new_scene, new_scene);
ASSERT_EQ(new_match->keypoints_number_match, 18);
std::list<std::shared_ptr<SceneBase>> ref_scenes;
ref_scenes.emplace_back(new_scene);
ref_scenes.emplace_back(new_scene2);
auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene);
ASSERT_EQ(matchings.size(), 2);
auto best_match = matchings.rbegin()->second;
ASSERT_EQ(best_match->match, true);
ASSERT_EQ(best_match->scene_1, new_scene);
ASSERT_EQ(best_match->scene_2, new_scene);
ASSERT_EQ(best_match->score, 1);
}
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
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