diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index bb0424c85..289a0ccec 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -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") diff --git a/cython/gtsam/examples/PreintegrationExample.py b/cython/gtsam/examples/PreintegrationExample.py index 68b1c8cc0..958221ac9 100644 --- a/cython/gtsam/examples/PreintegrationExample.py +++ b/cython/gtsam/examples/PreintegrationExample.py @@ -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)