Program Listing for File relocalization.h

↰ Return to documentation for file (include/mola_relocalization/relocalization.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 *
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * Licensed under the GNU GPL v3 for non-commercial applications.
 *
 * This file is part of MOLA.
 * MOLA is free software: you can redistribute it and/or modify it under the
 * terms of the GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later
 * version.
 *
 * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
 * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * MOLA. If not, see <https://www.gnu.org/licenses/>.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mola_pose_list/HashedSetSE3.h>
#include <mp2p_icp/ICP.h>
#include <mp2p_icp/Parameters.h>
#include <mp2p_icp/metricmap.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/poses/CPosePDFGrid.h>

#include <map>

namespace mola
{
struct RelocalizationLikelihood_SE2
{
    struct Input
    {
        mp2p_icp::metric_map_t   reference_map;
        mrpt::obs::CSensoryFrame observations;
        mrpt::math::TPose2D      corner_min, corner_max;
        double                   resolution_xy  = 0.5;
        double                   resolution_phi = mrpt::DEG2RAD(30.0);

        Input() = default;
    };

    struct Output
    {
        mrpt::poses::CPosePDFGrid likelihood_grid;
        double                    time_cost          = .0;
        double                    max_log_likelihood = 0;
        double                    min_log_likelihood = 0;

        Output() = default;
    };

    static Output run(const Input& in);
};

struct RelocalizationICP_SE2
{
    struct ProgressFeedback
    {
        ProgressFeedback() = default;

        size_t              current_cell = 0;
        size_t              total_cells  = 0;
        mrpt::math::TPose3D cell_init_guess;
        double              obtained_icp_quality = .0;
    };

    struct Input
    {
        mp2p_icp::metric_map_t reference_map;
        mp2p_icp::metric_map_t local_map;

        std::vector<mp2p_icp::ICP::Ptr> icp_pipeline;
        mp2p_icp::Parameters            icp_parameters;
        double                          icp_minimum_quality = 0.50;

        struct InputLattice
        {
            mrpt::math::TPose2D corner_min, corner_max;
            double              resolution_xy  = 1.0;
            double              resolution_phi = mrpt::DEG2RAD(40.0);
        };
        InputLattice initial_guess_lattice;

        struct OutputLattice
        {
            double resolution_xyz   = 0.10;
            double resolution_yaw   = mrpt::DEG2RAD(5.0);
            double resolution_pitch = mrpt::DEG2RAD(5.0);
            double resolution_roll  = mrpt::DEG2RAD(5.0);
        };
        OutputLattice output_lattice;

        std::function<void(const ProgressFeedback&)> on_progress_callback;

        Input() = default;
    };

    struct Output
    {
        mola::HashedSetSE3 found_poses;
        double             time_cost = .0;

        Output() = default;
    };

    static Output run(const Input& in);
};

auto find_best_poses_se2(
    const mrpt::poses::CPosePDFGrid& grid, const double percentile = 0.99)
    -> std::map<double, mrpt::math::TPose2D>;

}  // namespace mola