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"
...@@ -42,152 +42,155 @@ namespace laserscanutils { ...@@ -42,152 +42,155 @@ namespace laserscanutils {
typedef falkolib::BSCExtractor<falkolib::FALKO> bscExtractor; typedef falkolib::BSCExtractor<falkolib::FALKO> bscExtractor;
typedef falkolib::CGHExtractor<falkolib::FALKO> cghExtractor; typedef falkolib::CGHExtractor<falkolib::FALKO> cghExtractor;
template <typename T, typename D> using nn_matcher = falkolib::NNMatcher<T, D>; template <typename T, typename D> using nn_matcher = falkolib::NNMatcher<T, D>;
template <typename T, typename D> template <typename T, typename D> using aht_matcher = falkolib::AHTMatcher<T, D>;
using aht_matcher = falkolib::AHTMatcher<T, D>;
struct ParameterLoopClosureFalko
struct ParameterLoopClosureFalko { {
// Keypoints extractor Default // Keypoints extractor Default
double min_extraction_range_ = 0.1; double min_extraction_range_ = 0.1;
double max_extraction_range_ = 25; double max_extraction_range_ = 25;
bool enable_subbeam_ = true; bool enable_subbeam_ = true;
double nms_radius_ = 0.1; double nms_radius_ = 0.1;
double neigh_b_ = 0.01; double neigh_b_ = 0.01;
double b_ratio_ = 4; double b_ratio_ = 4;
int grid_sectors_ = 16; int grid_sectors_ = 16;
// Descriptors parameters Default // Descriptors parameters Default
int circularSectorNumber_ = 16; int circularSectorNumber_ = 16;
int radialRingNumber_ = 8; int radialRingNumber_ = 8;
// matcher threshold Default // matcher threshold Default
double matcher_distance_th_ = 0.2; double matcher_distance_th_ = 0.2;
int keypoints_number_th_ = 5; int keypoints_number_th_ = 5;
int match_type_ = 1; // match_type=1-> uses keypoints and descriptors for bool use_descriptors_ = 1; // match_type=1-> uses keypoints and descriptors
// matching. match_type=2-> uses only keypoints for // match_type=0-> uses only keypoints
// matching
double matcher_ddesc_th_ = 0.2; // matching
double matcher_ddesc_th_ = 0.2;
// aht matcher
double xRes_ = 0.1; // aht matcher
double yRes_ = 0.1; double xRes_ = 0.1;
double thetaRes_ = 0.04; double yRes_ = 0.1;
double xAbsMax_ = 5; double thetaRes_ = 0.04;
double yAbsMax_ = 5; double xAbsMax_ = 5;
double thetaAbsMax_ = 1.57; double yAbsMax_ = 5;
double thetaAbsMax_ = 1.57;
}; };
/** \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, template <typename, typename> typename M> template <typename D, typename Extr, template <typename, typename> typename M>
class LoopClosureFalko : public LoopClosureBase2d, class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor
public falkolib::FALKOExtractor { {
private: private:
public: public:
typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr; typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr; typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
Extr extractor_; Extr extractor_;
M<falkolib::FALKO, D> matcher_; M<falkolib::FALKO, D> matcher_;
// M<D, D> matcher_desc_; // M<D, D> matcher_desc_;
/** \brief Constructor for nn_matcher /** \brief Constructor for nn_matcher
**/ **/
LoopClosureFalko(ParameterLoopClosureFalko _param) LoopClosureFalko(ParameterLoopClosureFalko _param)
: LoopClosureBase2d(), falkolib::FALKOExtractor(), : LoopClosureBase2d()
extractor_(_param.circularSectorNumber_, _param.radialRingNumber_), , falkolib::FALKOExtractor()
matcher_() { , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
// FALKO Extractor Parameters , matcher_()
setMinExtractionRange(_param.min_extraction_range_); {
setMaxExtractionRange(_param.max_extraction_range_); // FALKO Extractor Parameters
enableSubbeam(_param.enable_subbeam_); setMinExtractionRange(_param.min_extraction_range_);
setNMSRadius(_param.nms_radius_); setMaxExtractionRange(_param.max_extraction_range_);
setNeighB(_param.neigh_b_); enableSubbeam(_param.enable_subbeam_);
setBRatio(_param.b_ratio_); setNMSRadius(_param.nms_radius_);
setGridSectors(_param.grid_sectors_); setNeighB(_param.neigh_b_);
setBRatio(_param.b_ratio_);
// Matcher Extractor Parameters setGridSectors(_param.grid_sectors_);
matcher_.setDistanceThreshold(_param.matcher_distance_th_);
matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_); // Matcher Extractor Parameters
keypoints_number_th_ = _param.keypoints_number_th_; matcher_.setDistanceThreshold(_param.matcher_distance_th_);
match_type_ = _param.match_type_; matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_);
}; keypoints_number_th_ = _param.keypoints_number_th_;
use_descriptors_ = _param.use_descriptors_;
/** \brief Destructor };
**/
~LoopClosureFalko() {} /** \brief Destructor
**/
/** \brief Create and update the scene struct with keypoints and descriptors ~LoopClosureFalko() {}
**/
sceneBasePtr extractScene(const LaserScan &_scan, /** \brief Create and update the scene struct with keypoints and descriptors
const LaserScanParams &_scan_params) override { **/
auto new_scene = std::make_shared<SceneFalko<D>>(); sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override
auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params); {
// Extract keypoints auto new_scene = std::make_shared<SceneFalko<D>>();
extract(*scan_falko, (new_scene->keypoints_list_)); auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
// Compute descriptors // Extract keypoints
extractor_.compute(*scan_falko, new_scene->keypoints_list_, extract(*scan_falko, (new_scene->keypoints_list_));
new_scene->descriptors_list_); // Compute descriptors
return new_scene; 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);
} }
auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number; /** \brief Convert scans from laserscanutils::LaserScan to
if (matching_number > keypoints_number_th_) { *falkolib::LaserScan object
new_match->match = computeTransform(scene_1_falko->keypoints_list_, **/
scene_2_falko->keypoints_list_, laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params)
asso_nn, new_match->transform); {
} else { auto scan_falko = std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, _scan_params.angle_max_,
new_match->match = false; _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; int keypoints_number_th_;
bool use_descriptors_;
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_;
}; };
} /* namespace laserscanutils */ } /* namespace laserscanutils */
......
...@@ -5,64 +5,57 @@ ...@@ -5,64 +5,57 @@
using namespace laserscanutils; using namespace laserscanutils;
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) { TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
// Initialization {
int scan_size = 1440; // Initialization
LaserScan scan, scan2; int scan_size = 1440;
LaserScanParams laser_params; LaserScan scan, scan2;
laser_params.angle_min_ = 0; LaserScanParams laser_params;
laser_params.angle_max_ = 2.0 * M_PI; laser_params.angle_min_ = 0;
for (int i = 0; i < scan_size; i++) { laser_params.angle_max_ = 2.0 * M_PI;
scan.ranges_raw_.push_back(testRanges1[i]); for (int i = 0; i < scan_size; i++)
scan2.ranges_raw_.push_back(testRanges2[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);
ParameterLoopClosureFalko param;
// Test convert2LaserScanFALKO LoopClosureFalko<bsc, bscExtractor, nn_matcher> loop_cl_falko(param);
std::shared_ptr<falkolib::LaserScan> scan_falko =
loop_cl_falko.convert2LaserScanFALKO(scan, laser_params); // Test convert2LaserScanFALKO
int firstPoint = scan_falko->ranges[0]; std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
int firstPoint = scan_falko->ranges[0];
ASSERT_EQ(firstPoint, 250);
ASSERT_EQ(firstPoint, 250);
// Test extractScene
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>( // Test extractScene
loop_cl_falko.extractScene(scan, laser_params)); 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>>( auto new_scene2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan2, laser_params));
loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints = new_scene->keypoints_list_.size();
int detectedKeypoints = new_scene->keypoints_list_.size(); int detectedDescriptors = new_scene->descriptors_list_.size();
int detectedDescriptors = new_scene->descriptors_list_.size(); ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedDescriptors, 18);
ASSERT_EQ(detectedDescriptors, 18);
// Test matcheScene
// Test matcheScene auto new_match = loop_cl_falko.matchScene(new_scene, new_scene);
auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); ASSERT_EQ(new_match->keypoints_number_match, 18);
ASSERT_EQ(new_match->keypoints_number_match, 18);
std::list<std::shared_ptr<SceneBase>> ref_scenes;
// TesT findLoopClosure ref_scenes.emplace_back(new_scene);
//auto ref_scenes = std::make_shared<SceneFalkoList<bsc>>(); ref_scenes.emplace_back(new_scene2);
//ref_scenes->scenes_.emplace_back(new_scene);
//ref_scenes->scenes_.emplace_back(new_scene2); auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene);
std::list<std::shared_ptr<SceneBase>> ref_scenes; ASSERT_EQ(matchings.size(), 2);
ref_scenes.emplace_back(new_scene); auto best_match = matchings.rbegin()->second;
ref_scenes.emplace_back(new_scene2); ASSERT_EQ(best_match->match, true);
ASSERT_EQ(best_match->scene_1, new_scene);
auto matchings = loop_cl_falko.findLoopClosure(ref_scenes, new_scene); ASSERT_EQ(best_match->scene_2, new_scene);
ASSERT_EQ(best_match->score, 1);
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");
} }
int main(int argc, char **argv) { int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv); {
return RUN_ALL_TESTS(); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment