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