Program Listing for File mrpt_pf_localization_core.h

↰ Return to documentation for file (include/mrpt_pf_localization/mrpt_pf_localization_core.h)

/* +------------------------------------------------------------------------+
   |                             mrpt_navigation                            |
   |                                                                        |
   | Copyright (c) 2014-2024, Individual contributors, see commit authors   |
   | See: https://github.com/mrpt-ros-pkg/mrpt_navigation                   |
   | All rights reserved. Released under BSD 3-Clause license. See LICENSE  |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mp2p_icp/ICP.h>
#include <mp2p_icp/Parameters.h>
#include <mp2p_icp/metricmap.h>
#include <mp2p_icp_filters/FilterBase.h>
#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/Clock.h>
#include <mrpt/core/pimpl.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>  // TLikelihoodOptions
#include <mrpt/maps/CPointsMap.h>  // TLikelihoodOptions
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/slam/CMonteCarloLocalization3D.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTimeLogger.h>

#include <mutex>
#include <optional>

class PFLocalizationCore : public mrpt::system::COutputLogger
{
   public:
    PFLocalizationCore();
    virtual ~PFLocalizationCore() = default;

    struct Parameters
    {
        Parameters();
        ~Parameters() = default;

        std::optional<mrpt::poses::CPose3DPDFGaussian> initial_pose;

        std::set<std::string> metric_map_use_only_these_layers;

        mrpt::maps::CMultiMetricMap::Ptr metric_map;
        std::optional<mp2p_icp::metric_map_t::Georeferencing> georeferencing;

        // used internally for relocalization only:
        std::vector<std::string> metric_map_layer_names;

        bool gui_enable = true;

        bool use_se3_pf = false;

        bool gui_camera_follow_robot = true;

        mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_2d;

        mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motion_model_no_odom_2d;

        mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motion_model_3d;

        mrpt::obs::CActionRobotMovement3D::TMotionModelOptions motion_model_no_odom_3d;

        mrpt::bayes::CParticleFilter::TParticleFilterOptions pf_options;

        mrpt::slam::TKLDParams kld_options;

        // likelihood option overrides:
        std::optional<mrpt::maps::CPointsMap::TLikelihoodOptions> override_likelihood_point_maps;
        std::optional<mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions>
            override_likelihood_gridmaps;

        unsigned int initial_particles_per_m2 = 10;

        bool initialize_from_gnss = false;

        uint32_t samples_drawn_from_gnss = 20;

        double gnss_samples_num_sigmas = 6.0;

        double relocalize_num_sigmas = 3.0;

        unsigned int relocalization_initial_divisions_xy = 4;
        unsigned int relocalization_initial_divisions_phi = 4;

        unsigned int relocalization_min_sample_copies_per_candidate = 4;

        double relocalization_resolution_xy = 0.50;  // [m]
        double relocalization_resolution_phi = 0.20;  // [rad]
        double relocalization_minimum_icp_quality = 0.50;
        double relocalization_icp_sigma = 5.0;  // [m]

        mp2p_icp::ICP::Ptr relocalization_icp;
        mp2p_icp::Parameters relocalization_icp_params;

        mp2p_icp_filters::FilterPipeline relocalization_obs_filter;
        mp2p_icp::ParameterSource paramSource;

        void load_from(const mrpt::containers::yaml& params);
    };

    enum class State : uint8_t
    {
        UNINITIALIZED,
        TO_BE_INITIALIZED,
        RUNNING,
    };

    void init_from_yaml(
        const mrpt::containers::yaml& pf_params,
        const mrpt::containers::yaml& relocalization_pipeline);

    void on_observation(const mrpt::obs::CObservation::Ptr& obs);

    void step();

    void reset();

    bool set_map_from_simple_map(
        const std::string& map_config_ini_file, const std::string& simplemap_file);

    void set_map_from_metric_map(
        const mrpt::maps::CMultiMetricMap::Ptr& metricMap,
        const std::optional<mp2p_icp::metric_map_t::Georeferencing>& georeferencing = std::nullopt,
        const std::vector<std::string>& layerNames = {});

    void set_map_from_metric_map(const mp2p_icp::metric_map_t& mm);

    void relocalize_here(const mrpt::poses::CPose3DPDFGaussian& pose);

    bool input_queue_has_odometry();
    std::optional<mrpt::Clock::time_point> input_queue_last_stamp();

    void set_fake_odometry_increment(const mrpt::poses::CPose3D& incrPose);

    // TODO: Getters
    State getState() const { return state_.fsm_state; }

    const Parameters getParams() { return params_; }

    mrpt::poses::CPose3DPDFParticles::Ptr getLastPoseEstimation() const;

   protected:
    Parameters params_;

    struct InternalState
    {
        InternalState();

        State fsm_state = State::UNINITIALIZED;

        mrpt::maps::CMultiMetricMap::Ptr metric_map;
        std::optional<mp2p_icp::metric_map_t::Georeferencing> georeferencing;

        mrpt::bayes::CParticleFilter pf;

        mrpt::bayes::CParticleFilter::TParticleFilterStats pf_stats;

        std::optional<mrpt::slam::CMonteCarloLocalization2D> pdf2d;
        std::optional<mrpt::slam::CMonteCarloLocalization3D> pdf3d;

        mrpt::Clock::time_point time_last_update;

        mrpt::obs::CObservationOdometry::Ptr last_odom;

        std::vector<mrpt::obs::CObservation::Ptr> pendingObs;

        mrpt::poses::CPose3DPDFParticles::Ptr lastResult;

        std::optional<mrpt::poses::CPose3D> nextFakeOdometryIncrPose;

        struct Relocalization;
        mrpt::pimpl<Relocalization> pendingRelocalization;
    };

    mrpt::obs::CObservationGPS::Ptr get_last_gnss_obs() const
    {
        auto lck = mrpt::lockHelper(pendingObsMtx_);
        return last_gnss_;
    }

   private:
    InternalState state_;
    std::mutex stateMtx_;

    std::mutex pendingObsMtx_;
    mrpt::obs::CObservationGPS::Ptr last_gnss_;  // use mtx: pendingObsMtx_

    mrpt::system::CTimeLogger profiler_{true /*enabled*/, "mrpt_pf_localization" /*name*/};

    mrpt::gui::CDisplayWindow3D::Ptr win3D_;

    void onStateUninitialized();

    void onStateToBeInitialized();

    void onStateRunning();

    void init_gui();
    void update_gui(const mrpt::obs::CSensoryFrame& sf);

    void internal_fill_state_lastResult();

    std::optional<mrpt::poses::CPose3DPDFGaussian> get_gnss_pose_prediction();
};