12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #346 from borglab/feature/improved-imu-example-python 

Improved Python IMU example
release/4.3a0
Varun Agrawal 2020-06-10 11:52:51 -05:00 committed by GitHub
parent 08487f7a43 4fbaa43e92
commit 0b5a1b8003
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cython/gtsam/examples/ImuFactorExample.py
View File

@ -5,7 +5,9 @@ All Rights Reserved
See LICENSE for the license information 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 from __future__ import print_function
@ -76,8 +78,14 @@ class ImuFactorExample(PreintegrationExample):
initial.insert(BIAS_KEY, self.actualBias) initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses): for i in range(num_poses):
state_i = self.scenario.navState(float(i)) 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 # simulate the loop
i = 0 # state index i = 0 # state index
@ -88,6 +96,12 @@ class ImuFactorExample(PreintegrationExample):
measuredAcc = self.runner.measuredSpecificForce(t) measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) 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 # Plot IMU many times
if k % 10 == 0: if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc) self.plotImu(t, measuredOmega, measuredAcc)
@ -133,6 +147,9 @@ class ImuFactorExample(PreintegrationExample):
pose_i = result.atPose3(X(i)) pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1) plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1 i += 1
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atimuBias_ConstantBias(BIAS_KEY)) print(result.atimuBias_ConstantBias(BIAS_KEY))
plt.ioff() plt.ioff()

4
gtsam.h
View File

@ -2913,6 +2913,8 @@ class PreintegratedImuMeasurements {
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const; Matrix preintMeasCov() const;
double deltaTij() const; double deltaTij() const;
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
@ -2972,6 +2974,8 @@ class PreintegratedCombinedMeasurements {
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const; Matrix preintMeasCov() const;
double deltaTij() const; double deltaTij() const;
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;