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-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/.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 f897d89ba0..f7ec3c3210 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). @@ -34,15 +33,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: 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 03251126ec..cfbb55b390 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -1,56 +1,88 @@ -############################################################################### -# 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() +# --- 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 +# +################################################################################ -# 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 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) +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) +# 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} + ) -# 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 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.") + 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) + +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) + # 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() + # 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.") 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() +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 diff --git a/gtsam/navigation/GalileanImuFactor.cpp b/gtsam/navigation/GalileanImuFactor.cpp new file mode 100644 index 0000000000..89f00e2b05 --- /dev/null +++ b/gtsam/navigation/GalileanImuFactor.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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() { + preintMatrix_ = Gal3().Identity(); + preintMeasCov_.setZero(); + preintBiasJacobian_.setZero(); +} + +void PreintegratedImuMeasurementsG::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + // 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 { + 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 { + 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 new file mode 100644 index 0000000000..7892cd74be --- /dev/null +++ b/gtsam/navigation/GalileanImuFactor.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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; + 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) + explicit PreintegratedImuMeasurementsG( + const std::shared_ptr& p = std::make_shared(), + const imuBias::ConstantBias& biasHat = {}); + + // Public (const) accessors – stubs + 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 { + 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 { + 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 + 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 + // {} +}; + +// Tell GTSAM about PreintegratedImuMeasurementsG print/equals: +template <> +struct traits + : public Testable {}; + +using GalileanImuFactor = ImuFactorT; + +} // namespace gtsam 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, 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"