Program Listing for File StateEstimationSmoother.h
↰ Return to documentation for file (include/mola_state_estimation_smoother/StateEstimationSmoother.h)
/* -------------------------------------------------------------------------
* A Modular Optimization framework for Localization and mApping (MOLA)
*
* Copyright (C) 2018-2025 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/LocalizationSourceBase.h>
#include <mola_kernel/interfaces/NavStateFilter.h>
#include <mola_kernel/interfaces/RawDataSourceBase.h>
#include <mola_state_estimation_smoother/FactorTricycleKinematics.h>
#include <mola_state_estimation_smoother/Parameters.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/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,
public mola::RawDataConsumer
{
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
void onNewObservation(const CObservation::Ptr& o) override;
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 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