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

improvements to ImuFactorExample

release/4.3a0
Varun Agrawal 2020-06-29 16:33:28 -05:00
parent 89bdebf5ca
commit 6fdcddbaa7
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample):
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
num_poses = T + 1 # assumes 1 factor per second
num_poses = T # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
@ -91,7 +91,7 @@ class ImuFactorExample(PreintegrationExample):
if (k+1) % int(1 / self.dt) == 0:
# Plot every second
self.plotGroundTruthPose(t)
self.plotGroundTruthPose(t, scale=1)
plt.title("Ground Truth Trajectory")
# create IMU factor every second
@ -107,7 +107,7 @@ class ImuFactorExample(PreintegrationExample):
pim.resetIntegration()
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
translationNoise = gtsam.Point3(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)
@ -132,6 +132,7 @@ class ImuFactorExample(PreintegrationExample):
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
initial.print_("Initial values:")
result.print_("")
if compute_covariances:
@ -148,7 +149,7 @@ class ImuFactorExample(PreintegrationExample):
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG+1, pose_i, 0.1)
plot_pose3(POSES_FIG+1, pose_i, 1)
i += 1
plt.title("Estimated Trajectory")

9
cython/gtsam/examples/PreintegrationExample.py
View File

@ -109,10 +109,10 @@ class PreintegrationExample(object):
ax.scatter(t, measuredAcc[i], color=color, marker='.')
ax.set_xlabel('specific force ' + label)
def plotGroundTruthPose(self, t):
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
# plot ground truth pose, as well as prediction from integrated IMU measurements
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, 0.3)
plot_pose3(POSES_FIG, actualPose, scale)
t = actualPose.translation()
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
ax = plt.gca()
@ -120,11 +120,10 @@ class PreintegrationExample(object):
ax.set_ylim3d(-self.maxDim, self.maxDim)
ax.set_zlim3d(-self.maxDim, self.maxDim)
plt.pause(0.01)
plt.pause(time_interval)
def run(self):
def run(self, T=12):
# simulate the loop
T = 12
for i, t in enumerate(np.arange(0, T, self.dt)):
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)