From 653a41bc5a7950fa1ca19db5683dbf96f0fe1406 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 27 Jan 2016 15:15:55 -0800 Subject: [PATCH] Compare prediction with actual navState in two scenarios --- python/gtsam_examples/ImuFactorExample.py | 40 +++++++++++++------ .../gtsam_examples/PreintegrationExample.py | 36 +++++++++-------- python/handwritten/navigation/ImuFactor.cpp | 7 ++++ 3 files changed, 54 insertions(+), 29 deletions(-) diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index c018dc7a7..4f9d29ddd 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -20,21 +20,23 @@ X = lambda i: int(gtsam.Symbol('x', i)) 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) + def run(self): graph = gtsam.NonlinearFactorGraph() - for i in [0, 12]: - priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) - graph.push_back(gtsam.PriorFactorPose3(X(i), gtsam.Pose3(), priorNoise)) - velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - graph.push_back(gtsam.PriorFactorVector3(V(i), np.array([2, 0, 0]), velNoise)) - + i = 0 # state index # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) # simulate the loop - T = self.timeForOneLoop + T = 3 + state = self.scenario.navState(0) for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) @@ -50,25 +52,37 @@ 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() + print(pim) + predicted = pim.predict(state, self.actualBias, H1, H2) pim.resetIntegration() + state = self.scenario.navState(t + self.dt) + print("predicted.{}\nstate.{}".format(predicted, state)) i += 1 - graph.print() + # 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)) + +# graph.print("\Graph:\n") initial = gtsam.Values() - initial.insert(BIAS_KEY, gtsam.ConstantBias()) + initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): - initial.insert(X(i), gtsam.Pose3()) - initial.insert(V(i), np.zeros(3, np.float)) + initial.insert(X(i), self.scenario.pose(float(i))) + initial.insert(V(i), self.velocity) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() - # result.print("\Result:\n") - print(self.actualBias) +# result.print("\Result:\n") # Plot cameras i = 0 diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py index 1cf96b9cd..808063a94 100644 --- a/python/gtsam_examples/PreintegrationExample.py +++ b/python/gtsam_examples/PreintegrationExample.py @@ -27,23 +27,25 @@ class PreintegrationExample(object): params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) return params - def __init__(self): + def __init__(self, twist=None): + """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" + # setup interactive plotting plt.ion() - # Setup loop scenario - # Forward velocity 2m/s - # Pitch up with angular velocity 6 degree/sec (negative in FLU) - v = 2 - w = math.radians(30) - W = np.array([0, -w, 0]) - V = np.array([v, 0, 0]) + # Setup loop as default scenario + if twist is not None: + (W, V) = twist + else: + # default = loop with forward velocity 2m/s, while pitching up + # with angular velocity 30 degree/sec (negative in FLU) + W = np.array([0, -math.radians(30), 0]) + V = np.array([2, 0, 0]) + self.scenario = gtsam.ConstantTwistScenario(W, V) self.dt = 1e-2 - # Calculate time to do 1 loop - self.radius = v / w - self.timeForOneLoop = 2.0 * math.pi / w + self.maxDim = 5 self.labels = list('xyz') self.colors = list('rgb') @@ -93,16 +95,18 @@ class PreintegrationExample(object): # plot ground truth pose, as well as prediction from integrated IMU measurements actualPose = self.scenario.pose(t) plotPose3(POSES_FIG, actualPose, 0.3) + t = actualPose.translation() + self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) ax = plt.gca() - ax.set_xlim3d(-self.radius, self.radius) - ax.set_ylim3d(-self.radius, self.radius) - ax.set_zlim3d(0, self.radius * 2) + ax.set_xlim3d(-self.maxDim, self.maxDim) + ax.set_ylim3d(-self.maxDim, self.maxDim) + ax.set_zlim3d(-self.maxDim, self.maxDim) plt.pause(0.01) def run(self): - # simulate the loop up to the top - T = self.timeForOneLoop + # 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) diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index d22c93dd9..b3d6126bd 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -24,7 +24,13 @@ using namespace boost::python; using namespace gtsam; +typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9; +typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96; + void exportImuFactor() { + class_("OptionalJacobian9", init<>()); + class_("OptionalJacobian96", init<>()); + class_("NavState", init<>()) // TODO(frank): overload with jacobians // .def("attitude", &NavState::attitude) @@ -61,6 +67,7 @@ void exportImuFactor() { init&, const imuBias::ConstantBias&>()) .def(repr(self)) + .def("predict", &PreintegratedImuMeasurements::predict) .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement)