diff --git a/.github/scripts/boost.sh b/.github/scripts/boost.sh deleted file mode 100644 index 3c7e01274..000000000 --- a/.github/scripts/boost.sh +++ /dev/null @@ -1,18 +0,0 @@ -### Script to install Boost -BOOST_FOLDER=boost_${BOOST_VERSION//./_} - -# Download Boost -wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz - -# Unzip -tar -zxf ${BOOST_FOLDER}.tar.gz - -# Bootstrap -cd ${BOOST_FOLDER}/ -./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex - -# Build and install -sudo ./b2 -j$(nproc) install - -# Rebuild ld cache -sudo ldconfig diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 1676ad537..c48921504 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -58,8 +58,10 @@ function configure() fi # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs + # CMAKE_CXX_FLAGS="-w": Suppress warnings to avoid IO latency in CI logs cmake $SOURCE_DIR \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ + -DCMAKE_CXX_FLAGS="-w" \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ @@ -70,11 +72,9 @@ function configure() -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ + -DGTSAM_FORCE_SHARED_LIB=${GTSAM_FORCE_SHARED_LIB:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DGTSAM_SINGLE_TEST_EXE=OFF \ - -DBOOST_ROOT=$BOOST_ROOT \ - -DBoost_NO_SYSTEM_PATHS=ON \ - -DBoost_ARCHITECTURE=-x64 + -DGTSAM_SINGLE_TEST_EXE=OFF } @@ -97,7 +97,7 @@ function build () if [ "$(uname)" == "Linux" ]; then if (($(nproc) > 2)); then - make -j$(nproc) + make -j4 else make -j2 fi diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index b678a71db..129b5aaaf 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -1,6 +1,6 @@ name: Linux CI -on: [push, pull_request] +on: [pull_request] jobs: build: @@ -12,7 +12,6 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} - BOOST_VERSION: 1.67.0 strategy: fail-fast: true @@ -82,7 +81,7 @@ jobs: - name: Install Boost run: | - bash .github/scripts/boost.sh + sudo apt-get -y install libboost-all-dev - name: Build and Test run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 442e26e47..f09d589dc 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -53,6 +53,7 @@ jobs: steps: - name: Checkout uses: actions/checkout@v3 + - name: Install (Linux) if: runner.os == 'Linux' run: | @@ -79,6 +80,7 @@ jobs: echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV fi + - name: Install (macOS) if: runner.os == 'macOS' run: | @@ -88,22 +90,27 @@ jobs: sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Set Swap Space if: runner.os == 'Linux' uses: pierotofy/set-swap-space@master with: swap-size-gb: 6 + - name: Install Dependencies run: | bash .github/scripts/python.sh -d + - name: Build run: | bash .github/scripts/python.sh -b + - name: Test run: | bash .github/scripts/python.sh -t diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 7582bf41c..0d1cd5bdd 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -12,7 +12,7 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} GTSAM_BUILD_UNSTABLE: ON - BOOST_VERSION: 1.67.0 + GTSAM_FORCE_SHARED_LIB: ON # Make shared library to save memory on CI strategy: fail-fast: false @@ -93,14 +93,20 @@ jobs: sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev - sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV + if [ "${{ matrix.compiler }}" = "gcc" ]; then + sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib + echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV + else + sudo apt-get install -y clang-${{ matrix.version }} g++-multilib + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV + fi - name: Install Boost if: runner.os == 'Linux' run: | - bash .github/scripts/boost.sh + sudo apt-get -y install libboost-all-dev - name: Install (macOS) if: runner.os == 'macOS' @@ -151,6 +157,12 @@ jobs: echo "GTSAM_USE_BOOST_FEATURES=OFF" >> $GITHUB_ENV echo "GTSAM will not use BOOST" + - name: Set Swap Space + if: runner.os == 'Linux' + uses: pierotofy/set-swap-space@master + with: + swap-size-gb: 12 + - name: Build & Test run: | bash .github/scripts/unix.sh -t diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1a0126107..a2eb9e0c0 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -74,10 +74,20 @@ bool PreintegratedCombinedMeasurements::equals( //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { + // Base class method to reset the preintegrated measurements PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } +//------------------------------------------------------------------------------ +void PreintegratedCombinedMeasurements::resetIntegration( + const gtsam::Matrix6& Q_init) { + // Base class method to reset the preintegrated measurements + PreintegrationType::resetIntegration(); + p().biasAccOmegaInt = Q_init; + preintMeasCov_.setZero(); +} + //------------------------------------------------------------------------------ // sugar for derivative blocks #define D_R_R(H) (H)->block<3,3>(0,0) @@ -112,21 +122,21 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - Matrix3 theta_H_biasOmega = C.topRows<3>(); - Matrix3 pos_H_biasAcc = B.middleRows<3>(3); - Matrix3 vel_H_biasAcc = B.bottomRows<3>(); + Matrix3 theta_H_omega = C.topRows<3>(); + Matrix3 pos_H_acc = B.middleRows<3>(3); + Matrix3 vel_H_acc = B.bottomRows<3>(); - Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega; - Matrix3 pos_H_biasAccInit = -pos_H_biasAcc; - Matrix3 vel_H_biasAccInit = -vel_H_biasAcc; + Matrix3 theta_H_biasOmegaInit = -theta_H_omega; + Matrix3 pos_H_biasAccInit = -pos_H_acc; + Matrix3 vel_H_biasAccInit = -vel_H_acc; // overall Jacobian wrt preintegrated measurements (df/dx) Eigen::Matrix F; F.setZero(); F.block<9, 9>(0, 0) = A; - F.block<3, 3>(0, 12) = theta_H_biasOmega; - F.block<3, 3>(3, 9) = pos_H_biasAcc; - F.block<3, 3>(6, 9) = vel_H_biasAcc; + F.block<3, 3>(0, 12) = theta_H_omega; + F.block<3, 3>(3, 9) = pos_H_acc; + F.block<3, 3>(6, 9) = vel_H_acc; F.block<6, 6>(9, 9) = I_6x6; // Update the uncertainty on the state (matrix F in [4]). @@ -151,17 +161,17 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // BLOCK DIAGONAL TERMS D_R_R(&G_measCov_Gt) = - (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose()) // + (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) // + (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose()); D_t_t(&G_measCov_Gt) = - (pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) // + (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) // + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) // + (dt * iCov); D_v_v(&G_measCov_Gt) = - (vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) // + (vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) // + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose()); D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; @@ -175,12 +185,12 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( D_t_R(&G_measCov_Gt) = pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose(); D_t_v(&G_measCov_Gt) = - (pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) + + (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) + (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose()); D_v_R(&G_measCov_Gt) = vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose(); D_v_t(&G_measCov_Gt) = - (vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) + + (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) + (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()); preintMeasCov_.noalias() += G_measCov_Gt; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 2910fc76b..0ffb386a0 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -23,10 +23,7 @@ #pragma once /* GTSAM includes */ -#include -#include -#include -#include +#include namespace gtsam { @@ -56,67 +53,6 @@ typedef ManifoldPreintegration PreintegrationType; * Robotics: Science and Systems (RSS), 2015. */ -/// Parameters for pre-integration using PreintegratedCombinedMeasurements: -/// Usage: Create just a single Params and pass a shared pointer to the constructor -struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { - Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk - Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate. - - /// Default constructor makes uninitialized params struct. - /// Used for serialization. - PreintegrationCombinedParams() - : biasAccCovariance(I_3x3), - biasOmegaCovariance(I_3x3), - biasAccOmegaInt(I_6x6) {} - - /// See two named constructors below for good values of n_gravity in body frame - PreintegrationCombinedParams(const Vector3& _n_gravity) : - PreintegrationParams(_n_gravity), biasAccCovariance(I_3x3), - biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { - - } - - // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static std::shared_ptr MakeSharedD(double g = 9.81) { - return std::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, g))); - } - - // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static std::shared_ptr MakeSharedU(double g = 9.81) { - return std::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); - } - - void print(const std::string& s="") const override; - bool equals(const PreintegratedRotationParams& other, double tol) const override; - - void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } - void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } - void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; } - - const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } - const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } - -private: - -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); - ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); - ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); - ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); - } -#endif - -public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; - /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. @@ -127,17 +63,17 @@ public: * * @ingroup navigation */ -class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType { - -public: +class GTSAM_EXPORT PreintegratedCombinedMeasurements + : public PreintegrationType { + public: typedef PreintegrationCombinedParams Params; protected: /* Covariance matrix of the preintegrated measurements - * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega] - * (first-order propagation from *measurementCovariance*). - * PreintegratedCombinedMeasurements also include the biases and keep the correlation - * between the preintegrated measurements and the biases + * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc + * BiasOmega] (first-order propagation from *measurementCovariance*). + * PreintegratedCombinedMeasurements also include the biases and keep the + * correlation between the preintegrated measurements and the biases */ Eigen::Matrix preintMeasCov_; @@ -148,31 +84,31 @@ public: /// @{ /// Default constructor only for serialization and wrappers - PreintegratedCombinedMeasurements() { - preintMeasCov_.setZero(); - } + PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } /** * Default constructor, initializes the class with no measurements - * @param p Parameters, typically fixed in a single application + * @param p Parameters, typically fixed in a single application * @param biasHat Current estimate of acceleration and rotation rate biases + * @param preintMeasCov Covariance matrix used in noise model. */ PreintegratedCombinedMeasurements( const std::shared_ptr& p, - const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) - : PreintegrationType(p, biasHat) { - preintMeasCov_.setZero(); - } + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(), + const Eigen::Matrix& preintMeasCov = + Eigen::Matrix::Zero()) + : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {} /** - * Construct preintegrated directly from members: base class and preintMeasCov - * @param base PreintegrationType instance - * @param preintMeasCov Covariance matrix used in noise model. - */ - PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix& preintMeasCov) - : PreintegrationType(base), - preintMeasCov_(preintMeasCov) { - } + * Construct preintegrated directly from members: base class and + * preintMeasCov + * @param base PreintegrationType instance + * @param preintMeasCov Covariance matrix used in noise model. + */ + PreintegratedCombinedMeasurements( + const PreintegrationType& base, + const Eigen::Matrix& preintMeasCov) + : PreintegrationType(base), preintMeasCov_(preintMeasCov) {} /// Virtual destructor ~PreintegratedCombinedMeasurements() override {} @@ -185,6 +121,14 @@ public: /// Re-initialize PreintegratedCombinedMeasurements void resetIntegration() override; + /** + * @brief Re-initialize PreintegratedCombinedMeasurements with initial bias + * covariance estimate. + * + * @param Q_init The initial bias covariance estimates as a 6x6 matrix. + */ + void resetIntegration(const gtsam::Matrix6& Q_init); + /// const reference to params, shadows definition in base class Params& p() const { return *std::static_pointer_cast(this->p_); } /// @} @@ -198,13 +142,13 @@ public: /// @name Testable /// @{ /// print - void print(const std::string& s = "Preintegrated Measurements:") const override; + void print( + const std::string& s = "Preintegrated Measurements:") const override; /// equals bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; /// @} - /// @name Main functionality /// @{ @@ -219,7 +163,8 @@ public: * @param dt Time interval between two consecutive IMU measurements */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt) override; + const Vector3& measuredOmega, + const double dt) override; /// @} @@ -235,7 +180,7 @@ public: } #endif -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -244,34 +189,32 @@ public: * velocity of the vehicle, as well as bias at previous time step), and current * state (pose, velocity, bias at current time step). Following the pre- * integration scheme proposed in [2], the CombinedImuFactor includes many IMU - * measurements, which are "summarized" using the PreintegratedCombinedMeasurements - * class. There are 3 main differences wrpt the ImuFactor class: - * 1) The factor is 6-ways, meaning that it also involves both biases (previous - * and current time step).Therefore, the factor internally imposes the biases - * to be slowly varying; in particular, the matrices "biasAccCovariance" and - * "biasOmegaCovariance" described the random walk that models bias evolution. - * 2) The preintegration covariance takes into account the noise in the bias - * estimate used for integration. - * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves - * the correlation between the bias uncertainty and the preintegrated - * measurements uncertainty. + * measurements, which are "summarized" using the + * PreintegratedCombinedMeasurements class. There are 3 main differences wrpt + * the ImuFactor class: 1) The factor is 6-ways, meaning that it also involves + * both biases (previous and current time step).Therefore, the factor internally + * imposes the biases to be slowly varying; in particular, the matrices + * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that + * models bias evolution. 2) The preintegration covariance takes into account + * the noise in the bias estimate used for integration. 3) The covariance matrix + * of the PreintegratedCombinedMeasurements preserves the correlation between + * the bias uncertainty and the preintegrated measurements uncertainty. * * @ingroup navigation */ -class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN { -public: - -private: - +class GTSAM_EXPORT CombinedImuFactor + : public NoiseModelFactorN { + public: + private: typedef CombinedImuFactor This; typedef NoiseModelFactorN Base; + imuBias::ConstantBias, imuBias::ConstantBias> + Base; PreintegratedCombinedMeasurements _PIM_; -public: - + public: // Provide access to Matrix& version of evaluateError: using Base::evaluateError; @@ -315,7 +258,8 @@ public: DefaultKeyFormatter) const override; /// equals - bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; /// @} /** Access the preintegrated measurements. */ @@ -328,11 +272,13 @@ public: /// vector of errors Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, - const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - OptionalMatrixType H1, OptionalMatrixType H2, - OptionalMatrixType H3, OptionalMatrixType H4, - OptionalMatrixType H5, OptionalMatrixType H6) const override; + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const imuBias::ConstantBias& bias_j, + OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3, OptionalMatrixType H4, + OptionalMatrixType H5, + OptionalMatrixType H6) const override; private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION @@ -347,7 +293,7 @@ public: } #endif -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // class CombinedImuFactor diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h new file mode 100644 index 000000000..30441ec36 --- /dev/null +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationCombinedParams.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + * @author Varun Agrawal + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Parameters for pre-integration using PreintegratedCombinedMeasurements: +/// Usage: Create just a single Params and pass a shared pointer to the +/// constructor +struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { + Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing + ///< accelerometer bias random walk + Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing + ///< gyroscope bias random walk + Matrix6 biasAccOmegaInt; ///< covariance of bias used as initial estimate. + + /// Default constructor makes uninitialized params struct. + /// Used for serialization. + PreintegrationCombinedParams() + : biasAccCovariance(I_3x3), + biasOmegaCovariance(I_3x3), + biasAccOmegaInt(I_6x6) {} + + /// See two named constructors below for good values of n_gravity in body + /// frame + PreintegrationCombinedParams(const Vector3& n_gravity) + : PreintegrationParams(n_gravity), + biasAccCovariance(I_3x3), + biasOmegaCovariance(I_3x3), + biasAccOmegaInt(I_6x6) {} + + // Default Params for a Z-down navigation frame, such as NED: gravity points + // along positive Z-axis + static std::shared_ptr MakeSharedD( + double g = 9.81) { + return std::shared_ptr( + new PreintegrationCombinedParams(Vector3(0, 0, g))); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points + // along negative Z-axis + static std::shared_ptr MakeSharedU( + double g = 9.81) { + return std::shared_ptr( + new PreintegrationCombinedParams(Vector3(0, 0, -g))); + } + + void print(const std::string& s = "") const override; + bool equals(const PreintegratedRotationParams& other, + double tol) const override; + + void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance = cov; } + void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance = cov; } + void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt = cov; } + + const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } + const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } + const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); + ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); + } +#endif + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 19d241c95..665287898 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -106,7 +106,7 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const { PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate( double T, const Bias& estimatedBias, bool corrupted) const { gttic_(integrate); - PreintegratedCombinedMeasurements pim(p_, estimatedBias); + PreintegratedCombinedMeasurements pim(p_, estimatedBias, preintMeasCov_); const double dt = imuSampleTime(); const size_t nrSteps = T / dt; diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 4d11793ef..13789b3e7 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -118,15 +118,19 @@ class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner { private: const SharedParams p_; const Bias estimatedBias_; + const Eigen::Matrix preintMeasCov_; public: CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p, double imuSampleTime = 1.0 / 100.0, - const Bias& bias = Bias()) + const Bias& bias = Bias(), + const Eigen::Matrix& preintMeasCov = + Eigen::Matrix::Zero()) : ScenarioRunner(scenario, static_cast(p), imuSampleTime, bias), p_(p), - estimatedBias_(bias) {} + estimatedBias_(bias), + preintMeasCov_(preintMeasCov) {} /// Integrate measurements for T seconds into a PIM PreintegratedCombinedMeasurements integrate( diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 858c9f9fe..c4fefb8ff 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -36,14 +36,17 @@ namespace testing { // Create default parameters with Z-down and above noise parameters -static std::shared_ptr Params() { +static std::shared_ptr Params( + const Matrix3& biasAccCovariance = Matrix3::Zero(), + const Matrix3& biasOmegaCovariance = Matrix3::Zero(), + const Matrix6& biasAccOmegaInt = Matrix6::Zero()) { auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->integrationCovariance = 0.0001 * I_3x3; - p->biasAccCovariance = Z_3x3; - p->biasOmegaCovariance = Z_3x3; - p->biasAccOmegaInt = Z_6x6; + p->biasAccCovariance = biasAccCovariance; + p->biasOmegaCovariance = biasOmegaCovariance; + p->biasAccOmegaInt = biasAccOmegaInt; return p; } } // namespace testing @@ -250,6 +253,7 @@ TEST(CombinedImuFactor, CheckCovariance) { EXPECT(assert_equal(expected, actual.preintMeasCov())); } +/* ************************************************************************* */ // Test that the covariance values for the ImuFactor and the CombinedImuFactor // (top-left 9x9) are the same TEST(CombinedImuFactor, SameCovariance) { @@ -316,6 +320,43 @@ TEST(CombinedImuFactor, Accelerating) { EXPECT(assert_equal(estimatedCov, expected, 0.1)); } +/* ************************************************************************* */ +TEST(CombinedImuFactor, ResetIntegration) { + const double a = 0.2, v = 50; + + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 initial_position(10, 20, 0); + const Vector3 initial_velocity(v, 0, 0); + + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); + + const double T = 3.0; // seconds + + auto preinMeasCov = 0.001 * Eigen::Matrix::Identity(); + CombinedScenarioRunner runner( + scenario, + testing::Params(Matrix3::Zero(), Matrix3::Zero(), + 0.1 * Matrix6::Identity()), + T / 10, imuBias::ConstantBias(), preinMeasCov); + + PreintegratedCombinedMeasurements pim = runner.integrate(T); + // Make copy for testing different conditions + PreintegratedCombinedMeasurements pim2 = pim; + + // Test default method + pim.resetIntegration(); + Matrix6 expected = 0.1 * I_6x6; + EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9)); + + // Test method where Q_init is provided + Matrix6 expected_Q_init = I_6x6 * 0.001; + pim2.resetIntegration(expected_Q_init); + EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInt, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/gtsam/examples/CombinedImuFactorExample.py b/python/gtsam/examples/CombinedImuFactorExample.py new file mode 100644 index 000000000..e3f8c6b34 --- /dev/null +++ b/python/gtsam/examples/CombinedImuFactorExample.py @@ -0,0 +1,256 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +A script validating and demonstrating inference with the CombinedImuFactor. + +Author: Varun Agrawal +""" + +# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order + +from __future__ import print_function + +import argparse +import math + +import matplotlib.pyplot as plt +import numpy as np +from gtsam.symbol_shorthand import B, V, X +from gtsam.utils.plot import plot_pose3 +from mpl_toolkits.mplot3d import Axes3D +from PreintegrationExample import POSES_FIG, PreintegrationExample + +import gtsam + +GRAVITY = 9.81 + +np.set_printoptions(precision=3, suppress=True) + + +def parse_args() -> argparse.Namespace: + """Parse command line arguments.""" + parser = argparse.ArgumentParser("CombinedImuFactorExample.py") + parser.add_argument("--twist_scenario", + default="sick_twist", + choices=("zero_twist", "forward_twist", "loop_twist", + "sick_twist")) + parser.add_argument("--time", + "-T", + default=12, + type=int, + help="Total navigation time in seconds") + parser.add_argument("--compute_covariances", + default=False, + action='store_true') + parser.add_argument("--verbose", default=False, action='store_true') + return parser.parse_args() + + +class CombinedImuFactorExample(PreintegrationExample): + """Class to run example of the Imu Factor.""" + def __init__(self, twist_scenario: str = "sick_twist"): + self.velocity = np.array([2, 0, 0]) + self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + self.biasNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.001) + + # Choose one of these twists to change scenario: + twist_scenarios = dict( + zero_twist=(np.zeros(3), np.zeros(3)), + forward_twist=(np.zeros(3), self.velocity), + loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity), + sick_twist=(np.array([math.radians(30), -math.radians(30), + 0]), self.velocity)) + + accBias = np.array([-0.3, 0.1, 0.2]) + gyroBias = np.array([0.1, 0.3, -0.1]) + bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) + + params = gtsam.PreintegrationCombinedParams.MakeSharedU(GRAVITY) + + # Some arbitrary noise sigmas + gyro_sigma = 1e-3 + accel_sigma = 1e-3 + I_3x3 = np.eye(3) + params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3) + params.setAccelerometerCovariance(accel_sigma**2 * I_3x3) + params.setIntegrationCovariance(1e-7**2 * I_3x3) + + dt = 1e-2 + super(CombinedImuFactorExample, + self).__init__(twist_scenarios[twist_scenario], bias, params, dt) + + def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph): + """Add a prior on the navigation state at time `i`.""" + state = self.scenario.navState(i) + graph.push_back( + gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back( + gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise)) + graph.push_back( + gtsam.PriorFactorConstantBias(B(i), self.actualBias, + self.biasNoise)) + + def optimize(self, graph: gtsam.NonlinearFactorGraph, + initial: gtsam.Values): + """Optimize using Levenberg-Marquardt optimization.""" + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + return result + + def plot(self, + values: gtsam.Values, + title: str = "Estimated Trajectory", + fignum: int = POSES_FIG + 1, + show: bool = False): + """ + Plot poses in values. + + Args: + values: The values object with the poses to plot. + title: The title of the plot. + fignum: The matplotlib figure number. + POSES_FIG is a value from the PreintegrationExample + which we simply increment to generate a new figure. + show: Flag indicating whether to display the figure. + """ + i = 0 + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + plot_pose3(fignum, pose_i, 1) + i += 1 + plt.title(title) + + gtsam.utils.plot.set_axes_equal(fignum) + + i = 0 + while values.exists(B(i)): + print("Bias Value {0}".format(i), values.atConstantBias(B(i))) + i += 1 + + plt.ioff() + + if show: + plt.show() + + def run(self, + T: int = 12, + compute_covariances: bool = False, + verbose: bool = True): + """ + Main runner. + + Args: + T: Total trajectory time. + compute_covariances: Flag indicating whether to compute marginal covariances. + verbose: Flag indicating if printing should be verbose. + """ + graph = gtsam.NonlinearFactorGraph() + + # initialize data structure for pre-integrated IMU measurements + pim = gtsam.PreintegratedCombinedMeasurements(self.params, + self.actualBias) + + num_poses = T # assumes 1 factor per second + initial = gtsam.Values() + + # simulate the loop + i = 0 # state index + initial_state_i = self.scenario.navState(0) + initial.insert(X(i), initial_state_i.pose()) + initial.insert(V(i), initial_state_i.velocity()) + initial.insert(B(i), self.actualBias) + + # add prior on beginning + self.addPrior(0, graph) + + for k, t in enumerate(np.arange(0, T, self.dt)): + # get measurements and add them to PIM + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + + if (k + 1) % int(1 / self.dt) == 0: + # Plot every second + self.plotGroundTruthPose(t, scale=1) + plt.title("Ground Truth Trajectory") + + # create IMU factor every second + factor = gtsam.CombinedImuFactor(X(i), V(i), X(i + 1), + V(i + 1), B(i), B(i + 1), pim) + graph.push_back(factor) + + if verbose: + print(factor) + print("Predicted state at {0}:\n{1}".format( + t + self.dt, + pim.predict(initial_state_i, self.actualBias))) + + pim.resetIntegration() + + rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1) + translationNoise = gtsam.Point3(*np.random.randn(3) * 1) + poseNoise = gtsam.Pose3(rotationNoise, translationNoise) + + actual_state_i = self.scenario.navState(t + self.dt) + print("Actual state at {0}:\n{1}".format( + t + self.dt, actual_state_i)) + + # Set initial state to current + initial_state_i = actual_state_i + + noisy_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3) * 0.1) + noisy_bias_i = self.actualBias + gtsam.imuBias.ConstantBias( + np.random.randn(3) * 0.1, + np.random.randn(3) * 0.1) + + initial.insert(X(i + 1), noisy_state_i.pose()) + initial.insert(V(i + 1), noisy_state_i.velocity()) + initial.insert(B(i + 1), noisy_bias_i) + i += 1 + + # add priors on end + self.addPrior(num_poses - 1, graph) + + initial.print("Initial values:") + + result = self.optimize(graph, initial) + + result.print("Optimized values:") + print("------------------") + print("Initial Error =", graph.error(initial)) + print("Final Error =", graph.error(result)) + print("------------------") + + if compute_covariances: + # Calculate and print marginal covariances + marginals = gtsam.Marginals(graph, result) + print("Covariance on bias:\n", + marginals.marginalCovariance(BIAS_KEY)) + for i in range(num_poses): + print("Covariance on pose {}:\n{}\n".format( + i, marginals.marginalCovariance(X(i)))) + print("Covariance on vel {}:\n{}\n".format( + i, marginals.marginalCovariance(V(i)))) + + self.plot(result, show=True) + + +if __name__ == '__main__': + args = parse_args() + + CombinedImuFactorExample(args.twist_scenario).run(args.time, + args.compute_covariances, + args.verbose) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index c86a4e216..a0c833274 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -17,15 +17,15 @@ from __future__ import print_function import argparse import math -import gtsam import matplotlib.pyplot as plt import numpy as np from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 from mpl_toolkits.mplot3d import Axes3D - from PreintegrationExample import POSES_FIG, PreintegrationExample +import gtsam + BIAS_KEY = B(0) GRAVITY = 9.81 @@ -185,7 +185,9 @@ class ImuFactorExample(PreintegrationExample): if verbose: print(factor) - print(pim.predict(initial_state_i, self.actualBias)) + print("Predicted state at {0}:\n{1}".format( + t + self.dt, + pim.predict(initial_state_i, self.actualBias))) pim.resetIntegration() @@ -197,6 +199,9 @@ class ImuFactorExample(PreintegrationExample): print("Actual state at {0}:\n{1}".format( t + self.dt, actual_state_i)) + # Set initial state to current + initial_state_i = actual_state_i + noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3) * 0.1)