diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index 75a194b74c6d6d861a6f4bbe5dfdf3c9405e1330..ecfda987de2ecb0d345fb3019600fde7a5d47031 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -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 */
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index e88305cdd2d49195681fc40d869d57d3a98beadf..f01b38bf2bda32e0219447e79d4263a84dd84697 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -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();
}