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

Fully working !

release/4.3a0
dellaert 2016-01-28 00:29:18 -08:00
parent c49a97a9c6
commit 85e231bea5
1 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
python/gtsam_examples/ImuFactorExample.py
View File

@ -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")