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

small improvements to the ImuFactorExample.py

release/4.3a0
Varun Agrawal 2020-07-02 13:03:17 -05:00
parent 561218ae82
commit ebd4db2380
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,6 +79,9 @@ class ImuFactorExample(PreintegrationExample):
initial.insert(X(i), initial_state_i.pose()) initial.insert(X(i), initial_state_i.pose())
initial.insert(V(i), initial_state_i.velocity()) initial.insert(V(i), initial_state_i.velocity())
# add prior on beginning
self.addPrior(0, graph)
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)
@ -121,19 +124,18 @@ class ImuFactorExample(PreintegrationExample):
initial.insert(V(i+1), noisy_state_i.velocity()) initial.insert(V(i+1), noisy_state_i.velocity())
i += 1 i += 1
# add prior on beginning
self.addPrior(0, graph)
# add prior on end # add prior on end
# self.addPrior(num_poses - 1, graph) # self.addPrior(num_poses - 1, graph)
initial.print_("Initial values:")
# optimize using Levenberg-Marquardt optimization # optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams() params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY") params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize() result = optimizer.optimize()
initial.print_("Initial values:") result.print_("Optimized values:")
result.print_("")
if compute_covariances: if compute_covariances:
# Calculate and print marginal covariances # Calculate and print marginal covariances