Program Listing for File NavStateFilter.h

↰ Return to documentation for file (include/mola_kernel/interfaces/NavStateFilter.h)

/* -------------------------------------------------------------------------
 *   A Modular Optimization framework for Localization and mApping  (MOLA)
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#include <mrpt/containers/yaml.h>
#include <mrpt/core/Clock.h>
#include <mrpt/math/TTwist3D.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/system/COutputLogger.h>

#pragma once

namespace mola
{
struct NavState
{
    NavState()  = default;
    ~NavState() = default;

    mrpt::poses::CPose3DPDFGaussianInf pose;

    mrpt::math::TTwist3D twist;

    mrpt::math::CMatrixDouble66 twist_inv_cov;

    std::string asString() const;
};

class NavStateFilter : public mrpt::system::COutputLogger
{
   public:
    NavStateFilter();
    ~NavStateFilter();

    virtual void reset() = 0;

    virtual void initialize(const mrpt::containers::yaml& cfg) = 0;

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

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

    virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu) = 0;

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

    virtual std::optional<NavState> estimated_navstate(
        const mrpt::Clock::time_point& timestamp,
        const std::string&             frame_id) = 0;
};

}  // namespace mola