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-2025 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