diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index 5ba4067aa..c1181d980 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -25,6 +25,13 @@ class ImuFactorExample(PreintegrationExample): forward_twist = (np.zeros(3), self.velocity) loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) super(ImuFactorExample, self).__init__(loop_twist) + self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) + self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + + def addPrior(self, i, graph): + state = self.scenario.navState(i) + graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) + graph.push_back(gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise)) def run(self): graph = gtsam.NonlinearFactorGraph() @@ -35,7 +42,7 @@ class ImuFactorExample(PreintegrationExample): pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) # simulate the loop - T = 3 + T = 12 actual_state_i = self.scenario.navState(0) for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM @@ -43,12 +50,15 @@ class ImuFactorExample(PreintegrationExample): measuredAcc = self.runner.measuredSpecificForce(t) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + # Plot every second if k % 100 == 0: - self.plotImu(t, measuredOmega, measuredAcc) self.plotGroundTruthPose(t) - # create factor every second + # create IMU factor every second if (k + 1) % 100 == 0: factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) @@ -63,11 +73,8 @@ class ImuFactorExample(PreintegrationExample): # add priors on beginning and end num_poses = i + 1 - priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - for i, pose in [(0, self.scenario.pose(0)), (num_poses - 1, self.scenario.pose(T))]: - graph.push_back(gtsam.PriorFactorPose3(X(i), pose, priorNoise)) - graph.push_back(gtsam.PriorFactorVector3(V(i), self.velocity, velNoise)) + self.addPrior(0, graph) + self.addPrior(num_poses - 1, graph) # graph.print("\Graph:\n")