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/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/topography/data_types.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_gnss(const mrpt::obs::CObservationGPS& gps) = 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;

    void set_georeferencing_params(
        mrpt::topography::TGeodeticCoords geo_coord,
        mrpt::poses::CPose3DPDFGaussian T_enu_to_map)
    {
        auto& g = geoRefParams_.emplace();

        g.geo_coord    = geo_coord;
        g.T_enu_to_map = T_enu_to_map;
    }

   protected:
    struct GeoReferenceParams
    {
        mrpt::topography::TGeodeticCoords geo_coord;
        mrpt::poses::CPose3DPDFGaussian   T_enu_to_map;
    };
    std::optional<GeoReferenceParams> geoRefParams_;
};

}  // namespace mola