Compare prediction with actual navState in two scenarios
Frank
parent
8126e6b51d
commit
653a41bc5a
|
|
@ -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,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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue