From b11b184f228f8c629d97bf936503d2dde5cc4a82 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Jun 2020 12:14:44 -0500 Subject: [PATCH 1/3] add resetIntegrationAndSetBias to wrapper --- gtsam.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam.h b/gtsam.h index 94e8baee4..092355875 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2913,6 +2913,8 @@ class PreintegratedImuMeasurements { void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + Matrix preintMeasCov() const; double deltaTij() const; gtsam::Rot3 deltaRij() const; @@ -2972,6 +2974,8 @@ class PreintegratedCombinedMeasurements { void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + Matrix preintMeasCov() const; double deltaTij() const; gtsam::Rot3 deltaRij() const; From 744727707ebeff9c017a0f2fa8f4240a2884b607 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Jun 2020 12:15:08 -0500 Subject: [PATCH 2/3] add guassian noise to initial estimates in Imu example --- cython/gtsam/examples/ImuFactorExample.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 6cc8979f1..ca38e9173 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -76,8 +76,14 @@ class ImuFactorExample(PreintegrationExample): initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): state_i = self.scenario.navState(float(i)) - initial.insert(X(i), state_i.pose()) - initial.insert(V(i), state_i.velocity()) + + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + pose = state_i.pose().compose(poseNoise) + + velocity = state_i.velocity() + np.random.randn(3)*0.1 + + initial.insert(X(i), pose) + initial.insert(V(i), velocity) # simulate the loop i = 0 # state index @@ -88,6 +94,12 @@ class ImuFactorExample(PreintegrationExample): measuredAcc = self.runner.measuredSpecificForce(t) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + + actual_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3)*0.1) + # Plot IMU many times if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) @@ -133,6 +145,9 @@ class ImuFactorExample(PreintegrationExample): pose_i = result.atPose3(X(i)) plot_pose3(POSES_FIG, pose_i, 0.1) i += 1 + + gtsam.utils.plot.set_axes_equal(POSES_FIG) + print(result.atimuBias_ConstantBias(BIAS_KEY)) plt.ioff() From 4fbaa43e92a3605143582386dea6ae799f6c3ebb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Jun 2020 17:51:29 -0500 Subject: [PATCH 3/3] docstring update --- cython/gtsam/examples/ImuFactorExample.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index ca38e9173..d7be83bab 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -5,7 +5,9 @@ All Rights Reserved See LICENSE for the license information -A script validating the ImuFactor inference. +A script validating and demonstrating the ImuFactor inference. + +Author: Frank Dellaert, Varun Agrawal """ from __future__ import print_function