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

reduce for loops, add titles to plots, better space subplots for IMU data

release/4.3a0
Varun Agrawal 2020-06-16 15:09:11 -05:00
parent d3591dac0b
commit 3db8862480
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,32 +69,19 @@ class ImuFactorExample(PreintegrationExample):
num_poses = T + 1 # assumes 1 factor per second num_poses = T + 1 # assumes 1 factor per second
initial = gtsam.Values() initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias) initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
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
actual_state_i = self.scenario.navState(0) actual_state_i = self.scenario.navState(0)
initial.insert(X(i), actual_state_i.pose())
initial.insert(V(i), actual_state_i.velocity())
for k, t in enumerate(np.arange(0, T, self.dt)): for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM # get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t) measuredOmega = self.runner.measuredAngularVelocity(t)
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)
@ -102,6 +89,7 @@ class ImuFactorExample(PreintegrationExample):
# Plot every second # Plot every second
if k % int(1 / self.dt) == 0: if k % int(1 / self.dt) == 0:
self.plotGroundTruthPose(t) self.plotGroundTruthPose(t)
plt.title("Ground Truth Trajectory")
# create IMU factor every second # create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0: if (k + 1) % int(1 / self.dt) == 0:
@ -112,6 +100,17 @@ class ImuFactorExample(PreintegrationExample):
print(factor) print(factor)
print(pim.predict(actual_state_i, self.actualBias)) print(pim.predict(actual_state_i, self.actualBias))
pim.resetIntegration() pim.resetIntegration()
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
translationNoise = gtsam.Point3(np.random.randn(3)*0.1)
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
actual_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
initial.insert(X(i+1), actual_state_i.pose())
initial.insert(V(i+1), actual_state_i.velocity())
actual_state_i = self.scenario.navState(t + self.dt) actual_state_i = self.scenario.navState(t + self.dt)
i += 1 i += 1
@ -138,10 +137,11 @@ class ImuFactorExample(PreintegrationExample):
i = 0 i = 0
while result.exists(X(i)): while result.exists(X(i)):
pose_i = result.atPose3(X(i)) pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1) plot_pose3(POSES_FIG+1, pose_i, 0.1)
i += 1 i += 1
plt.title("Estimated Trajectory")
gtsam.utils.plot.set_axes_equal(POSES_FIG) gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
print(result.atimuBias_ConstantBias(BIAS_KEY)) print(result.atimuBias_ConstantBias(BIAS_KEY))