From ebd4db2380ce9d9c14e8ce3ccbbc08e37657fcc2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jul 2020 13:03:17 -0500 Subject: [PATCH] small improvements to the ImuFactorExample.py --- cython/gtsam/examples/ImuFactorExample.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 289a0ccec..06742fcd1 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -79,6 +79,9 @@ class ImuFactorExample(PreintegrationExample): initial.insert(X(i), initial_state_i.pose()) 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)): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) @@ -121,19 +124,18 @@ class ImuFactorExample(PreintegrationExample): initial.insert(V(i+1), noisy_state_i.velocity()) i += 1 - # add prior on beginning - self.addPrior(0, graph) # add prior on end # self.addPrior(num_poses - 1, graph) + initial.print_("Initial values:") + # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() - initial.print_("Initial values:") - result.print_("") + result.print_("Optimized values:") if compute_covariances: # Calculate and print marginal covariances