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

Compare prediction with actual navState in two scenarios

release/4.3a0
Frank 2016-01-27 15:15:55 -08:00
parent 8126e6b51d
commit 653a41bc5a
3 changed files with 54 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
python/gtsam_examples/ImuFactorExample.py
View File

@ -20,13 +20,14 @@ 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
@ -34,7 +35,8 @@ class ImuFactorExample(PreintegrationExample):
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,17 +52,30 @@ 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()
@ -68,7 +83,6 @@ class ImuFactorExample(PreintegrationExample):
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
# result.print("\Result:\n")
print(self.actualBias)
# Plot cameras
i = 0

36
python/gtsam_examples/PreintegrationExample.py
View File

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

7
python/handwritten/navigation/ImuFactor.cpp
View File

@ -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>("OptionalJacobian9", init<>());
class_<OptionalJacobian96>("OptionalJacobian96", init<>());
class_<NavState>("NavState", init<>())
// TODO(frank): overload with jacobians
// .def("attitude", &NavState::attitude)
@ -61,6 +67,7 @@ void exportImuFactor() {
init<const boost::shared_ptr<PreintegrationParams>&,
const imuBias::ConstantBias&>())
.def(repr(self))
.def("predict", &PreintegratedImuMeasurements::predict)
.def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration)
.def("integrateMeasurement",
&PreintegratedImuMeasurements::integrateMeasurement)