Program Listing for File NavStateFG.h

↰ Return to documentation for file (include/mola_navstate_fg/NavStateFG.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

// this package:
#include <mola_kernel/interfaces/NavStateFilter.h>
#include <mola_navstate_fg/NavStateFGParams.h>

// MOLA:
#include <mola_imu_preintegration/RotationIntegrator.h>
#include <mola_kernel/factors/FactorConstVelKinematics.h>

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

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

namespace mola
{
class NavStateFG : public mola::NavStateFilter
{
   public:
    NavStateFG();
    ~NavStateFG();

    NavStateFGParams params_;

    void initialize(const mrpt::containers::yaml& cfg) 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_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>;

   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;
    };

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

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

    // 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 PointData
    {
        PointData() = default;

        PointData(const PoseData& p) : pose(p) {}
        PointData(const OdomData& p) : odom(p) {}
        PointData(const TwistData& p) : twist(p) {}
        PointData(const QueryPointData& p) : query(p) {}

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

        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, PoseData>>;
    };

    State state_;

    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 delete_too_old_entries();
};

}  // namespace mola