Program Listing for File StateEstimationSmoother.h

↰ Return to documentation for file (include/mola_state_estimation_smoother/StateEstimationSmoother.h)

/*               _
 _ __ ___   ___ | | __ _
| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
| | | | | | (_) | | (_| | Localization and mApping (MOLA)
|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola

 Copyright (C) 2018-2025 Jose Luis Blanco, University of Almeria,
                         and individual contributors.
 SPDX-License-Identifier: GPL-3.0
 See LICENSE for full license information.
 Closed-source licenses available upon request, for this odometry package
 alone or in combination with the complete SLAM system.
*/

#pragma once

// this package:
#include <mola_kernel/interfaces/LocalizationSourceBase.h>
#include <mola_kernel/interfaces/NavStateFilter.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <mola_kernel/version.h>
#include <mola_state_estimation_smoother/FactorConstVelKinematics.h>
#include <mola_state_estimation_smoother/FactorTricycleKinematics.h>
#include <mola_state_estimation_smoother/Parameters.h>

// MOLA:
#include <mola_imu_preintegration/ImuIntegrator.h>

// MRPT:
#include <mrpt/containers/bimap.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/pimpl.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationIMU.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTimeLogger.h>

// std:
#include <mutex>
#include <optional>
#include <set>

namespace mola::state_estimation_smoother
{
class StateEstimationSmoother : public mola::NavStateFilter, public mola::LocalizationSourceBase
{
    DEFINE_MRPT_OBJECT(StateEstimationSmoother, mola::state_estimation_smoother)

   public:
    StateEstimationSmoother();

    Parameters params;

    void initialize(const mrpt::containers::yaml& cfg) override;

    void spinOnce() override;

    void reset() override;

    void fuse_pose(
        const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose,
        const std::string& frame_id) override;

    void fuse_odometry(
        const mrpt::obs::CObservationOdometry& odom,
        const std::string&                     odomName = "odom_wheels") override;

    void fuse_imu(const mrpt::obs::CObservationIMU& imu) override;

    void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override;

    void fuse_twist(
        const mrpt::Clock::time_point& timestamp, const mrpt::math::TTwist3D& twist,
        const mrpt::math::CMatrixDouble66& twistCov) override;

    std::optional<NavState> estimated_navstate(
        const mrpt::Clock::time_point& timestamp, const std::string& frame_id) override;

    auto known_frame_ids() -> std::set<std::string>;

   protected:
    // Implementation of RawDataConsumer
#if MOLA_VERSION_CHECK(2, 1, 0)
    void onNewObservation(const CObservation::ConstPtr& o) override;
#else
    void onNewObservation(const CObservation::Ptr& o) override;
#endif

   private:
    // everything related to gtsam is hidden in the public API via pimpl
    struct GtsamImpl;

    using frameid_t = uint8_t;

    // an observation from fuse_pose()
    struct PoseData
    {
        PoseData() = default;

        mrpt::poses::CPose3DPDFGaussian pose;
        frameid_t                       frameId = 0;
    };

    // an observation from fuse_odometry()
    struct OdomData
    {
        OdomData() = default;

        mrpt::poses::CPose3D pose;
        frameid_t            frameId = 0;
    };

    // an observation from fuse_twist()
    struct TwistData
    {
        TwistData() = default;
        mrpt::math::TTwist3D        twist;  // in the local frame of reference
        mrpt::math::CMatrixDouble66 twistCov;
    };

    // Dummy type representing the query point.
    struct QueryPointData
    {
        QueryPointData() = default;
    };

    struct KinematicState
    {
        mrpt::poses::CPose3D pose;
        mrpt::math::TTwist3D twist;
    };

    struct PointData
    {
        PointData() = default;

        PointData(const PoseData& p, const KinematicState& ks = {}) : pose(p), last_known_state(ks)
        {
        }
        PointData(const OdomData& p, const KinematicState& ks = {}) : odom(p), last_known_state(ks)
        {
        }
        PointData(const TwistData& p, const KinematicState& ks = {})
            : twist(p), last_known_state(ks)
        {
        }
        PointData(const QueryPointData& p, const KinematicState& ks = {})
            : query(p), last_known_state(ks)
        {
        }

        std::optional<PoseData>       pose;
        std::optional<OdomData>       odom;
        std::optional<TwistData>      twist;
        std::optional<QueryPointData> query;

        // Estimation from last iteration, or initial guess,
        // to make estimation faster starting closer to the real values:
        KinematicState last_known_state;

        std::string asString() const;

        bool empty() const { return !pose && !odom && !twist && !query; }
    };

    struct State
    {
        State();
        ~State();

        mrpt::pimpl<GtsamImpl> impl;

        mrpt::containers::bimap<std::string, frameid_t> known_frames;

        frameid_t frame_id(const std::string& frame_name);

        std::map<mrpt::Clock::time_point, PointData> data;

        auto last_pose_of_frame_id(const std::string& frame_id)
            -> std::optional<std::pair<mrpt::Clock::time_point, PointData>>;

        void update_last_input_stamp(const mrpt::Clock::time_point& t)
        {
            last_observation_stamp_           = t;
            last_observation_wallclock_stamp_ = mrpt::Clock::now();
        }

        std::optional<mrpt::Clock::time_point> get_current_extrapolated_stamp() const
        {
            if (!last_observation_stamp_) return {};
            return mrpt::Clock::fromDouble(
                (mrpt::Clock::nowDouble() -
                 mrpt::Clock::toDouble(last_observation_wallclock_stamp_)) +
                mrpt::Clock::toDouble(*last_observation_stamp_));
        }

       private:
        std::optional<mrpt::Clock::time_point> last_observation_stamp_;
        mrpt::Clock::time_point                last_observation_wallclock_stamp_;
    };

    State                state_;
    std::recursive_mutex stateMutex_;

    std::optional<NavState> build_and_optimize_fg(
        const mrpt::Clock::time_point queryTimestamp, const std::string& frame_id);

    void addFactor(const mola::FactorConstVelKinematics& f);
    void addFactor(const mola::FactorTricycleKinematics& f);

    void delete_too_old_entries();

    mrpt::system::CTimeLogger profiler_{true, "StateEstimationSmoother"};
};

}  // namespace mola::state_estimation_smoother