template class mrpt::graphslam::deciders::CLoopCloserERD

Overview

Edge Registration Decider scheme specialized in Loop Closing.

#include <mrpt/graphslam/ERD/CLoopCloserERD.h>

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CLoopCloserERD: public mrpt::graphslam::deciders::CRangeScanEdgeRegistrationDecider
{
public:
    // typedefs

    typedef CRangeScanEdgeRegistrationDecider<GRAPH_T> parent_t;
    typedef typename GRAPH_T::constraint_t constraint_t;
    typedef typename GRAPH_T::constraint_t::type_value pose_t;
    typedef typename GRAPH_T::global_pose_t global_pose_t;
    typedef CLoopCloserERD<GRAPH_T> decider_t;
    typedef typename parent_t::range_ops_t range_ops_t;
    typedef typename parent_t::nodes_to_scans2D_t nodes_to_scans2D_t;
    typedef std::vector<std::vector<uint32_t>> partitions_t;
    typedef typename GRAPH_T::edges_map_t::const_iterator edges_citerator;
    typedef typename GRAPH_T::edges_map_t::iterator edges_iterator;
    typedef typename mrpt::graphs::detail::THypothesis<GRAPH_T> hypot_t;
    typedef std::vector<hypot_t> hypots_t;
    typedef std::vector<hypot_t*> hypotsp_t;
    typedef std::map<std::pair<hypot_t*, hypot_t*>, double> hypotsp_to_consist_t;
    typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> path_t;
    typedef std::vector<path_t> paths_t;
    typedef mrpt::graphslam::detail::TNodeProps<GRAPH_T> node_props_t;

    // structs

    struct TGenerateHypotsPoolAdParams;
    struct TGetICPEdgeAdParams;
    struct TLaserParams;
    struct TLoopClosureParams;

    // construction

    CLoopCloserERD();

    // methods

    virtual bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation);
    virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager* win_manager);
    virtual void notifyOfWindowEvents(const std::map<std::string, bool>& events_occurred);
    virtual void getEdgesStats(std::map<std::string, int>* edge_type_to_num) const;
    virtual void initializeVisuals();
    virtual void updateVisuals();
    virtual void loadParams(const std::string& source_fname);
    virtual void printParams() const;
    virtual void getDescriptiveReport(std::string* report_str) const;
    void getCurrentPartitions(partitions_t& partitions_out) const;
    const partitions_t& getCurrentPartitions() const;
    size_t getDijkstraExecutionThresh() const;
    void setDijkstraExecutionThresh(size_t new_thresh);

    void generateHypotsPool(
        const std::vector<uint32_t>& groupA,
        const std::vector<uint32_t>& groupB,
        hypotsp_t* generated_hypots,
        const TGenerateHypotsPoolAdParams* ad_params = nullptr
        );

    void generatePWConsistenciesMatrix(
        const std::vector<uint32_t>& groupA,
        const std::vector<uint32_t>& groupB,
        const hypotsp_t& hypots_pool,
        mrpt::math::CMatrixDouble* consist_matrix,
        const paths_t* groupA_opt_paths = nullptr,
        const paths_t* groupB_opt_paths = nullptr
        );

    void evalPWConsistenciesMatrix(const mrpt::math::CMatrixDouble& consist_matrix, const hypotsp_t& hypots_pool, hypotsp_t* valid_hypots);
};

Inherited Members

public:
    // typedefs

    typedef mrpt::graphslam::CRegistrationDeciderOrOptimizer<GRAPH_T> parent;
    typedef typename GRAPH_T::constraint_t constraint_t;
    typedef typename GRAPH_T::constraint_t::type_value pose_t;
    typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T> parent_t;
    typedef mrpt::graphslam::deciders::CRangeScanOps<GRAPH_T> range_ops_t;
    typedef std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr> nodes_to_scans2D_t;

    // structs

    struct TMsg;
    struct TParams;

    // methods

    virtual void setClassName(const std::string& name);
    bool isMultiRobotSlamClass();
    std::string getClassName() const;
    virtual void getEdgesStats(std::map<std::string, int>* edge_type_to_num) const;
    virtual bool justInsertedLoopClosure() const;
    virtual void getDescriptiveReport(std::string* report_str) const;
    virtual bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) = 0;
    virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager* win_manager);
    virtual void setCriticalSectionPtr(std::mutex* graph_section);
    virtual void initializeVisuals();
    virtual void updateVisuals();
    virtual void notifyOfWindowEvents(const std::map<std::string, bool>& events_occurred);
    virtual void loadParams(const std::string& source_fname);
    virtual void printParams() const;
    virtual void setGraphPtr(typename mrpt::graphs::CNetworkOfPoses2DInf* graph);
    virtual void initializeLoggers(const std::string& name);

Typedefs

typedef CRangeScanEdgeRegistrationDecider<GRAPH_T> parent_t

Edge Registration Decider.

typedef CLoopCloserERD<GRAPH_T> decider_t

self type

Methods

virtual bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)

Generic method for fetching the incremental action-observations (or observation-only) measurements.

Returns:

True if operation was successful. Criteria for Success depend on the decider/optimizer implementing this method

virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager* win_manager)

Fetch a CWindowManager pointer.

CWindowManager instance should contain a CDisplayWindow3D* and, optionally, a CWindowObserver pointer so that interaction with the window is possible

virtual void notifyOfWindowEvents(const std::map<std::string, bool>& events_occurred)

Get a list of the window events that happened since the last call.

Method in derived classes is automatically called from the CGraphSlamEngine_t instance. After that, decider/optimizer should just fetch the parameters that it is interested in.

virtual void getEdgesStats(std::map<std::string, int>* edge_type_to_num) const

Fill the given map with the type of registered edges as well as the corresponding number of registration of each edge.

virtual void initializeVisuals()

Initialize visual objects in CDisplayWindow (e.g.

add an object to scene).

Parameters:

std::exception

If the method is called without having first provided a CDisplayWindow3D* to the class instance

See also:

setWindowManagerPtr, updateVisuals

virtual void updateVisuals()

Update the relevant visual features in CDisplayWindow.

Parameters:

std::exception

If the method is called without having first provided a CDisplayWindow3D* to the class instance

See also:

setWindowManagerPtr, initializeVisuals

virtual void loadParams(const std::string& source_fname)

Load the necessary for the decider/optimizer configuration parameters.

virtual void printParams() const

Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way.

virtual void getDescriptiveReport(std::string* report_str) const

Fill the provided string with a detailed report of the decider/optimizer state.

Report should include (part of) the following:

Timing of important methods
Properties fo class at the current time
Logging of commands until current time

size_t getDijkstraExecutionThresh() const

Return the minimum number of nodes that should exist in the graph prior to running Dijkstra.

void generateHypotsPool(
    const std::vector<uint32_t>& groupA,
    const std::vector<uint32_t>& groupB,
    hypotsp_t* generated_hypots,
    const TGenerateHypotsPoolAdParams* ad_params = nullptr
    )

Generate the hypothesis pool for all the inter-group constraints between two groups of nodes.

Parameters:

groupA	First group to be tested
groupB	Second group to be tested
generated_hypots	Pool of generated hypothesis. Hypotheses are generated in the heap, so the caller is responsible of afterwards calling delete.

void generatePWConsistenciesMatrix(
    const std::vector<uint32_t>& groupA,
    const std::vector<uint32_t>& groupB,
    const hypotsp_t& hypots_pool,
    mrpt::math::CMatrixDouble* consist_matrix,
    const paths_t* groupA_opt_paths = nullptr,
    const paths_t* groupB_opt_paths = nullptr
    )

Compute the pair-wise consistencies Matrix.

Parameters:

groupA	First group to be used
groupB	Second group to be used
hypots_pool	Pool of hypothesis that has been generated between the two groups pram[out] consist_matrix Pointer to Pair-wise consistencies matrix that is to be filled
groupA_opt_paths	Pointer to vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistencyElement method
groupB_opt_paths

See also:

generatePWConsistencyElement

evalPWConsistenciesMatrix

void evalPWConsistenciesMatrix(
    const mrpt::math::CMatrixDouble& consist_matrix,
    const hypotsp_t& hypots_pool,
    hypotsp_t* valid_hypots
    )

Evalute the consistencies matrix, fill the valid hypotheses.

Call to this method should be made right after generating the consistencies matrix using the generatePWConsistenciesMatrix method

See also:

generatePWConsistenciesMatrix