diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index c1181d980..ca5e524ee 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -22,12 +22,18 @@ class ImuFactorExample(PreintegrationExample): def __init__(self): self.velocity = np.array([2, 0, 0]) - 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) - + + forward_twist = (np.zeros(3), self.velocity) + loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) + + accBias = np.array([-0.3, 0.1, 0.2]) + gyroBias = np.array([0.1, 0.3, -0.1]) + bias = gtsam.ConstantBias(accBias, gyroBias) + + super(ImuFactorExample, self).__init__(loop_twist, bias) + def addPrior(self, i, graph): state = self.scenario.navState(i) graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) @@ -62,11 +68,6 @@ class ImuFactorExample(PreintegrationExample): 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) - H1 = gtsam.OptionalJacobian9() - H2 = gtsam.OptionalJacobian96() - predicted_state_j = pim.predict(actual_state_i, self.actualBias, H1, H2) - error = pim.computeError(actual_state_i, predicted_state_j, self.actualBias, H1, H1, H2) - print("error={}, norm ={}".format(error, np.linalg.norm(error))) pim.resetIntegration() actual_state_i = self.scenario.navState(t + self.dt) i += 1 @@ -76,33 +77,26 @@ class ImuFactorExample(PreintegrationExample): self.addPrior(0, graph) self.addPrior(num_poses - 1, graph) -# graph.print("\Graph:\n") - initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): state_i = self.scenario.navState(float(i)) - plotPose3(POSES_FIG, state_i.pose(), 0.9) initial.insert(X(i), state_i.pose()) initial.insert(V(i), state_i.velocity()) - for idx in range(num_poses - 1): - ff = gtsam.getNonlinearFactor(graph, idx) - print(ff.error(initial)) - # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() - result.print("\Result:\n") - # Plot cameras + # Plot resulting poses i = 0 while result.exists(X(i)): - pose_i = result.pose3_at(X(i)) + pose_i = result.atPose3(X(i)) plotPose3(POSES_FIG, pose_i, 0.1) i += 1 + print(result.atConstantBias(BIAS_KEY)) plt.ioff() plt.show() diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py index 808063a94..7095dc59e 100644 --- a/python/gtsam_examples/PreintegrationExample.py +++ b/python/gtsam_examples/PreintegrationExample.py @@ -27,7 +27,7 @@ class PreintegrationExample(object): params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) return params - def __init__(self, twist=None): + def __init__(self, twist=None, bias=None): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -53,9 +53,14 @@ class PreintegrationExample(object): self.g = 10 # simple gravity constant self.params = self.defaultParams(self.g) ptr = gtsam.ScenarioPointer(self.scenario) - accBias = np.array([0, 0.1, 0]) - gyroBias = np.array([0, 0, 0]) - self.actualBias = gtsam.ConstantBias(accBias, gyroBias) + + if bias is not None: + self.actualBias = bias + else: + accBias = np.array([0, 0.1, 0]) + gyroBias = np.array([0, 0, 0]) + self.actualBias = gtsam.ConstantBias(accBias, gyroBias) + self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias) def plotImu(self, t, measuredOmega, measuredAcc):