From dd9007057295eae4cd2aaed6fe761782ec704d0c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 7 Oct 2024 13:30:25 +0200 Subject: [PATCH] Start integrating GNSS observation. Added a new CLI program mola-navstate-cli for testing state fusion --- mola_navstate_fg/CMakeLists.txt | 9 ++ mola_navstate_fg/apps/mola-navstate-cli.cpp | 93 +++++++++++++++++++ .../include/mola_navstate_fg/NavStateFG.h | 3 + mola_navstate_fg/src/NavStateFG.cpp | 8 ++ .../tests/mola-navstate-fg-params.yaml | 10 ++ .../include/mola_navstate_fuse/NavStateFuse.h | 3 + mola_navstate_fuse/src/NavStateFuse.cpp | 6 ++ 7 files changed, 132 insertions(+) create mode 100644 mola_navstate_fg/apps/mola-navstate-cli.cpp create mode 100644 mola_navstate_fg/tests/mola-navstate-fg-params.yaml diff --git a/mola_navstate_fg/CMakeLists.txt b/mola_navstate_fg/CMakeLists.txt index 95fd16b1..80878d28 100644 --- a/mola_navstate_fg/CMakeLists.txt +++ b/mola_navstate_fg/CMakeLists.txt @@ -45,6 +45,15 @@ mola_add_library( ) target_include_directories(${PROJECT_NAME} PRIVATE ".") +# ----------------------- +# define cli apps: +mola_add_executable( + TARGET mola-navstate-cli + SOURCES apps/mola-navstate-cli.cpp + LINK_LIBRARIES + ${PROJECT_NAME} +) + # ----------------------- # define tests: enable_testing() diff --git a/mola_navstate_fg/apps/mola-navstate-cli.cpp b/mola_navstate_fg/apps/mola-navstate-cli.cpp new file mode 100644 index 00000000..3e811d29 --- /dev/null +++ b/mola_navstate_fg/apps/mola-navstate-cli.cpp @@ -0,0 +1,93 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ + +#include +#include +#include +#include +#include +#include + +#include + +namespace +{ +void run_navstate(const std::string& paramsFile, const std::string& rawlogFile) +{ + using mrpt::obs::CObservationGPS; + using mrpt::obs::CObservationIMU; + using mrpt::obs::CObservationRobotPose; + + mola::NavStateFG nav; + + std::cout << "Initalizing from: " << paramsFile << std::endl; + + const auto cfg = mrpt::containers::yaml::FromFile(paramsFile); + nav.initialize(cfg); + + std::cout << "Reading dataset from: " << rawlogFile << std::endl; + + mrpt::obs::CRawlog dataset; + dataset.loadFromRawLogFile(rawlogFile); + + const std::string frame_id = "map"; + + std::cout << "Read entries: " << dataset.size() << std::endl; + + for (size_t i = 0; i < dataset.size(); i++) + { + const auto o = dataset.getAsObservation(i); + if (!o) continue; + + if (auto oGPS = std::dynamic_pointer_cast(o); oGPS) + { + nav.fuse_gnss(*oGPS); + } + else if (auto oPose = + std::dynamic_pointer_cast(o); + oPose) + { + nav.fuse_pose(oPose->timestamp, oPose->pose, frame_id); + } + else if (auto oImu = std::dynamic_pointer_cast(o); + oImu) + { + nav.fuse_imu(*oImu); + } + else + { + std::cout << "[Warning] Ignoring observation #" << i << ": '" + << o->sensorLabel + << "' of type: " << o->GetRuntimeClass()->className + << "\n"; + } + + } // for each entry +} + +} // namespace + +int main(int argc, char** argv) +{ + try + { + if (argc != 3) + { + std::cerr << "Usage: " << argv[0] << " params.yaml dataset.rawlog" + << std::endl; + return 1; + } + + run_navstate(argv[1], argv[2]); + + return 0; + } + catch (const std::exception& e) + { + std::cerr << "Exception: " << e.what() << std::endl; + return 1; + } +} diff --git a/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h b/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h index 4c23812a..14a958c1 100644 --- a/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h +++ b/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h @@ -128,6 +128,9 @@ class NavStateFG : public mola::NavStateFilter /** Integrates new IMU observations into the estimator */ void fuse_imu(const mrpt::obs::CObservationIMU& imu) override; + /** Integrates new GNSS observations into the estimator */ + void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override; + /** Integrates new twist estimation (in the odom frame) */ void fuse_twist( const mrpt::Clock::time_point& timestamp, diff --git a/mola_navstate_fg/src/NavStateFG.cpp b/mola_navstate_fg/src/NavStateFG.cpp index 3bce7707..4c7633f1 100644 --- a/mola_navstate_fg/src/NavStateFG.cpp +++ b/mola_navstate_fg/src/NavStateFG.cpp @@ -154,6 +154,14 @@ void NavStateFG::fuse_imu(const mrpt::obs::CObservationIMU& imu) delete_too_old_entries(); } +void NavStateFG::fuse_gnss(const mrpt::obs::CObservationGPS& gps) +{ + THROW_EXCEPTION("TODO"); + (void)gps; + + delete_too_old_entries(); +} + void NavStateFG::fuse_pose( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose, const std::string& frame_id) diff --git a/mola_navstate_fg/tests/mola-navstate-fg-params.yaml b/mola_navstate_fg/tests/mola-navstate-fg-params.yaml new file mode 100644 index 00000000..3b849a8e --- /dev/null +++ b/mola_navstate_fg/tests/mola-navstate-fg-params.yaml @@ -0,0 +1,10 @@ +# Config for NavStateFGParams +sliding_window_length: 5.0 # [s] +max_time_to_use_velocity_model: 2.0 # [s] +time_between_frames_to_warning: 2.0 # [s] +sigma_random_walk_acceleration_linear: 1.0 # [m/s²] +sigma_random_walk_acceleration_angular: 1.0 # [rad/s²] +sigma_integrator_position: 0.10 # [m] +sigma_integrator_orientation: 0.10 # [rad] +robust_param: 0 +max_rmse: 2 diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h index 2291feb9..10f321d7 100644 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h +++ b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h @@ -98,6 +98,9 @@ class NavStateFuse : public mola::NavStateFilter /** Integrates new IMU observations into the estimator */ void fuse_imu(const mrpt::obs::CObservationIMU& imu) override; + /** Integrates new GNSS observations into the estimator */ + void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override; + /** Integrates new twist estimation (in the odom frame) */ void fuse_twist( const mrpt::Clock::time_point& timestamp, diff --git a/mola_navstate_fuse/src/NavStateFuse.cpp b/mola_navstate_fuse/src/NavStateFuse.cpp index 5359cbb5..88d19e3d 100644 --- a/mola_navstate_fuse/src/NavStateFuse.cpp +++ b/mola_navstate_fuse/src/NavStateFuse.cpp @@ -70,6 +70,12 @@ void NavStateFuse::fuse_imu(const mrpt::obs::CObservationIMU& imu) (void)imu; } +void NavStateFuse::fuse_gnss(const mrpt::obs::CObservationGPS& gps) +{ + // TODO(jlbc) + (void)gps; +} + void NavStateFuse::fuse_pose( const mrpt::Clock::time_point& timestamp, const mrpt::poses::CPose3DPDFGaussian& pose,