loop_closure_falko.cpp

/**
 * \file loop_closure_base_2d.h
 *
 *  Created on: Feb 9, 2021
 *      \author: spujol
 */

# include "loop_closure_falko.h"

namespace laserscanutils{

    //CONSTRUCTOR
    template <typename D,typename Extr, typename M>
    loopClosureFalko<D,Extr, M>::loopClosureFalko(int _circularSectorNumber, int _radialRingNumber) : 
    loopClosureBase2d(), 
    falkolib::FALKOExtractor(),
    extractor_(_circularSectorNumber, _radialRingNumber),
    matcher_()
    {
        // FALKO Extractor Parameters
        setMinExtractionRange(0.1);
        setMaxExtractionRange(25);
        enableSubbeam(true);
        setNMSRadius(0.1);
        setNeighB(0.01);
        setBRatio(4);
        setGridSectors(16);

        // Matcher Extractor Parameters
        matcher_.setDistanceThreshold(0.1);

    }


    // DESTRUCTOR
    template <typename D,typename Extr, typename M>
    loopClosureFalko<D,Extr,M>::~loopClosureFalko(){}

    template <typename D,typename Extr, typename M>
    std::shared_ptr<falkolib::LaserScan> loopClosureFalko<D,Extr, M>::convert2LaserScanFALKO(LaserScan &scan,LaserScanParams &scanParams){
        auto scanFALKO=std::make_shared<falkolib::LaserScan>(scanParams.angle_min_, scanParams.angle_max_, scan.ranges_raw_.size());
        std::vector<double> doubleRanges(scan.ranges_raw_.begin(), scan.ranges_raw_.end());
        ((*scanFALKO).fromRanges)(doubleRanges);
        return scanFALKO;
    }

    template <typename D,typename Extr, typename M>
    std::shared_ptr<sceneFalko<D>> loopClosureFalko<D,Extr, M>::extractScene(LaserScan &scan,LaserScanParams &scanParams){
        auto newScene=std::make_shared<sceneFalko<D>>();
        auto scanFALKO =loopClosureFalko<D,Extr,M>::convert2LaserScanFALKO(scan, scanParams);
        // Extract keypoints
        extract((*scanFALKO), (newScene->keypointsList));

        // Compute descriptors
        extractor_.compute(*scanFALKO,newScene->keypointsList, newScene->descriptorsList);
        return newScene;
    }


    //void findLoopClosure(std::list<sceneFalko>& scenes, const cornerScene newScene){}

    //Explicitly compile all the templates
    template class loopClosureFalko <bsc, bscExtractor, NNMatcher>;
}