From 843f53c6cc84fb206f95e74e862529990349b7a0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 14 Oct 2022 16:50:19 -0400 Subject: [PATCH 01/13] move PreintegrationCombinedParams to its own file --- gtsam/navigation/CombinedImuFactor.h | 66 +---------- .../navigation/PreintegrationCombinedParams.h | 104 ++++++++++++++++++ 2 files changed, 108 insertions(+), 62 deletions(-) create mode 100644 gtsam/navigation/PreintegrationCombinedParams.h diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 69d72ad9b..05e4b481e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -23,11 +23,12 @@ #pragma once /* GTSAM includes */ -#include -#include -#include #include #include +#include +#include +#include +#include namespace gtsam { @@ -57,65 +58,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 boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::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 boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::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: - - /** 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); - } - -public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; - /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h new file mode 100644 index 000000000..2c99463bc --- /dev/null +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * 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 boost::shared_ptr MakeSharedD( + double g = 9.81) { + return boost::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 boost::shared_ptr MakeSharedU( + double g = 9.81) { + return boost::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: + /** 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); + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace gtsam From 090fb4e275179c94a37d838e0f3b6c8d119606db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 14 Oct 2022 17:11:24 -0400 Subject: [PATCH 02/13] Add python example for CombinedImuFactor --- .../examples/CombinedImuFactorExample.py | 251 ++++++++++++++++++ 1 file changed, 251 insertions(+) create mode 100644 python/gtsam/examples/CombinedImuFactorExample.py diff --git a/python/gtsam/examples/CombinedImuFactorExample.py b/python/gtsam/examples/CombinedImuFactorExample.py new file mode 100644 index 000000000..82714dd7c --- /dev/null +++ b/python/gtsam/examples/CombinedImuFactorExample.py @@ -0,0 +1,251 @@ +""" +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 the CobiendImuFactor inference. + +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 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 + +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(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)) + + 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) From 9891449049a0b359d486e882c21a84d07c8cf5e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 15 Oct 2022 09:02:45 -0400 Subject: [PATCH 03/13] rename jacobian variables so it matches the math in ImuFactor.pdf --- gtsam/navigation/CombinedImuFactor.cpp | 28 +++++++++++++------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 8d3a7dd31..5260597fa 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -110,21 +110,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]). @@ -149,17 +149,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; @@ -173,12 +173,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; From 7f14b6e6ce5180dbea1115eea0144e4f627f1cd4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 15 Oct 2022 09:29:00 -0400 Subject: [PATCH 04/13] update resetIntegration to set the initial covariance of the bias --- gtsam/navigation/CombinedImuFactor.cpp | 11 +++++++ gtsam/navigation/CombinedImuFactor.h | 9 +++++ .../tests/testCombinedImuFactor.cpp | 33 +++++++++++++++++++ 3 files changed, 53 insertions(+) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 5260597fa..57e86d3b6 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -72,6 +72,17 @@ bool PreintegratedCombinedMeasurements::equals( //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { + // Set the initial bias covariance to + // the estimated covariance from the last step. + p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9); + PreintegrationType::resetIntegration(); + preintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void PreintegratedCombinedMeasurements::resetIntegration( + const gtsam::Matrix6& Q_init) { + p().biasAccOmegaInt = Q_init; PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 05e4b481e..ff247009e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -126,6 +126,15 @@ 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 6x6 matrix. If not + * provided, it uses the last values from the preintMeasCov. + */ + void resetIntegration(const gtsam::Matrix6& Q_init); + /// const reference to params, shadows definition in base class Params& p() const { return *boost::static_pointer_cast(this->p_); } /// @} diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aacfff0f0..250a0dc42 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -250,6 +250,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 +317,38 @@ 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 + + CombinedScenarioRunner runner(scenario, testing::Params(), T / 10); + + PreintegratedCombinedMeasurements pim = runner.integrate(T); + // Make copy for testing different conditions + PreintegratedCombinedMeasurements pim2 = pim; + + // Test default method + pim.resetIntegration(); + Matrix6 expected = Z_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; From 6eef703019e38af836d3dadbffb6d5627e4ff657 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 16:21:42 -0500 Subject: [PATCH 05/13] reorder operations in resetIntegration and add docstrings --- gtsam/navigation/CombinedImuFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 57e86d3b6..142faedd3 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -72,18 +72,20 @@ bool PreintegratedCombinedMeasurements::equals( //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { + // Base class method to reset the preintegrated measurements + PreintegrationType::resetIntegration(); // Set the initial bias covariance to // the estimated covariance from the last step. p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9); - PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration( const gtsam::Matrix6& Q_init) { - p().biasAccOmegaInt = Q_init; + // Base class method to reset the preintegrated measurements PreintegrationType::resetIntegration(); + p().biasAccOmegaInt = Q_init; preintMeasCov_.setZero(); } From dc83c6f6185ade4bc0b56914c9d276662cbeb5c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 19:22:33 -0500 Subject: [PATCH 06/13] update CombinedScenarioRunner to accept preintMeasCov --- gtsam/navigation/ScenarioRunner.cpp | 2 +- gtsam/navigation/ScenarioRunner.h | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 9d3e258de..856f0e692 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -108,7 +108,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 cee5a54ab..7f1efa063 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( From 41d0606816524f3e546e641e5887c32345513bd6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 19:23:55 -0500 Subject: [PATCH 07/13] update PreintegratedCombinedMeasurements constructor to take default preintMeasCov --- gtsam/navigation/CombinedImuFactor.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ff247009e..71169ab2c 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -95,15 +95,16 @@ public: /** * 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 boost::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 From 3006a764446d6b7680a2aae840f32dc642d26c8a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 19:24:22 -0500 Subject: [PATCH 08/13] update CombinedImuFactor test to better test for integration reset --- .../tests/testCombinedImuFactor.cpp | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 250a0dc42..fcda1da11 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 boost::shared_ptr Params() { +static boost::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 @@ -332,7 +335,12 @@ TEST(CombinedImuFactor, ResetIntegration) { const double T = 3.0; // seconds - CombinedScenarioRunner runner(scenario, testing::Params(), T / 10); + 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 @@ -340,8 +348,8 @@ TEST(CombinedImuFactor, ResetIntegration) { // Test default method pim.resetIntegration(); - Matrix6 expected = Z_6x6; - EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt , 1e-9)); + Matrix6 expected = 0.001 * 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; From 2fb93dbdfb53783b775f73da6f127040776f8a4f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Jan 2023 10:15:40 -0500 Subject: [PATCH 09/13] remove biasAccOmegaInit default reset --- gtsam/navigation/CombinedImuFactor.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 142faedd3..e5ce5a711 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -74,9 +74,6 @@ bool PreintegratedCombinedMeasurements::equals( void PreintegratedCombinedMeasurements::resetIntegration() { // Base class method to reset the preintegrated measurements PreintegrationType::resetIntegration(); - // Set the initial bias covariance to - // the estimated covariance from the last step. - p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9); preintMeasCov_.setZero(); } From 4f9582d0ca588dfb84cbbdc152bfd1992bdc8f13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Jan 2023 11:14:51 -0500 Subject: [PATCH 10/13] update test --- gtsam/navigation/tests/testCombinedImuFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fcda1da11..339564a2b 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -348,7 +348,7 @@ TEST(CombinedImuFactor, ResetIntegration) { // Test default method pim.resetIntegration(); - Matrix6 expected = 0.001 * I_6x6; + Matrix6 expected = 0.1 * I_6x6; EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9)); // Test method where Q_init is provided From 2e1c51478cd83aa7f2c176f71a9b68adc88a5307 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Mar 2023 15:40:11 -0500 Subject: [PATCH 11/13] fix docstring --- gtsam/navigation/CombinedImuFactor.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 71169ab2c..42e3db34e 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -131,8 +131,7 @@ public: * @brief Re-initialize PreintegratedCombinedMeasurements with initial bias * covariance estimate. * - * @param Q_init The initial bias covariance estimates as 6x6 matrix. If not - * provided, it uses the last values from the preintMeasCov. + * @param Q_init The initial bias covariance estimates as a 6x6 matrix. */ void resetIntegration(const gtsam::Matrix6& Q_init); From e4b1505e38c887fb6c63c30b4a800a2eacc33511 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Mar 2023 15:40:31 -0500 Subject: [PATCH 12/13] fix ImuFactor examples to show correct prediction values --- python/gtsam/examples/CombinedImuFactorExample.py | 13 +++++++++---- python/gtsam/examples/ImuFactorExample.py | 11 ++++++++--- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/CombinedImuFactorExample.py b/python/gtsam/examples/CombinedImuFactorExample.py index 82714dd7c..e3f8c6b34 100644 --- a/python/gtsam/examples/CombinedImuFactorExample.py +++ b/python/gtsam/examples/CombinedImuFactorExample.py @@ -5,7 +5,7 @@ All Rights Reserved See LICENSE for the license information -A script validating and demonstrating the CobiendImuFactor inference. +A script validating and demonstrating inference with the CombinedImuFactor. Author: Varun Agrawal """ @@ -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 + GRAVITY = 9.81 np.set_printoptions(precision=3, suppress=True) @@ -192,7 +192,9 @@ class CombinedImuFactorExample(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() @@ -204,6 +206,9 @@ class CombinedImuFactorExample(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) 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) From cc9a2dc404e57ec51f919b12c7375d8c85dd73e9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Mar 2023 15:51:11 -0500 Subject: [PATCH 13/13] convert from boost to std shared pointers --- gtsam/navigation/PreintegrationCombinedParams.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h index 2c99463bc..30441ec36 100644 --- a/gtsam/navigation/PreintegrationCombinedParams.h +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -59,17 +59,17 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { // Default Params for a Z-down navigation frame, such as NED: gravity points // along positive Z-axis - static boost::shared_ptr MakeSharedD( + static std::shared_ptr MakeSharedD( double g = 9.81) { - return boost::shared_ptr( + 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 boost::shared_ptr MakeSharedU( + static std::shared_ptr MakeSharedU( double g = 9.81) { - return boost::shared_ptr( + return std::shared_ptr( new PreintegrationCombinedParams(Vector3(0, 0, -g))); } @@ -86,6 +86,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -96,6 +97,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); } +#endif public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW