12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Works with bias on all 6 axes !

release/4.3a0
dellaert 2016-01-28 00:58:31 -08:00
parent dbe2fe59a3
commit 1e1c0dbff1
2 changed files with 22 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
python/gtsam_examples/ImuFactorExample.py
View File

@ -22,12 +22,18 @@ class ImuFactorExample(PreintegrationExample):
def __init__(self): def __init__(self):
self.velocity = np.array([2, 0, 0]) 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.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 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): def addPrior(self, i, graph):
state = self.scenario.navState(i) state = self.scenario.navState(i)
graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
@ -62,11 +68,6 @@ class ImuFactorExample(PreintegrationExample):
if (k + 1) % 100 == 0: if (k + 1) % 100 == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim)
graph.push_back(factor) 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() pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt) actual_state_i = self.scenario.navState(t + self.dt)
i += 1 i += 1
@ -76,33 +77,26 @@ class ImuFactorExample(PreintegrationExample):
self.addPrior(0, graph) self.addPrior(0, graph)
self.addPrior(num_poses - 1, graph) self.addPrior(num_poses - 1, graph)
# graph.print("\Graph:\n")
initial = gtsam.Values() initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias) initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses): for i in range(num_poses):
state_i = self.scenario.navState(float(i)) state_i = self.scenario.navState(float(i))
plotPose3(POSES_FIG, state_i.pose(), 0.9)
initial.insert(X(i), state_i.pose()) initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity()) 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 # optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams() params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY") params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize() result = optimizer.optimize()
result.print("\Result:\n")
# Plot cameras # Plot resulting poses
i = 0 i = 0
while result.exists(X(i)): 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) plotPose3(POSES_FIG, pose_i, 0.1)
i += 1 i += 1
print(result.atConstantBias(BIAS_KEY))
plt.ioff() plt.ioff()
plt.show() plt.show()

13
python/gtsam_examples/PreintegrationExample.py
View File

@ -27,7 +27,7 @@ class PreintegrationExample(object):
params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float)
return params return params
def __init__(self, twist=None): def __init__(self, twist=None, bias=None):
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
# setup interactive plotting # setup interactive plotting
@ -53,9 +53,14 @@ class PreintegrationExample(object):
self.g = 10 # simple gravity constant self.g = 10 # simple gravity constant
self.params = self.defaultParams(self.g) self.params = self.defaultParams(self.g)
ptr = gtsam.ScenarioPointer(self.scenario) ptr = gtsam.ScenarioPointer(self.scenario)
accBias = np.array([0, 0.1, 0])
gyroBias = np.array([0, 0, 0]) if bias is not None:
self.actualBias = gtsam.ConstantBias(accBias, gyroBias) 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) self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias)
def plotImu(self, t, measuredOmega, measuredAcc): def plotImu(self, t, measuredOmega, measuredAcc):