-
Sergi Pujol Badell authoredSergi Pujol Badell authored
loop_closure_falko.h 7.52 KiB
/**
* \file loop_closure_base_2d.h
*
* Created on: Feb 9, 2021
* \author: spujol
*/
#ifndef LOOP_CLOSURE_FALKO_H_
#define LOOP_CLOSURE_FALKO_H_
#include <fstream>
#include <iostream>
#include <iterator>
#include <list>
#include <math.h>
#include <memory>
/**************************
* laser_scan_utils includes *
**************************/
#include "laser_scan.h"
#include "loop_closure_base.h"
#include "match_loop_closure_scene.h"
#include "scene_falko.h"
/**************************
* Falko includes *
**************************/
#include <falkolib/Matching/AHTMatcher.h>
#include <falkolib/Matching/NNMatcher.h>
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>;
/** \brief Struct class that store falkolib parameters
**/
struct ParameterLoopClosureFalko
{
// Keypoints extractor Default
double min_extraction_range_ = 0;
double max_extraction_range_ = 30;
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 class for loop closure using falko library
*
* The class is a wrapper of the falkolib that is designed to be used for loop closures in the wolf problem
*
* It extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors
*
* It matches a target scene against a list of reference scenes.
*
* The reference scenes are found from a search of the previous captures
*
* Diferent types of descriptors can be used, and are specified as template parameters.
*
* \tparam D Descriptor type. <bsc> or <cgh>
* \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
* \tparam M Matcher type <nn_matcher> or <aht_matcher>
* \param _param parameter struct with falko lib parameters
**/
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_;
/** \brief Constructor
* \param _param parameter struct with falko lib parameters
**/
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;
}
/** \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
* \param _scene_1 reference scene struct
* \param _scene_2 target scene struct
**/
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_ == 0)
{
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn);
}
else if (use_descriptors_ == 1)
{
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());
new_match->transform_vector.head(2) = new_match->transform.translation();
new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
return new_match;
}
int keypoints_number_th_;
bool use_descriptors_;
};
} /* namespace laserscanutils */
#endif /* LOOP_CLOSURE_FALKO_H_ */