Skip to content
Snippets Groups Projects
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_ */