Program Listing for File NavStateFuse.h

↰ Return to documentation for file (include/mola_navstate_fuse/NavStateFuse.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_kernel/interfaces/NavStateFilter.h>
#include <mola_navstate_fuse/NavStateFuseParams.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/optional_ref.h>
#include <mrpt/obs/CObservationIMU.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>

#include <optional>

namespace mola
{
class NavStateFuse : public mola::NavStateFilter

{
   public:
    NavStateFuse()  = default;
    ~NavStateFuse() = default;

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

    std::optional<mrpt::math::TTwist3D> get_last_twist() const
    {
        return state_.last_twist;
    }

   private:
    struct State
    {
        State()  = default;
        ~State() = default;

        std::optional<mrpt::obs::CObservationOdometry> last_odom_obs;
        std::optional<mrpt::Clock::time_point>         last_pose_obs_tim;
        std::optional<mrpt::poses::CPose3DPDFGaussian> last_pose;
        std::optional<mrpt::math::TTwist3D>            last_twist;
        bool pose_already_updated_with_odom = false;
    };

    State state_;
};

}  // namespace mola