diff --git a/mola_navstate_fuse/CMakeLists.txt b/mola_navstate_fuse/CMakeLists.txt index 0fe3b10a..fdc96792 100644 --- a/mola_navstate_fuse/CMakeLists.txt +++ b/mola_navstate_fuse/CMakeLists.txt @@ -18,8 +18,10 @@ find_package(mola_common REQUIRED) # find CMake dependencies: find_package(mrpt-obs) +find_package(GTSAM REQUIRED) # Find MOLA packages: +find_package(mola_kernel REQUIRED) find_package(mola_imu_preintegration REQUIRED) # ----------------------- @@ -32,13 +34,16 @@ mola_add_library( SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS} PUBLIC_LINK_LIBRARIES mrpt::obs + mola::mola_kernel mola::mola_imu_preintegration - #PRIVATE_LINK_LIBRARIES - # mrpt::obs + PRIVATE_LINK_LIBRARIES + gtsam CMAKE_DEPENDENCIES mola_common mola_imu_preintegration + mola_kernel ) +target_include_directories(${PROJECT_NAME} PRIVATE ".") # ----------------------- # define tests: diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavState.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavState.h index 99b13f24..58bd902d 100644 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavState.h +++ b/mola_navstate_fuse/include/mola_navstate_fuse/NavState.h @@ -30,6 +30,10 @@ namespace mola { +/** The state returned by NavStateFuse + * + * \ingroup mola_navstate_fuse_grp + */ struct NavState { NavState() = default; diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h index f15a9c30..78c75718 100644 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h +++ b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h @@ -25,20 +25,50 @@ */ #pragma once -#include +// this package: #include #include + +// MOLA: +#include +#include + +// MRPT: +#include #include -#include +#include #include #include #include +#include +// std: +#include #include namespace mola { -/** Fuse of odometry, IMU, and SE(3) pose/twist estimations. +/** Sliding window Factor-graph data fusion for odometry, IMU, GNNS, and SE(3) + * pose/twist estimations. + * + * Frame conventions: + * - There is a frame of reference for each source of odometry, e.g. + * there may be one for LiDAR-odometry, another for visual-odometry, or + * wheels-based odometry, etc. Each such frame is referenced with a "frame + * name" (an arbitrary string). + * - Internally, the first frame of reference will be used as "global" + * coordinates, despite it may be actually either a `map` or `odom` frame, in + * the [ROS REP 105](https://www.ros.org/reps/rep-0105.html) sense. + * - IMU readings are, by definition, given in the robot body frame, although + * they can have a relative transformation between the vehicle and sensor. + * + * Main API methods and frame conventions: + * - `estimated_navstate()`: Output estimations can be requested in any of the + * existing frames of reference. + * - `fuse_pose()`: Can be used to integrate information from any "odometry" or + * "localization" input, as mentioned above. + * - `fuse_gnns()`: TO-DO. + * - `fuse_imu()`: TO-DO. * * Usage: * - (1) Call initialize() or set the required parameters directly in params_. @@ -51,13 +81,18 @@ namespace mola * * Old observations are automatically removed. * - * \sa IMUIntegrator - * \ingroup mola_imu_preintegration_grp + * A constant SE(3) velocity model is internally used, without any + * particular assumptions on the vehicle kinematics. + * + * For more theoretical descriptions, see the papers cited in + * https://docs.mola-slam.org/latest/ + * + * \ingroup mola_navstate_fuse_grp */ -class NavStateFuse +class NavStateFuse : public mrpt::system::COutputLogger { public: - NavStateFuse() = default; + NavStateFuse(); ~NavStateFuse() = default; /** \name Main API @@ -76,17 +111,25 @@ class NavStateFuse */ void reset(); - /** Integrates new odometry observations into the estimator */ - void fuse_odometry(const mrpt::obs::CObservationOdometry& odom); + /** Integrates new SE(3) pose estimation of the vehicle wrt frame_id + */ + void fuse_pose( + const mrpt::Clock::time_point& timestamp, + const mrpt::poses::CPose3DPDFGaussian& pose, + const std::string& frame_id); + + /** Integrates new wheels-based odometry observations into the estimator. + * This is a convenience method that internally ends up calling + * fuse_pose(), but computing the uncertainty of odometry increments + * according to a given motion model. + */ + void fuse_odometry( + const mrpt::obs::CObservationOdometry& odom, + const std::string& odomName = "odom_wheels"); /** Integrates new IMU observations into the estimator */ void fuse_imu(const mrpt::obs::CObservationIMU& imu); - /** Integrates new SE(3) pose estimation into the estimator */ - void fuse_pose( - const mrpt::Clock::time_point& timestamp, - const mrpt::poses::CPose3DPDFGaussian& pose); - /** Integrates new twist estimation (in the odom frame) */ void fuse_twist( const mrpt::Clock::time_point& timestamp, @@ -98,31 +141,83 @@ class NavStateFuse * validity time window (e.g. too far in the future to be trustful). */ std::optional estimated_navstate( - const mrpt::Clock::time_point& timestamp) const; + const mrpt::Clock::time_point& timestamp); +#if 0 std::optional get_last_twist() const { return state_.last_twist; } void force_last_twist(const mrpt::math::TTwist3D& twist); +#endif /** @} */ 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; + }; + + // Dummy type representing the query point. + struct QueryPointData + { + QueryPointData() = default; + }; + + struct PointData + { + PointData(const PoseData& p) : pose(p) {} + PointData(const QueryPointData& p) : query(p) {} + + std::optional pose; + std::optional query; + + private: + PointData() = default; + }; + struct State { - State() = default; - ~State() = default; + State(); + ~State(); + + mrpt::pimpl impl; + + /// A bimap of known "frame_id" <=> "numeric IDs": + mrpt::containers::bimap known_frames; + + /// Returns the existing ID, or creates a new ID, for a frame: + frameid_t frame_id(const std::string& frame_name); + + /// The sliding window of observation data: + std::multimap data; +#if 0 std::optional last_pose_obs_tim; + std::optional last_odom_obs; std::optional last_pose; std::optional last_twist; - std::optional last_odom_obs; +#endif }; - // const State& current_state() const { return state_; } - State state_; + State state_; + std::recursive_mutex state_mtx_; /// to access state_ + + void build_and_optimize_fg(const mrpt::Clock::time_point queryTimestamp); + + /// Implementation of Eqs (1),(4) in the MOLA RSS2019 paper. + void addFactor(const mola::FactorConstVelKinematics& f); }; } // namespace mola diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h index f5072483..9d27cf89 100644 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h +++ b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h @@ -33,7 +33,7 @@ namespace mola { /** Parameters needed by NavStateFuse. * - * \ingroup mola_imu_preintegration_grp + * \ingroup mola_navstate_fuse__grp */ class NavStateFuseParams { @@ -45,11 +45,25 @@ class NavStateFuseParams void loadFrom(const mrpt::containers::yaml& cfg); /** Valid estimations will be extrapolated only up to this time since the - * last incorporated observation. */ + * last incorporated observation. If a request is done farther away, an + * empty estimation will be returned. + */ double max_time_to_use_velocity_model = 2.0; // [s] + /// Time to keep past observations in the filter + double sliding_window_length = 5.0; // [s] + + /// If the time between two keyframes is larger than this, a warning will be + /// emitted; but the algorithm will keep trying its best. + double time_between_frames_to_warning = 3.0; // [s] + double sigma_random_walk_acceleration_linear = 1.0; // [m/s²] double sigma_random_walk_acceleration_angular = 1.0; // [rad/s²] + double sigma_integrator_position = 0.10; // [m] + double sigma_integrator_orientation = 0.10; // [rad] + + /** Const. velocity model: sigma of lin and angular velocity */ + double const_vel_model_std_linvel{1.0}, const_vel_model_std_angvel{1.0}; }; } // namespace mola diff --git a/mola_navstate_fuse/package.xml b/mola_navstate_fuse/package.xml index f6f9db0d..13261fe6 100644 --- a/mola_navstate_fuse/package.xml +++ b/mola_navstate_fuse/package.xml @@ -16,8 +16,10 @@ GPLv3 mola_common + mola_kernel mola_imu_preintegration mrpt2 + gtsam doxygen diff --git a/mola_navstate_fuse/src/FactorAngularVelocityIntegration.h b/mola_navstate_fuse/src/FactorAngularVelocityIntegration.h new file mode 100644 index 00000000..cc58d6dc --- /dev/null +++ b/mola_navstate_fuse/src/FactorAngularVelocityIntegration.h @@ -0,0 +1,136 @@ +/* ------------------------------------------------------------------------- + * 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 . + * ------------------------------------------------------------------------- */ +/** + * @file FactorAngularVelocityIntegration.h + * @brief GTSAM factor + * @author Jose Luis Blanco Claraco + * @date Jun 13, 2024 + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace mola +{ +/** + * Factor for angular velocity integration model, equivalent to expressions: + * + * gtsam::Expression deltaWi = dt * bWi; + * gtsam::Expression expmap_(>sam::Rot3::Expmap, deltaWi); + * + * gtsam::between( gtsam::compose(Ri, expmap_), Rj ) = Rot_Identity + * + */ +class FactorAngularVelocityIntegration + : public gtsam::ExpressionFactorN< + gtsam::Rot3 /*return type*/, gtsam::Rot3 /* Ri */, + gtsam::Point3 /* bWi */, gtsam::Rot3 /* Rj */ + > +{ + private: + using This = FactorAngularVelocityIntegration; + using Base = gtsam::ExpressionFactorN< + gtsam::Rot3 /*return type*/, gtsam::Rot3 /* Ri */, + gtsam::Point3 /* bWi */, gtsam::Rot3 /* Rj */ + >; + + double dt_ = .0; + + public: + /// default constructor + FactorAngularVelocityIntegration() = default; + ~FactorAngularVelocityIntegration() override = default; + + FactorAngularVelocityIntegration( + gtsam::Key kRi, gtsam::Key kbWi, gtsam::Key kRj, const double dt, + const gtsam::SharedNoiseModel& model) + : Base({kRi, kbWi, kRj}, model, gtsam::Rot3()), dt_(dt) + { + this->initialize(Base::expression({kRi, kbWi, kRj})); + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override + { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + gtsam::Expression expression( + const std::array& keys) const override + { + gtsam::Rot3_ Ri(keys[0]); + gtsam::Point3_ bWi(keys[1]); + gtsam::Rot3_ Rj(keys[2]); + gtsam::Expression deltaWi = dt_ * bWi; + gtsam::Expression expmap_(>sam::Rot3::Expmap, deltaWi); + + return gtsam::between(gtsam::compose(Ri, expmap_), Rj); + } + + /** implement functions needed for Testable */ + + /** print */ + void print( + const std::string& s, const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const override + { + std::cout << s << "FactorAngularVelocityIntegration(" + << keyFormatter(Factor::keys_[0]) << "," + << keyFormatter(Factor::keys_[1]) << "," + << keyFormatter(Factor::keys_[2]) << ")\n"; + gtsam::traits::Print(measured_, " measured: "); + gtsam::traits::Print(dt_, " dt: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) + const override + { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + gtsam::traits::Equals(dt_, e->dt_, tol); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) + { + // **IMPORTANT** We need to deserialize parameters before the base + // class, since it calls expression() and we need all parameters ready + // at that point. + ar& BOOST_SERIALIZATION_NVP(measured_); + ar& BOOST_SERIALIZATION_NVP(dt_); + ar& boost::serialization::make_nvp( + "FactorAngularVelocityIntegration", + boost::serialization::base_object(*this)); + } +}; + +} // namespace mola diff --git a/mola_navstate_fuse/src/FactorConstAngularVelocity.h b/mola_navstate_fuse/src/FactorConstAngularVelocity.h new file mode 100644 index 00000000..694d3a76 --- /dev/null +++ b/mola_navstate_fuse/src/FactorConstAngularVelocity.h @@ -0,0 +1,129 @@ +/* ------------------------------------------------------------------------- + * 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 . + * ------------------------------------------------------------------------- */ +/** + * @file FactorAngularVelocityIntegration.h + * @brief GTSAM factor + * @author Jose Luis Blanco Claraco + * @date Jun 13, 2024 + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace mola +{ +/** + * Factor for constant angular velocity model, equivalent to expression: + * + * gtsam::rotate(Ri, bWi) - gtsam::rotate(Rj, bWj) = errZero + * + */ +class FactorConstAngularVelocity + : public gtsam::ExpressionFactorN< + gtsam::Point3 /*return type*/, gtsam::Rot3, gtsam::Point3, + gtsam::Rot3, gtsam::Point3> +{ + private: + using This = FactorConstAngularVelocity; + using Base = gtsam::ExpressionFactorN< + gtsam::Point3, gtsam::Rot3, gtsam::Point3, gtsam::Rot3, gtsam::Point3>; + + public: + /// default constructor + FactorConstAngularVelocity() = default; + ~FactorConstAngularVelocity() override = default; + + FactorConstAngularVelocity( + gtsam::Key kRi, gtsam::Key kWi, gtsam::Key kRj, gtsam::Key kWj, + const gtsam::SharedNoiseModel& model) + : Base({kRi, kWi, kRj, kWj}, model, {0, 0, 0}) + { + this->initialize(Base::expression({kRi, kWi, kRj, kWj})); + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override + { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + gtsam::Expression expression( + const std::array& keys) const override + { + gtsam::Expression Ri_(keys[0]); + gtsam::Expression bWi_(keys[1]); + gtsam::Expression Rj_(keys[2]); + gtsam::Expression bWj_(keys[3]); + return {gtsam::rotate(Ri_, bWi_) - gtsam::rotate(Rj_, bWj_)}; + } + + /** implement functions needed for Testable */ + + /** print */ + void print( + const std::string& s, const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const override + { + std::cout << s << "FactorConstAngularVelocity(" + << keyFormatter(Factor::keys_[0]) << "," + << keyFormatter(Factor::keys_[1]) << "," + << keyFormatter(Factor::keys_[2]) << "," + << keyFormatter(Factor::keys_[3]) << ")\n"; + gtsam::traits::Print(measured_, " measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) + const override + { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol); + } + + /** implement functions needed to derive from Factor */ + + /** number of variables attached to this factor */ + // std::size_t size() const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) + { + // **IMPORTANT** We need to deserialize parameters before the base + // class, since it calls expression() and we need all parameters ready + // at that point. + ar& BOOST_SERIALIZATION_NVP(measured_); + ar& boost::serialization::make_nvp( + "FactorConstAngularVelocity", + boost::serialization::base_object(*this)); + } +}; + +} // namespace mola diff --git a/mola_navstate_fuse/src/FactorTrapezoidalIntegrator.h b/mola_navstate_fuse/src/FactorTrapezoidalIntegrator.h new file mode 100644 index 00000000..43811720 --- /dev/null +++ b/mola_navstate_fuse/src/FactorTrapezoidalIntegrator.h @@ -0,0 +1,131 @@ +/* ------------------------------------------------------------------------- + * 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 . + * ------------------------------------------------------------------------- */ +/** + * @file FactorAngularVelocityIntegration.h + * @brief GTSAM factor + * @author Jose Luis Blanco Claraco + * @date Jun 13, 2024 + */ + +#pragma once + +#include +#include +#include +#include + +namespace mola +{ +/** + * Factor for constant angular velocity model, equivalent to expression: + * + * Pi + 0.5 * dt * (Vi + Vj) - Pj = errZero + * + */ +class FactorTrapezoidalIntegrator + : public gtsam::ExpressionFactorN< + gtsam::Point3 /*return type*/, gtsam::Point3, gtsam::Point3, + gtsam::Point3, gtsam::Point3> +{ + private: + using This = FactorTrapezoidalIntegrator; + using Base = gtsam::ExpressionFactorN< + gtsam::Point3 /*return type*/, gtsam::Point3, gtsam::Point3, + gtsam::Point3, gtsam::Point3>; + + double dt_ = .0; + + public: + /// default constructor + FactorTrapezoidalIntegrator() = default; + ~FactorTrapezoidalIntegrator() override = default; + + FactorTrapezoidalIntegrator( + gtsam::Key kPi, gtsam::Key kVi, gtsam::Key kPj, gtsam::Key kVj, + const double dt, const gtsam::SharedNoiseModel& model) + : Base({kPi, kVi, kPj, kVj}, model, /* error=0 */ {0, 0, 0}), dt_(dt) + { + this->initialize(Base::expression({kPi, kVi, kPj, kVj})); + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override + { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + gtsam::Expression expression( + const std::array& keys) const override + { + gtsam::Expression Pi_(keys[0]); + gtsam::Expression Vi_(keys[1]); + + gtsam::Expression Pj_(keys[2]); + gtsam::Expression Vj_(keys[3]); + + return {Pi_ + 0.5 * dt_ * (Vi_ + Vj_) - Pj_}; + } + + /** implement functions needed for Testable */ + + /** print */ + void print( + const std::string& s, const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const override + { + std::cout << s << "FactorTrapezoidalIntegrator(" + << keyFormatter(Factor::keys_[0]) << "," + << keyFormatter(Factor::keys_[1]) << "," + << keyFormatter(Factor::keys_[2]) << "," + << keyFormatter(Factor::keys_[3]) << ")\n"; + gtsam::traits::Print(dt_, " dt: "); + gtsam::traits::Print(measured_, " measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) + const override + { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + gtsam::traits::Equals(e->dt_, dt_, tol); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) + { + // **IMPORTANT** We need to deserialize parameters before the base + // class, since it calls expression() and we need all parameters ready + // at that point. + ar& BOOST_SERIALIZATION_NVP(measured_); + ar& BOOST_SERIALIZATION_NVP(dt_); + ar& boost::serialization::make_nvp( + "FactorTrapezoidalIntegrator", + boost::serialization::base_object(*this)); + } +}; + +} // namespace mola diff --git a/mola_navstate_fuse/src/NavStateFuse.cpp b/mola_navstate_fuse/src/NavStateFuse.cpp index a6846b9a..696fcac1 100644 --- a/mola_navstate_fuse/src/NavStateFuse.cpp +++ b/mola_navstate_fuse/src/NavStateFuse.cpp @@ -24,11 +24,76 @@ * @date Jan 22, 2024 */ +// MOLA & MRPT: #include +#include #include +#include + +// GTSAM: +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Custom factors: +#include "FactorAngularVelocityIntegration.h" +#include "FactorConstAngularVelocity.h" +#include "FactorTrapezoidalIntegrator.h" using namespace mola; +using gtsam::symbol_shorthand::F; // Frame of reference origin pose (Pose3) +using gtsam::symbol_shorthand::P; // Position (Point3) +using gtsam::symbol_shorthand::R; // Rotation (Rot3) +using gtsam::symbol_shorthand::V; // Lin velocity (Point3) +using gtsam::symbol_shorthand::W; // Ang velocity (Point3) + +// -------- GtsamImpl ------- + +struct NavStateFuse::GtsamImpl +{ + GtsamImpl() = default; + ~GtsamImpl() = default; + + gtsam::NonlinearFactorGraph fg; + gtsam::Values values; +}; + +// -------- NavStateFuse::State ------- +NavStateFuse::State::State() : impl(mrpt::make_impl()) +{ +} + +NavStateFuse::frameid_t NavStateFuse::State::frame_id( + const std::string& frame_name) +{ + if (auto it = known_frames.find_key(frame_name); + it != known_frames.getDirectMap().end()) + { + return it->second; + } + else + { + const auto newId = static_cast(known_frames.size()); + known_frames.insert(frame_name, newId); + return newId; + } +} + +// -------- NavStateFuse ------- +NavStateFuse::NavStateFuse() +{ + this->mrpt::system::COutputLogger::setLoggerName("NavStateFuse"); +} + void NavStateFuse::initialize(const mrpt::containers::yaml& cfg) { reset(); @@ -43,10 +108,11 @@ void NavStateFuse::reset() state_ = State(); } -void NavStateFuse::fuse_odometry(const mrpt::obs::CObservationOdometry& odom) +void NavStateFuse::fuse_odometry( + const mrpt::obs::CObservationOdometry& odom, const std::string& odomName) { - // TODO(jlbc): proper time-based data fusion. - + THROW_EXCEPTION("TODO"); +#if 0 // temporarily, this will work well only for simple datasets: if (state_.last_odom_obs && state_.last_pose) { @@ -60,114 +126,193 @@ void NavStateFuse::fuse_odometry(const mrpt::obs::CObservationOdometry& odom) } // copy: state_.last_odom_obs = odom; +#endif } void NavStateFuse::fuse_imu(const mrpt::obs::CObservationIMU& imu) { - // TODO(jlbc) + THROW_EXCEPTION("TODO"); (void)imu; } void NavStateFuse::fuse_pose( const mrpt::Clock::time_point& timestamp, - const mrpt::poses::CPose3DPDFGaussian& pose) + const mrpt::poses::CPose3DPDFGaussian& pose, const std::string& frame_id) { - mrpt::poses::CPose3D incrPose; + auto lck = mrpt::lockHelper(state_mtx_); // numerical sanity: for (int i = 0; i < 6; i++) ASSERT_GT_(pose.cov(i, i), .0); - double dt = 0; - if (state_.last_pose_obs_tim) - dt = mrpt::system::timeDifference(*state_.last_pose_obs_tim, timestamp); - - if (dt < params_.max_time_to_use_velocity_model && state_.last_pose) - { - ASSERT_GT_(dt, .0); - - auto& tw = state_.last_twist.emplace(); - - incrPose = pose.mean - (state_.last_pose)->mean; - - tw.vx = incrPose.x() / dt; - tw.vy = incrPose.y() / dt; - tw.vz = incrPose.z() / dt; + PoseData d; + d.frameId = state_.frame_id(frame_id); + d.pose = pose; - const auto logRot = - mrpt::poses::Lie::SO<3>::log(incrPose.getRotationMatrix()); - - tw.wx = logRot[0] / dt; - tw.wy = logRot[1] / dt; - tw.wz = logRot[2] / dt; - } - else - { - state_.last_twist.reset(); - } - - // save for next iter: - state_.last_pose = pose; - state_.last_pose_obs_tim = timestamp; + state_.data.insert(std::pair(timestamp, d)); } void NavStateFuse::fuse_twist( const mrpt::Clock::time_point& timestamp, const mrpt::math::TTwist3D& twist) { (void)timestamp; - - state_.last_twist = twist; + THROW_EXCEPTION("TODO"); } +#if 0 void NavStateFuse::force_last_twist(const mrpt::math::TTwist3D& twist) { state_.last_twist = twist; } +#endif std::optional NavStateFuse::estimated_navstate( - const mrpt::Clock::time_point& timestamp) const + const mrpt::Clock::time_point& timestamp) { - if (!state_.last_pose_obs_tim) return {}; // None + build_and_optimize_fg(timestamp); - const double dt = - mrpt::system::timeDifference(*state_.last_pose_obs_tim, timestamp); + NavState ret; + return ret; +} - if (!state_.last_twist || !state_.last_pose || - std::abs(dt) > params_.max_time_to_use_velocity_model) - return {}; // None +void NavStateFuse::build_and_optimize_fg( + const mrpt::Clock::time_point queryTimestamp) +{ + auto lck = mrpt::lockHelper(state_mtx_); - NavState ret; + if (state_.data.empty() || state_.known_frames.empty()) return; - const auto& tw = state_.last_twist.value(); + state_.impl->values.clear(); + state_.impl->fg = {}; - // For the velocity model, we don't have any known "bias": - const mola::RotationIntegrationParams rotParams = {}; + // Build the sequence of time points: + // FG variable indices will use the indices in this vector: + auto itQuery = state_.data.insert({queryTimestamp, QueryPointData()}); - const auto rot33 = - mola::incremental_rotation({tw.wx, tw.wy, tw.wz}, rotParams, dt); + using map_it_t = + std::multimap::value_type; - // pose mean: - ret.pose.mean = - (state_.last_pose->mean + - mrpt::poses::CPose3D::FromRotationAndTranslation( - rot33, mrpt::math::TVector3D(tw.vx, tw.vy, tw.vz) * dt)); + std::vector entries; + for (const auto& it : state_.data) entries.push_back(&it); - // pose cov: - auto cov = state_.last_pose->cov; + // add const vel kinematic factors between consecutive KFs: + if (entries.size() >= 2) + { + for (size_t i = 1; i < entries.size(); i++) + { + mola::FactorConstVelKinematics f; + f.from_kf_ = i - 1; + f.to_kf_ = i; + f.deltaTime_ = mrpt::system::timeDifference( + entries[i - 1]->first, entries[i]->first); + + addFactor(f); + } + } - double varXYZ = - mrpt::square(dt * params_.sigma_random_walk_acceleration_linear); - double varRot = - mrpt::square(dt * params_.sigma_random_walk_acceleration_angular); + // Init values: + for (size_t i = 0; i < entries.size(); i++) + { + state_.impl->values.insert(P(i), gtsam::Z_3x1); + state_.impl->values.insert(R(i), gtsam::Rot3::Identity()); + state_.impl->values.insert(V(i), gtsam::Z_3x1); + state_.impl->values.insert(W(i), gtsam::Z_3x1); + } + for (const auto& [frameName, frameId] : state_.known_frames.getDirectMap()) + { + state_.impl->values.insert( + F(frameId), gtsam::Pose3::Identity()); + } + // Unary prior for first frameId only: + state_.impl->fg.addPrior(F(0 /*first one*/), gtsam::Pose3::Identity()); - for (int i = 0; i < 3; i++) cov(i, i) += varXYZ; - for (int i = 3; i < 6; i++) cov(i, i) += varRot; +#if 1 + state_.impl->fg.print(); + state_.impl->values.print(); +#endif - ret.pose.cov_inv = cov.inverse_LLt(); + gtsam::LevenbergMarquardtOptimizer lm(state_.impl->fg, state_.impl->values); - // twist: - ret.twist = state_.last_twist.value(); + const auto& optimal = lm.optimize(); - // TODO(jlbc): twist covariance +#if 1 + optimal.print("Optimized:"); +#endif - return ret; + // delete temporary entry: + state_.data.erase(itQuery); +} + +/// Implementation of Eqs (1),(4) in the MOLA RSS2019 paper. +void NavStateFuse::addFactor(const mola::FactorConstVelKinematics& f) +{ + MRPT_LOG_DEBUG_STREAM( + "[addFactor] FactorConstVelKinematics: " + << f.from_kf_ << " ==> " << f.to_kf_ << " dt=" << f.deltaTime_); + + // Add const-vel factor to gtsam itself: + const double dt = f.deltaTime_; + ASSERT_GE_(dt, 0.); + + // errors in constant vel: + const double std_linvel = params_.sigma_random_walk_acceleration_linear; + const double std_angvel = params_.sigma_random_walk_acceleration_angular; + + auto noise_linVelModel = + gtsam::noiseModel::Isotropic::Sigma(3, std_linvel * dt); + auto noise_angVelModel = + gtsam::noiseModel::Isotropic::Sigma(3, std_angvel * dt); + + if (dt > params_.time_between_frames_to_warning) + { + MRPT_LOG_WARN_FMT( + "A constant-velocity kinematics factor has been added for a " + "dT=%.03f s.", + dt); + } + + // 1) Add GTSAM factors for constant velocity model + // ------------------------------------------------- + + auto Pi = gtsam::Point3_(P(f.from_kf_)); + auto Pj = gtsam::Point3_(P(f.to_kf_)); + auto Ri = gtsam::Rot3_(R(f.from_kf_)); + auto Rj = gtsam::Rot3_(R(f.to_kf_)); + auto Vi = gtsam::Point3_(V(f.from_kf_)); + auto Vj = gtsam::Point3_(V(f.to_kf_)); + auto bWi = gtsam::Point3_(W(f.from_kf_)); + auto bWj = gtsam::Point3_(W(f.to_kf_)); + + const auto kPi = P(f.from_kf_); + const auto kPj = P(f.to_kf_); + const auto kVi = V(f.from_kf_); + const auto kVj = V(f.to_kf_); + const auto kRi = R(f.from_kf_); + const auto kRj = R(f.to_kf_); + const auto kbWi = W(f.from_kf_); + const auto kbWj = W(f.to_kf_); + + // See line 3 of eq (4) in the MOLA RSS2019 paper + state_.impl->fg.emplace_shared>( + kVi, kVj, gtsam::Z_3x1, noise_linVelModel); + + // \omega is in the body frame, we need a special factor to rotate it: + // See line 4 of eq (4) in the MOLA RSS2019 paper. + state_.impl->fg.emplace_shared( + kRi, kbWi, kRj, kbWj, noise_angVelModel); + + // 2) Add kinematics / numerical integration factor + // --------------------------------------------------- + auto noise_kinematicsPosition = gtsam::noiseModel::Isotropic::Sigma( + 3, params_.sigma_integrator_position); + + auto noise_kinematicsOrientation = gtsam::noiseModel::Isotropic::Sigma( + 3, params_.sigma_integrator_orientation); + + // Impl. line 2 of eq (1) in the MOLA RSS2019 paper + state_.impl->fg.emplace_shared( + kPi, kVi, kPj, kVj, dt, noise_kinematicsPosition); + + // Impl. line 1 of eq (4) in the MOLA RSS2019 paper. + state_.impl->fg.emplace_shared( + kRi, kbWi, kRj, dt, noise_kinematicsOrientation); } diff --git a/mola_navstate_fuse/src/NavStateFuseParams.cpp b/mola_navstate_fuse/src/NavStateFuseParams.cpp index bd8f2ef7..725c338c 100644 --- a/mola_navstate_fuse/src/NavStateFuseParams.cpp +++ b/mola_navstate_fuse/src/NavStateFuseParams.cpp @@ -18,10 +18,10 @@ * MOLA. If not, see . * ------------------------------------------------------------------------- */ /** - * @file IMUIntegrationParams.cpp - * @brief Parameters for IMU preintegration. + * @file NavStateFuseParams.cpp + * @brief Parameters for NavStateFuse * @author Jose Luis Blanco Claraco - * @date Sep 19, 2021 + * @date Jan 22, 2024 */ #include @@ -32,6 +32,10 @@ void NavStateFuseParams::loadFrom(const mrpt::containers::yaml& cfg) { MCP_LOAD_REQ(cfg, max_time_to_use_velocity_model); + MCP_LOAD_REQ(cfg, sliding_window_length); + MCP_LOAD_OPT(cfg, sigma_random_walk_acceleration_linear); MCP_LOAD_OPT(cfg, sigma_random_walk_acceleration_angular); + + MCP_LOAD_OPT(cfg, time_between_frames_to_warning); }