From a0592a6b5ab161194da1b162caaedda78ef3f2bf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Aug 2025 17:57:23 -0400 Subject: [PATCH 1/7] Fix Boost --- README.md | 22 +++++++++++++--- cmake/HandleBoost.cmake | 56 ++++++++++++++++++++++++++++------------- 2 files changed, 56 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index f897d89ba0..6d85d36581 100644 --- a/README.md +++ b/README.md @@ -34,15 +34,29 @@ In the root library folder execute: mkdir build cd build cmake .. -make check (optional, runs unit tests) +make check # optional, runs all unit tests make install ``` Prerequisites: -- [Boost](http://www.boost.org/users/download/) >= 1.65 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) -- A modern compiler, i.e., at least gcc 4.7.3 on Linux. +- A modern compiler: + - Mac: at least xcode-14.2 + - Linux: at least clang-11 or gcc-9 + - Windows: at least msvc-14.2 +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 + - Ubuntu: `sudo apt-get install cmake` + +Optional Boost prerequisite: + +Boost is now *optional*. Two cmake flags govern its behavior: + - `GTSAM_USE_BOOST_FEATURES` = `ON|OFF`: some of our timers and concept checking in the tests still depend on boost. + - `GTSAM_ENABLE_BOOST_SERIALIZATION` = `ON|OFF`: serialization of factor graphs, factors, etc still is done using boost + +If one or both of these flags are `ON`, you need to install [Boost](http://www.boost.org/users/download/) >= 1.70 + - Mac: `brew install boost` + - Ubuntu: `sudo apt-get install libboost-all-dev` + - Windows: recommend https://github.com/microsoft/vcpkg Optional prerequisites - used automatically if findable by CMake: diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake index 03251126ec..100c2021f6 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -20,37 +20,57 @@ if(MSVC) endif() endif() +# Prefer Boost's CMake package over FindBoost when available (CMake >= 3.30) +if(POLICY CMP0167) + cmake_policy(SET CMP0167 NEW) +endif() -# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.65) -set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) +set(BOOST_FIND_MINIMUM_VERSION 1.70) +# Do NOT include 'system' as a required component; it's header-only in modern Boost +set(BOOST_FIND_MINIMUM_COMPONENTS serialization filesystem thread program_options date_time timer chrono regex) -find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} REQUIRED) +# Prefer the Boost CMake package (CONFIG); fall back to the module if needed. +# Make 'system' optional to support Boost >= 1.69 where it is header-only. +find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} REQUIRED + COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} + OPTIONAL_COMPONENTS system CONFIG) -# Required components -if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR - NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.") -endif() +# Verify required imported targets exist (works for both BoostConfig and FindBoost) +foreach(_t IN ITEMS Boost::serialization Boost::filesystem Boost::thread Boost::date_time) + if(NOT TARGET ${_t}) + message(FATAL_ERROR "Missing required Boost component target: ${_t}. Please install/upgrade Boost or set BOOST_ROOT/Boost_DIR correctly.") + endif() +endforeach() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) -# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) + set(GTSAM_BOOST_LIBRARIES Boost::serialization - Boost::system Boost::filesystem Boost::thread Boost::date_time Boost::regex ) -if (GTSAM_DISABLE_NEW_TIMERS) - message("WARNING: GTSAM timing instrumentation manually disabled") - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) + +# Link Boost::system only when the target exists (older Boost providing a stub library) +if(TARGET Boost::system) + list(APPEND GTSAM_BOOST_LIBRARIES Boost::system) +endif() + +if(GTSAM_DISABLE_NEW_TIMERS) + message("WARNING: GTSAM timing instrumentation manually disabled") + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() - if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) + # Prefer linking Boost::timer and Boost::chrono if available as imported targets + if(TARGET Boost::timer AND TARGET Boost::chrono) + list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) + else() + # Fall back: when using the header-only timer, librt is needed on Linux only + if(UNIX AND NOT APPLE) + list(APPEND GTSAM_BOOST_LIBRARIES rt) + message("WARNING: Using header-only Boost timer; adding -lrt on Linux.") else() - list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt - message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") + message("WARNING: Using header-only Boost timer; no extra libs required on this platform.") endif() + endif() endif() From 2592f18f4c9f3e663e431c6b904aa17e0c1788b5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 22 Aug 2025 00:21:08 -0400 Subject: [PATCH 2/7] Fix badges --- .github/workflows/build-python.yml | 14 +++++++++++++- README.md | 13 ++++++------- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 97377e46fa..2bf0c7fe76 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -3,7 +3,19 @@ name: Python CI # Since this is a required check, specify paths-ignore in the check-paths job # instead of under 'pull_request:'. Otherwise, the check is still required but # never runs, and a maintainer must bypass the check in order to merge the PR. -on: [pull_request] +on: + push: + branches: + - develop + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' + pull_request: + paths-ignore: + - '**.md' + - '**.ipynb' + - 'myst.yml' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. diff --git a/README.md b/README.md index 6d85d36581..f82cd0b350 100644 --- a/README.md +++ b/README.md @@ -13,14 +13,13 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes Networks as the underlying computing paradigm rather than sparse matrices. -The current support matrix is: - -| Platform | Compiler | Build Status | -| :----------------: | :-------: | :------------------------------------------------------------------------------: | -| Ubuntu 22.04/24.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) | -| macOS | clang | ![macOS CI](https://github.com/borglab/gtsam/workflows/macOS%20CI/badge.svg) | -| Windows | MSVC | ![Windows CI](https://github.com/borglab/gtsam/workflows/Windows%20CI/badge.svg) | + +| CI Status | Platform | Compiler | +|:----------|:---------|:---------| +| [![Python CI](https://github.com/borglab/gtsam/actions/workflows/build-python.yml/badge.svg?branch=develop)](https://github.com/borglab/gtsam/actions/workflows/build-python.yml?query=branch%3Adevelop) | Ubuntu 22.04, MacOS 13-14, Windows | gcc/clang,MSVC | +| [![vcpkg](https://github.com/borglab/gtsam/actions/workflows/vcpkg.yml/badge.svg?branch=develop)](https://github.com/borglab/gtsam/actions/workflows/vcpkg.yml?query=branch%3Adevelop) | Latest Windows/Ubuntu/Mac | - | +| [![Build Wheels for Develop](https://github.com/borglab/gtsam/actions/workflows/build-cibw.yml/badge.svg?branch=develop)](https://github.com/borglab/gtsam/actions/workflows/build-cibw.yml?query=branch%3Adevelop) | See [pypi files](https://pypi.org/project/gtsam-develop/#files); no Windows| - | On top of the C++ library, GTSAM includes [wrappers for MATLAB & Python](#wrappers). From 9c6fcd98ee75c648b04c50761cfcbd47e1bf0c60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 23 Aug 2025 12:25:16 -0400 Subject: [PATCH 3/7] Fix boost on windows Documentation Simplify Force MODULE Fix back-slashes Upgrade cmake req Well-documented gemini solution Going in circles... Give gemini a chance yet another try More windows/boost fixes Yet another attempt Another attempt, aided by GPT-5 Try fixing boost on windows --- .github/scripts/python.sh | 16 ++++- .github/workflows/build-windows.yml | 10 +-- README.md | 2 +- cmake/HandleBoost.cmake | 70 ++++++++++++--------- gtsam/3rdparty/GeographicLib/CMakeLists.txt | 2 +- gtsam/3rdparty/metis/CMakeLists.txt | 2 +- 6 files changed, 64 insertions(+), 38 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 0c04eeec3d..60e35fc3f8 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -45,6 +45,16 @@ function build() { export CMAKE_GENERATOR=Ninja BUILD_PYBIND="ON" + + # Add Boost hints on Windows + BOOST_CMAKE_ARGS="" + if [[ "$OSTYPE" == "msys" || "$OSTYPE" == "win32" || "$OSTYPE" == "cygwin" ]]; then + if [ -n "${BOOST_ROOT}" ]; then + BOOST_ROOT_UNIX=$(echo "$BOOST_ROOT" | sed 's/\\/\//g') + BOOST_CMAKE_ARGS="-DBOOST_ROOT=${BOOST_ROOT_UNIX} -DBOOST_INCLUDEDIR=${BOOST_ROOT_UNIX}/include -DBOOST_LIBRARYDIR=${BOOST_ROOT_UNIX}/lib" + fi + fi + cmake $GITHUB_WORKSPACE \ -B build \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \ @@ -58,9 +68,11 @@ function build() -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ + -DGTSAM_USE_BOOST_FEATURES=ON \ + -DGTSAM_ENABLE_BOOST_SERIALIZATION=ON \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \ - -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install - + -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install \ + $BOOST_CMAKE_ARGS # Set to 2 cores so that Actions does not error out during resource provisioning. cmake --build build -j2 diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 94ea5c6e32..e16471ce3b 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -115,18 +115,20 @@ jobs: - name: Configuration shell: bash run: | - export CMAKE_GENERATOR=Ninja + export CMAKE_GENERATOR=Ninja + # Convert Windows backslashes to forward slashes for CMake + BOOST_ROOT_UNIX=$(echo "$BOOST_ROOT" | sed 's/\\/\//g') cmake -E remove_directory build if [ "${{ matrix.build_type }}" = "Release" ]; then cmake -B build \ -S . \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \ - -DBOOST_ROOT="${BOOST_ROOT}" \ -DGTSAM_USE_BOOST_FEATURES=ON \ -DGTSAM_ENABLE_BOOST_SERIALIZATION=ON \ - -DBOOST_INCLUDEDIR="${BOOST_ROOT}\\boost\\include" \ - -DBOOST_LIBRARYDIR="${BOOST_ROOT}\\lib" + -DBOOST_ROOT="${BOOST_ROOT_UNIX}" \ + -DBOOST_INCLUDEDIR="${BOOST_ROOT_UNIX}/include" \ + -DBOOST_LIBRARYDIR="${BOOST_ROOT_UNIX}/lib" else cmake -B build \ -S . \ diff --git a/README.md b/README.md index f82cd0b350..f7ec3c3210 100644 --- a/README.md +++ b/README.md @@ -55,7 +55,7 @@ Boost is now *optional*. Two cmake flags govern its behavior: If one or both of these flags are `ON`, you need to install [Boost](http://www.boost.org/users/download/) >= 1.70 - Mac: `brew install boost` - Ubuntu: `sudo apt-get install libboost-all-dev` - - Windows: recommend https://github.com/microsoft/vcpkg + - Windows: We highly recommend using the [vcpkg](https://github.com/microsoft/vcpkg) package manager. For other installation methods or troubleshooting, please see the guidance in the [cmake/HandleBoost.cmake](cmake/HandleBoost.cmake) script. Optional prerequisites - used automatically if findable by CMake: diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake index 100c2021f6..cfbb55b390 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -1,41 +1,58 @@ -############################################################################### -# Find boost - -# To change the path for boost, you will need to set: -# BOOST_ROOT: path to install prefix for boost -# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT +################################################################################ +# Find and configure the Boost libraries. +# +# To customize the Boost installation path, you can set the following variables +# when running CMake: +# - BOOST_ROOT: Path to the Boost installation prefix. +# - BOOST_INCLUDEDIR: Path to the Boost include directory. +# - BOOST_LIBRARYDIR: Path to the Boost library directory. +# +# These hints are particularly important for manual installations on Windows. +################################################################################ if(MSVC) - # By default, boost only builds static libraries on windows - set(Boost_USE_STATIC_LIBS ON) # only find static libs - # If we ever reset above on windows and, ... - # If we use Boost shared libs, disable auto linking. - # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. + # By default, Boost pre-compiled binaries for Windows are static libraries. + set(Boost_USE_STATIC_LIBS ON) if(NOT Boost_USE_STATIC_LIBS) list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) endif() - # Virtual memory range for PCH exceeded on VS2015 if(MSVC_VERSION LESS 1910) # older than VS2017 list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) endif() endif() -# Prefer Boost's CMake package over FindBoost when available (CMake >= 3.30) -if(POLICY CMP0167) - cmake_policy(SET CMP0167 NEW) -endif() +# --- GUIDANCE FOR LOCAL WINDOWS DEVELOPMENT --- +# +# If you enable Boost features on Windows, there are two primary ways to provide +# the Boost libraries: +# +# 1. RECOMMENDED: Use a package manager like vcpkg. +# - Vcpkg (https://github.com/microsoft/vcpkg) handles the download, build, +# and integration of Boost. It provides modern CMake config files and works +# seamlessly with this script without any special configuration. +# +# 2. ALTERNATIVE: Manual Installation (e.g., pre-compiled binaries). +# - If you download pre-compiled binaries, you MUST provide hints to CMake so +# it can find your installation. At a minimum, set BOOST_ROOT, e.g.: +# cmake .. -DBOOST_ROOT=C:/local/boost_1_87_0 +# +################################################################################ +# Set minimum required Boost version and components. +# NOTE: "system" is intentionally omitted. It is a transitive dependency of other +# components (like filesystem) and will be found automatically. Explicitly +# requesting it can cause issues with modern header-only versions of Boost. set(BOOST_FIND_MINIMUM_VERSION 1.70) -# Do NOT include 'system' as a required component; it's header-only in modern Boost set(BOOST_FIND_MINIMUM_COMPONENTS serialization filesystem thread program_options date_time timer chrono regex) -# Prefer the Boost CMake package (CONFIG); fall back to the module if needed. -# Make 'system' optional to support Boost >= 1.69 where it is header-only. +# Find the Boost package. On systems with modern installations (vcpkg, Homebrew), +# this will use CMake's "Config mode". With manual installations (especially on +# Windows), providing the hints above will trigger the legacy "Module mode". find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} REQUIRED COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} - OPTIONAL_COMPONENTS system CONFIG) + ) -# Verify required imported targets exist (works for both BoostConfig and FindBoost) +# Verify that the required Boost component targets were successfully found and imported. foreach(_t IN ITEMS Boost::serialization Boost::filesystem Boost::thread Boost::date_time) if(NOT TARGET ${_t}) message(FATAL_ERROR "Missing required Boost component target: ${_t}. Please install/upgrade Boost or set BOOST_ROOT/Boost_DIR correctly.") @@ -52,20 +69,15 @@ set(GTSAM_BOOST_LIBRARIES Boost::regex ) -# Link Boost::system only when the target exists (older Boost providing a stub library) -if(TARGET Boost::system) - list(APPEND GTSAM_BOOST_LIBRARIES Boost::system) -endif() - if(GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() - # Prefer linking Boost::timer and Boost::chrono if available as imported targets + # Link against compiled timer libraries if they exist. if(TARGET Boost::timer AND TARGET Boost::chrono) list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) else() - # Fall back: when using the header-only timer, librt is needed on Linux only + # Fallback for header-only timer: link librt on Linux. if(UNIX AND NOT APPLE) list(APPEND GTSAM_BOOST_LIBRARIES rt) message("WARNING: Using header-only Boost timer; adding -lrt on Linux.") @@ -73,4 +85,4 @@ else() message("WARNING: Using header-only Boost timer; no extra libs required on this platform.") endif() endif() -endif() +endif() \ No newline at end of file diff --git a/gtsam/3rdparty/GeographicLib/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/CMakeLists.txt index 25c923e018..8811071761 100644 --- a/gtsam/3rdparty/GeographicLib/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/CMakeLists.txt @@ -47,7 +47,7 @@ set (LIBVERSION_BUILD 17.1.2) string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER) -cmake_minimum_required (VERSION 3.5) # This version was released 2011-02-16 +cmake_minimum_required (VERSION 3.10) # User-settable variables diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index 8a577c73f0..4c395246ba 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.10) project(METIS) # Add flags for currect directory and below From a198644161aaa733031dc12657f8d08f838fca23 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Aug 2025 17:57:48 -0400 Subject: [PATCH 4/7] Move batch-integrate method to base --- gtsam/navigation/ImuFactor.cpp | 16 ---------------- gtsam/navigation/ImuFactor.h | 4 ---- gtsam/navigation/PreintegrationBase.cpp | 15 +++++++++++++++ gtsam/navigation/PreintegrationBase.h | 4 ++++ 4 files changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index cee2f5cb3d..077745e41c 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -90,22 +90,6 @@ void PreintegratedImuMeasurementsT::integrateMeasurement( preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; } -//------------------------------------------------------------------------------ -template -void PreintegratedImuMeasurementsT::integrateMeasurements( - const Matrix& measuredAccs, const Matrix& measuredOmegas, - const Matrix& dts) { - assert( - measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); - assert(dts.cols() >= 1); - assert(measuredAccs.cols() == dts.cols()); - assert(measuredOmegas.cols() == dts.cols()); - size_t n = static_cast(dts.cols()); - for (size_t j = 0; j < n; j++) { - integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j)); - } -} - //------------------------------------------------------------------------------ // ImuFactorT methods //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b03453a765..a269c2618f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -134,10 +134,6 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsT: public PreintegrationType { void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt) override; - /// Add multiple measurements, in matrix columns - void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, - const Matrix& dts); - /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index dd86b829c5..8653a077c1 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -113,6 +113,21 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, update(measuredAcc, measuredOmega, dt, &A, &B, &C); } +//------------------------------------------------------------------------------ +void PreintegrationBase::integrateMeasurements(const Matrix& measuredAccs, + const Matrix& measuredOmegas, + const Matrix& dts) { + assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && + dts.rows() == 1); + assert(dts.cols() >= 1); + assert(measuredAccs.cols() == dts.cols()); + assert(measuredOmegas.cols() == dts.cols()); + size_t n = static_cast(dts.cols()); + for (size_t j = 0; j < n; j++) { + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j)); + } +} + //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index de3a380cfe..9778c8ab1b 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -145,6 +145,10 @@ class GTSAM_EXPORT PreintegrationBase { virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); + /// Add multiple measurements, in matrix columns + void integrateMeasurements(const Matrix& measuredAccs, + const Matrix& measuredOmegas, const Matrix& dts); + /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, From 18ea6d7aa1f2e1ded863683b7e77db873820be32 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Aug 2025 17:58:13 -0400 Subject: [PATCH 5/7] Add stubbed GalileanImuFactor --- gtsam/navigation/GalileanImuFactor.cpp | 47 +++++++++++ gtsam/navigation/GalileanImuFactor.h | 110 +++++++++++++++++++++++++ 2 files changed, 157 insertions(+) create mode 100644 gtsam/navigation/GalileanImuFactor.cpp create mode 100644 gtsam/navigation/GalileanImuFactor.h diff --git a/gtsam/navigation/GalileanImuFactor.cpp b/gtsam/navigation/GalileanImuFactor.cpp new file mode 100644 index 0000000000..a4869eaf27 --- /dev/null +++ b/gtsam/navigation/GalileanImuFactor.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GalileanImuFactor.cpp + * @brief Implementation of Delama et al. IMU Factor + * @author Frank Dellaert + * @author Giulio Delama + */ + +#include + +namespace gtsam { + +// -- Constructors ------------------------------------------------------------ +PreintegratedImuMeasurementsG::PreintegratedImuMeasurementsG( + const std::shared_ptr& p, const imuBias::ConstantBias& biasHat) + : Base(p, biasHat), p_(p) {} + +// -- Integration API --------------------------------------------------------- +void PreintegratedImuMeasurementsG::resetIntegration() { + throw std::runtime_error("Not implemented: resetIntegration"); +} + +void PreintegratedImuMeasurementsG::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + throw std::runtime_error("Not implemented: integrateMeasurement"); +} + +// -- print / equals ---------------------------------------------------------- +void PreintegratedImuMeasurementsG::print(const std::string& s) const { + throw std::runtime_error("Not implemented: print"); +} +bool PreintegratedImuMeasurementsG::equals( + const PreintegratedImuMeasurementsG& o, double tol) const { + throw std::runtime_error("Not implemented: equals"); +} + +} // namespace gtsam diff --git a/gtsam/navigation/GalileanImuFactor.h b/gtsam/navigation/GalileanImuFactor.h new file mode 100644 index 0000000000..63949428b6 --- /dev/null +++ b/gtsam/navigation/GalileanImuFactor.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GalileanImuFactor.h + * @brief Equivariant IMU Preintegration with Biases: a Galilean Group + * Approach + * + * This class implements a new approach for Inertial Measurement Unit (IMU) + * preintegration based on the Galilean group, as described in: + * + * Giulio Delama, Alessandro Fornasier, Robert Mahony, Stephan Weiss, + * "Equivariant IMU Preintegration with Biases: a Galilean Group Approach," + * IEEE Robotics and Automation Letters, 2024. + * + * Inspired by recent advances in equivariant theory applied to biased inertial + * navigation systems (INS), this approach derives a discrete-time formulation + * of IMU preintegration on the Galilean group, geometrically coupling + * navigation states and biases for improved consistency and lower linearization + * error compared to traditional methods. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + +/// IMU preintegration based on the Galilean group (see class docs above). +class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase { + using Base = PreintegrationBase; + using Params = PreintegrationBase::Params; + + std::shared_ptr p_; + + public: + // Constructors (stubs) + explicit PreintegratedImuMeasurementsG( + const std::shared_ptr& p = std::make_shared(), + const imuBias::ConstantBias& biasHat = {}); + + // Public (const) accessors – stubs + double deltaTij() const { + throw std::runtime_error("Not implemented: deltaTij"); + } + Rot3 deltaRij() const override { + throw std::runtime_error("Not implemented: deltaRij"); + } + Vector3 deltaPij() const override { + throw std::runtime_error("Not implemented: deltaPij"); + } + Vector3 deltaVij() const override { + throw std::runtime_error("Not implemented: deltaVij"); + } + NavState deltaXij() const override { + throw std::runtime_error("Not implemented: deltaXij"); + } + Matrix9 preintMeasCov() const { + throw std::runtime_error("Not implemented: preintMeasCov"); + } + + // Bias correction (first‑order) – stub + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = {}) const override { + throw std::runtime_error("Not implemented: biasCorrectedDelta"); + } + + // integration API + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt) override; + + void resetIntegration() override; + + // Testable – stubs + void print( + const std::string& s = "PreintegratedImuMeasurementsG") const override; + bool equals(const PreintegratedImuMeasurementsG& other, + double tol = 1e-9) const; + + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override { + throw std::runtime_error("Not implemented: update"); + } +}; + +// Tell GTSAM about PreintegratedImuMeasurementsG print/equals: +template <> +struct traits + : public Testable {}; + +using GalileanImuFactor = ImuFactorT; + +} // namespace gtsam From 02b578ed06d41dd580884bef9ac8403799f4c2ea Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 21 Aug 2025 17:58:31 -0400 Subject: [PATCH 6/7] Make tests fail :-) --- gtsam/navigation/tests/imuFactorTesting.h | 2 + .../tests/testCombinedImuFactor.cpp | 85 ++++++++++--------- gtsam/navigation/tests/testImuFactor.cpp | 11 +-- 3 files changed, 55 insertions(+), 43 deletions(-) diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index eaccdb6e04..7eb8c18292 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -102,6 +102,8 @@ namespace { using CT = PreintegratedCombinedMeasurementsT; \ testGroup##testName##Helper(result_, this->name_); \ testGroup##testName##Helper(result_, this->name_); \ + using PG = PreintegratedImuMeasurementsG; \ + testGroup##testName##Helper(result_, this->name_); \ } \ template \ void testGroup##testName##Helper(TestResult& result_, \ diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 69cf8b4f0f..bbc22c7794 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -50,9 +51,13 @@ static std::shared_ptr Params( } // namespace combined /* ************************************************************************* */ -TEST_PIM(CombinedImuFactor, PreintegratedMeasurements ) { +TEST_PIM(CombinedImuFactor, PreintegratedMeasurements) { // Linearization point - Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases + Bias bias( + Vector3(0, 0, 0), + Vector3( + 0, 0, + 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); @@ -76,19 +81,18 @@ TEST_PIM(CombinedImuFactor, PreintegratedMeasurements ) { DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } - /* ************************************************************************* */ -TEST_PIM(CombinedImuFactor, ErrorWithBiases ) { - Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) +TEST_PIM(CombinedImuFactor, ErrorWithBiases) { + Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(0.5, 0.0, 0.0); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); + Point3(5.5, 1.0, -50.0)); Vector3 v2(0.5, 0.0, 0.0); auto p = combined::Params(); - p->omegaCoriolis = Vector3(0,0.1,0.1); + p->omegaCoriolis = Vector3(0, 0.1, 0.1); PIM pim(p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); // Measurements @@ -101,29 +105,30 @@ TEST_PIM(CombinedImuFactor, ErrorWithBiases ) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedPIM combined_pim(p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + CombinedPIM combined_pim( + p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor ImuFactorT imuFactor(X(1), V(1), X(2), V(2), B(1), pim); - CombinedImuFactorT combinedFactor(X(1), V(1), X(2), V(2), B(1), B(2), - combined_pim); + CombinedImuFactorT combinedFactor(X(1), V(1), X(2), V(2), B(1), + B(2), combined_pim); Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); - Vector errorActual = combinedFactor.evaluateError(x1, v1, x2, v2, bias, - bias2); + Vector errorActual = + combinedFactor.evaluateError(x1, v1, x2, v2, bias, bias2); EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); // Expected Jacobians Matrix H1e, H2e, H3e, H4e, H5e; - (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); + (void)imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) combinedFactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, - H3a, H4a, H5a, H6a); + (void)combinedFactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, + H4a, H5a, H6a); EXPECT(assert_equal(H1e, H1a.topRows(9))); EXPECT(assert_equal(H2e, H2a.topRows(9))); @@ -148,9 +153,11 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { PreintegratedCombinedMeasurementsT pim(p); testing::integrateMeasurements(measurements, &pim); - EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21( + preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22( + preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasOmega(), 1e-3)); } @@ -173,7 +180,8 @@ TEST_PIM(CombinedImuFactor, PredictPositionAndVelocity) { // Create factor const noiseModel::Gaussian::shared_ptr combinedmodel = noiseModel::Gaussian::Covariance(pim.preintMeasCov()); - const CombinedImuFactorT Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); + const CombinedImuFactorT Combinedfactor(X(1), V(1), X(2), V(2), + B(1), B(2), pim); // Predict const NavState actual = pim.predict(NavState(), bias); @@ -185,16 +193,17 @@ TEST_PIM(CombinedImuFactor, PredictPositionAndVelocity) { /* ************************************************************************* */ TEST_PIM(CombinedImuFactor, PredictRotation) { - const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) auto p = combined::Params(); CombinedPIM pim(p, bias); - const Vector3 measuredAcc = - kGravityAlongNavZDown; + const Vector3 measuredAcc = -kGravityAlongNavZDown; const Vector3 measuredOmega(0, 0, M_PI / 10.0); const double deltaT = 0.01; const double tol = 1e-4; for (int i = 0; i < 100; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - const CombinedImuFactorT Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); + const CombinedImuFactorT Combinedfactor(X(1), V(1), X(2), V(2), + B(1), B(2), pim); // Predict const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; @@ -226,20 +235,20 @@ TEST_PIM(CombinedImuFactor, CheckCovariance) { actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); Eigen::Matrix expected; - expected << 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, // + expected << 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 1.53125e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0.003125, 0, 0, 0.00125, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0.00125, 0, 0, 0.0005, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, 0, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5., 0, // 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5.; // regression @@ -282,8 +291,8 @@ TEST_PIM(CombinedImuFactor, SameCovariance) { cpim.integrateMeasurement(accMeas, omegaMeas, deltaT); // Assert if the noise covariance - EXPECT(assert_equal(pim.preintMeasCov(), - cpim.preintMeasCov().block(0, 0, 9, 9))); + EXPECT(assert_equal( + pim.preintMeasCov(), Matrix9(cpim.preintMeasCov().block(0, 0, 9, 9)))); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 4faf0570ea..d606c752ff 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -20,16 +20,17 @@ // #define ENABLE_TIMING // uncomment for timing results +#include +#include +#include +#include +#include +#include #include #include -#include #include #include -#include -#include -#include -#include #include #include "imuFactorTesting.h" From cb46ddeb98d9850dcef4ac91c257822e66a9ab37 Mon Sep 17 00:00:00 2001 From: "Delama, Giulio" Date: Mon, 8 Sep 2025 13:41:23 +0200 Subject: [PATCH 7/7] Add preintegration on Gal(3) (W.I.P.) - no IMU biases - preintegration covariance propagation is for RI error --- gtsam/navigation/GalileanImuFactor.cpp | 52 ++++++++++++++++++++-- gtsam/navigation/GalileanImuFactor.h | 61 ++++++++++++++++---------- 2 files changed, 85 insertions(+), 28 deletions(-) diff --git a/gtsam/navigation/GalileanImuFactor.cpp b/gtsam/navigation/GalileanImuFactor.cpp index a4869eaf27..89f00e2b05 100644 --- a/gtsam/navigation/GalileanImuFactor.cpp +++ b/gtsam/navigation/GalileanImuFactor.cpp @@ -27,21 +27,65 @@ PreintegratedImuMeasurementsG::PreintegratedImuMeasurementsG( // -- Integration API --------------------------------------------------------- void PreintegratedImuMeasurementsG::resetIntegration() { - throw std::runtime_error("Not implemented: resetIntegration"); + preintMatrix_ = Gal3().Identity(); + preintMeasCov_.setZero(); + preintBiasJacobian_.setZero(); } void PreintegratedImuMeasurementsG::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - throw std::runtime_error("Not implemented: integrateMeasurement"); + // Correct measurements for bias and construct input + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + if (p_->body_P_sensor) { + const auto& bRs = p_->body_P_sensor->rotation(); + acc = bRs * acc; + omega = bRs * omega; + } + Vector10 wInput = Vector10::Zero(); + wInput.head<3>() = omega; + wInput.segment<3>(3) = acc; + wInput(9) = 1.0; // time scale + Matrix10 Jl; + Gal3 wExp = Gal3::Expmap(-wInput * dt, Jl).inverse(); + Matrix10 preintMatrixAdjoint = preintMatrix_.AdjointMap(); + + // Propagate mean - from eq. (22) in the paper + preintMatrix_ = preintMatrix_.compose(wExp); + + // Propagate covariance - from eq. (35) in the paper + // IMPORTANT: the covariance propagation is based on the Right-Invariant (RI) + // error since the equivariant error (34) is by definition RI. If we want to + // use the Left-Invariant (LI) error to use NavState in the error formulation + // for the factor, we need to change the covariance propagation accordingly + // with the Adjoint. For RI error, the A_ matrix is identity. + Matrix10 B_ = preintMatrixAdjoint * Jl * dt; + Matrix10 Qd = Matrix10::Zero(); + Qd.block<3, 3>(0, 0) = p_->gyroscopeCovariance / dt; + Qd.block<3, 3>(3, 3) = p_->accelerometerCovariance / dt; + preintMeasCov_.noalias() += B_ * Qd * B_.transpose(); + + // Propagate bias Jacobian - from eq. (38) in the paper + Matrix20 phiBiasJacobian = Matrix20::Identity(); + phiBiasJacobian.block<10, 10>(9, 9) = -preintMatrixAdjoint * Jl * dt; + preintBiasJacobian_ = phiBiasJacobian * preintBiasJacobian_; } // -- print / equals ---------------------------------------------------------- void PreintegratedImuMeasurementsG::print(const std::string& s) const { - throw std::runtime_error("Not implemented: print"); + std::cout << (s.empty() ? s : s + "\n") + << "PreintegratedImuMeasurementsG:" << std::endl; + std::cout << " preintMatrix_: " << preintMatrix_ << std::endl; + std::cout << " preintMeasCov_: \n" << preintMeasCov_ << std::endl; + std::cout << " preintBiasJacobian_: \n" << preintBiasJacobian_ << std::endl; + std::cout << " biasHat_: " << biasHat_.vector().transpose() << std::endl; } bool PreintegratedImuMeasurementsG::equals( const PreintegratedImuMeasurementsG& o, double tol) const { - throw std::runtime_error("Not implemented: equals"); + return p_->equals(*o.p_, tol) && biasHat_.equals(o.biasHat_, tol) && + preintMatrix_.equals(o.preintMatrix_, tol) && + equal_with_abs_tol(preintMeasCov_, o.preintMeasCov_, tol) && + equal_with_abs_tol(preintBiasJacobian_, o.preintBiasJacobian_, tol); } } // namespace gtsam diff --git a/gtsam/navigation/GalileanImuFactor.h b/gtsam/navigation/GalileanImuFactor.h index 63949428b6..7892cd74be 100644 --- a/gtsam/navigation/GalileanImuFactor.h +++ b/gtsam/navigation/GalileanImuFactor.h @@ -32,11 +32,12 @@ #include #include +#include #include #include #include #include -#include +#include #include #include @@ -47,8 +48,17 @@ namespace gtsam { class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase { using Base = PreintegrationBase; using Params = PreintegrationBase::Params; - - std::shared_ptr p_; + using Matrix10 = Eigen::Matrix; + using Matrix20 = Eigen::Matrix; + + protected: + std::shared_ptr p_; ///< Preintegration parameters + Gal3 preintMatrix_; ///< Preintegration Matrix (Galilean group element) + Matrix20 preintBiasJacobian_; ///< Jacobian w.r.t. bias states + Matrix10 preintMeasCov_; ///< Preintegration covariance + ///< The dimension of the preintegration covariance matrix is 10x10 + ///< Because it includes Galilean Group (n=10) + ///< preintROTATION, preintVELOCITY, preintPOSITION, preintTIME public: // Constructors (stubs) @@ -57,29 +67,33 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase { const imuBias::ConstantBias& biasHat = {}); // Public (const) accessors – stubs - double deltaTij() const { - throw std::runtime_error("Not implemented: deltaTij"); - } - Rot3 deltaRij() const override { - throw std::runtime_error("Not implemented: deltaRij"); - } - Vector3 deltaPij() const override { - throw std::runtime_error("Not implemented: deltaPij"); - } - Vector3 deltaVij() const override { - throw std::runtime_error("Not implemented: deltaVij"); - } + double deltaTij() const { return preintMatrix_.time(); } + Rot3 deltaRij() const override { return preintMatrix_.rotation(); } + Vector3 deltaPij() const override { return preintMatrix_.translation(); } + Vector3 deltaVij() const override { return preintMatrix_.velocity(); } NavState deltaXij() const override { - throw std::runtime_error("Not implemented: deltaXij"); - } - Matrix9 preintMeasCov() const { - throw std::runtime_error("Not implemented: preintMeasCov"); + return NavState(preintMatrix_.rotation(), preintMatrix_.translation(), + preintMatrix_.velocity()); } + Matrix9 preintMeasCov() const { return preintMeasCov_.block<9, 9>(0, 0); } // Bias correction (first‑order) – stub Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = {}) const override { - throw std::runtime_error("Not implemented: biasCorrectedDelta"); + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Gal3 correctedPreintMatrix = + Gal3::Expmap(preintBiasJacobian_.block<10, 6>(0, 0) * biasIncr.vector()) + .compose(preintMatrix_); + + Vector9 xi; + NavState::dR(xi) = Rot3::Logmap(correctedPreintMatrix.rotation()); + NavState::dP(xi) = correctedPreintMatrix.translation(); + NavState::dV(xi) = correctedPreintMatrix.velocity(); + + if (H) { + throw std::runtime_error("Not implemented: biasCorrectedDelta Jacobian"); + } + return xi; } // integration API @@ -94,10 +108,9 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase { bool equals(const PreintegratedImuMeasurementsG& other, double tol = 1e-9) const; - void update(const Vector3& measuredAcc, const Vector3& measuredOmega, - const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override { - throw std::runtime_error("Not implemented: update"); - } + // void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + // const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override + // {} }; // Tell GTSAM about PreintegratedImuMeasurementsG print/equals: