mola-relocalization

Overview

Algorithms for localization starting with large uncertainty.

// structs

struct mola::CandidateSelectionParameters;
struct mola::RelocalizationICP_SE2;
struct mola::RelocalizationLikelihood_SE2;

// global functions

auto mola::refine_relocalization_candidates(
    const std::vector<RelocalizationICP_SE2::Candidate>& candidates,
    const mp2p_icp::metric_map_t& local_map,
    const mp2p_icp::metric_map_t& reference_map,
    mp2p_icp::ICP& icp,
    const mp2p_icp::Parameters& icp_parameters,
    const CandidateSelectionParameters& modes,
    size_t max_modes = 20
    );

auto mola::select_relocalization_candidate(const std::vector<RelocalizationICP_SE2::Candidate>& candidates, const CandidateSelectionParameters& params = {});
auto mola::find_best_poses_se2(const mrpt::poses::CPosePDFGrid& grid, const double percentile = 0.99);

Global Functions

auto mola::refine_relocalization_candidates(
    const std::vector<RelocalizationICP_SE2::Candidate>& candidates,
    const mp2p_icp::metric_map_t& local_map,
    const mp2p_icp::metric_map_t& reference_map,
    mp2p_icp::ICP& icp,
    const mp2p_icp::Parameters& icp_parameters,
    const CandidateSelectionParameters& modes,
    size_t max_modes = 20
    )

Runs ICP again from the best candidate of each mode (at most max_modes of them, best first), and returns the results, best first. The search ICP must converge from far, so its quality may not tell apart nearby poses: use here a stricter one (e.g. shorter matching distances), whose quality does.

auto mola::select_relocalization_candidate(
    const std::vector<RelocalizationICP_SE2::Candidate>& candidates,
    const CandidateSelectionParameters& params = {}
    )

Picks the best of a set of candidates (e.g. RelocalizationICP_SE2::Output::candidates), and tells whether it is unambiguous.

auto mola::find_best_poses_se2(
    const mrpt::poses::CPosePDFGrid& grid,
    const double percentile = 0.99
    )

Finds the SE(2) poses with the top given percentile likelihood, and returns them sorted by likelihood (higher values are better matches).

Parameters:

grid	To be used with the output of RelocalizationLikelihood_SE2
percentile	If set to 0.99, only those poses with a likelihood >=99% of the whole pdf will be returned.