Merged in fix/458_scenario_examples (pull request #433)
close issue #458 ImuFactor python examples portedrelease/4.3a0
Frank Dellaert
commit
357e739127
|
|
@ -0,0 +1,143 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A script validating the ImuFactor inference.
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
from PreintegrationExample import POSES_FIG, PreintegrationExample
|
||||
|
||||
BIAS_KEY = int(gtsam.symbol(ord('b'), 0))
|
||||
|
||||
|
||||
def X(key):
|
||||
"""Create symbol for pose key."""
|
||||
return gtsam.symbol(ord('x'), key)
|
||||
|
||||
|
||||
def V(key):
|
||||
"""Create symbol for velocity key."""
|
||||
return gtsam.symbol(ord('v'), key)
|
||||
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
|
||||
|
||||
class ImuFactorExample(PreintegrationExample):
|
||||
|
||||
def __init__(self):
|
||||
self.velocity = np.array([2, 0, 0])
|
||||
self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
|
||||
self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
|
||||
# Choose one of these twists to change scenario:
|
||||
zero_twist = (np.zeros(3), np.zeros(3))
|
||||
forward_twist = (np.zeros(3), self.velocity)
|
||||
loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
|
||||
sick_twist = (
|
||||
np.array([math.radians(30), -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.imuBias_ConstantBias(accBias, gyroBias)
|
||||
|
||||
dt = 1e-2
|
||||
super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
|
||||
|
||||
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.PriorFactorVector(
|
||||
V(i), state.velocity(), self.velNoise))
|
||||
|
||||
def run(self):
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# initialize data structure for pre-integrated IMU measurements
|
||||
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
|
||||
|
||||
T = 12
|
||||
num_poses = T + 1 # assumes 1 factor per second
|
||||
initial = gtsam.Values()
|
||||
initial.insert(BIAS_KEY, self.actualBias)
|
||||
for i in range(num_poses):
|
||||
state_i = self.scenario.navState(float(i))
|
||||
initial.insert(X(i), state_i.pose())
|
||||
initial.insert(V(i), state_i.velocity())
|
||||
|
||||
# simulate the loop
|
||||
i = 0 # state index
|
||||
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
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
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 % int(1 / self.dt) == 0:
|
||||
self.plotGroundTruthPose(t)
|
||||
|
||||
# create IMU factor every second
|
||||
if (k + 1) % int(1 / self.dt) == 0:
|
||||
factor = gtsam.ImuFactor(X(i), V(i), X(
|
||||
i + 1), V(i + 1), BIAS_KEY, pim)
|
||||
graph.push_back(factor)
|
||||
if True:
|
||||
print(factor)
|
||||
print(pim.predict(actual_state_i, self.actualBias))
|
||||
pim.resetIntegration()
|
||||
actual_state_i = self.scenario.navState(t + self.dt)
|
||||
i += 1
|
||||
|
||||
# add priors on beginning and end
|
||||
self.addPrior(0, graph)
|
||||
self.addPrior(num_poses - 1, graph)
|
||||
|
||||
# optimize using Levenberg-Marquardt optimization
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
params.setVerbosityLM("SUMMARY")
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
|
||||
# Calculate and print marginal covariances
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
|
||||
for i in range(num_poses):
|
||||
print("Covariance on pose {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(X(i))))
|
||||
print("Covariance on vel {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(V(i))))
|
||||
|
||||
# Plot resulting poses
|
||||
i = 0
|
||||
while result.exists(X(i)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
plot_pose3(POSES_FIG, pose_i, 0.1)
|
||||
i += 1
|
||||
print(result.atimuBias_ConstantBias(BIAS_KEY))
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
ImuFactorExample().run()
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A script validating the Preintegration of IMU measurements
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
|
||||
IMU_FIG = 1
|
||||
POSES_FIG = 2
|
||||
|
||||
|
||||
class PreintegrationExample(object):
|
||||
|
||||
@staticmethod
|
||||
def defaultParams(g):
|
||||
"""Create default parameters with Z *up* and realistic noise parameters"""
|
||||
params = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||
kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
|
||||
kAccelSigma = 0.1 / 60 # 10 cm VRW
|
||||
params.setGyroscopeCovariance(
|
||||
kGyroSigma ** 2 * np.identity(3, np.float))
|
||||
params.setAccelerometerCovariance(
|
||||
kAccelSigma ** 2 * np.identity(3, np.float))
|
||||
params.setIntegrationCovariance(
|
||||
0.0000001 ** 2 * np.identity(3, np.float))
|
||||
return params
|
||||
|
||||
def __init__(self, twist=None, bias=None, dt=1e-2):
|
||||
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
|
||||
|
||||
# setup interactive plotting
|
||||
plt.ion()
|
||||
|
||||
# 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 = dt
|
||||
|
||||
self.maxDim = 5
|
||||
self.labels = list('xyz')
|
||||
self.colors = list('rgb')
|
||||
|
||||
# Create runner
|
||||
self.g = 10 # simple gravity constant
|
||||
self.params = self.defaultParams(self.g)
|
||||
|
||||
if bias is not None:
|
||||
self.actualBias = bias
|
||||
else:
|
||||
accBias = np.array([0, 0.1, 0])
|
||||
gyroBias = np.array([0, 0, 0])
|
||||
self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
|
||||
|
||||
self.runner = gtsam.ScenarioRunner(
|
||||
self.scenario, self.params, self.dt, self.actualBias)
|
||||
|
||||
def plotImu(self, t, measuredOmega, measuredAcc):
|
||||
plt.figure(IMU_FIG)
|
||||
|
||||
# plot angular velocity
|
||||
omega_b = self.scenario.omega_b(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
plt.subplot(4, 3, i + 1)
|
||||
plt.scatter(t, omega_b[i], color='k', marker='.')
|
||||
plt.scatter(t, measuredOmega[i], color=color, marker='.')
|
||||
plt.xlabel('angular velocity ' + label)
|
||||
|
||||
# plot acceleration in nav
|
||||
acceleration_n = self.scenario.acceleration_n(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
plt.subplot(4, 3, i + 4)
|
||||
plt.scatter(t, acceleration_n[i], color=color, marker='.')
|
||||
plt.xlabel('acceleration in nav ' + label)
|
||||
|
||||
# plot acceleration in body
|
||||
acceleration_b = self.scenario.acceleration_b(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
plt.subplot(4, 3, i + 7)
|
||||
plt.scatter(t, acceleration_b[i], color=color, marker='.')
|
||||
plt.xlabel('acceleration in body ' + label)
|
||||
|
||||
# plot actual specific force, as well as corrupted
|
||||
actual = self.runner.actualSpecificForce(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
plt.subplot(4, 3, i + 10)
|
||||
plt.scatter(t, actual[i], color='k', marker='.')
|
||||
plt.scatter(t, measuredAcc[i], color=color, marker='.')
|
||||
plt.xlabel('specific force ' + label)
|
||||
|
||||
def plotGroundTruthPose(self, t):
|
||||
# plot ground truth pose, as well as prediction from integrated IMU measurements
|
||||
actualPose = self.scenario.pose(t)
|
||||
plot_pose3(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.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
|
||||
T = 12
|
||||
for i, t in enumerate(np.arange(0, T, self.dt)):
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||
if i % 25 == 0:
|
||||
self.plotImu(t, measuredOmega, measuredAcc)
|
||||
self.plotGroundTruthPose(t)
|
||||
pim = self.runner.integrate(t, self.actualBias, True)
|
||||
predictedNavState = self.runner.predict(pim, self.actualBias)
|
||||
plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
PreintegrationExample().run()
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
ScenarioRunner unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import math
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestScenarioRunner(GtsamTestCase):
|
||||
def setUp(self):
|
||||
self.g = 10 # simple gravity constant
|
||||
|
||||
def test_loop_runner(self):
|
||||
# Forward velocity 2m/s
|
||||
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
v = 2
|
||||
w = math.radians(6)
|
||||
W = np.array([0, -w, 0])
|
||||
V = np.array([v, 0, 0])
|
||||
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
|
||||
dt = 0.1
|
||||
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
|
||||
bias = gtsam.imuBias_ConstantBias()
|
||||
runner = gtsam.ScenarioRunner(
|
||||
scenario, params, dt, bias)
|
||||
|
||||
# Test specific force at time 0: a is pointing up
|
||||
t = 0.0
|
||||
a = w * v
|
||||
np.testing.assert_almost_equal(
|
||||
np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
24
gtsam.h
24
gtsam.h
|
|
@ -2856,6 +2856,30 @@ virtual class AcceleratingScenario : gtsam::Scenario {
|
|||
Vector omega_b);
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/ScenarioRunner.h>
|
||||
class ScenarioRunner {
|
||||
ScenarioRunner(const gtsam::Scenario& scenario,
|
||||
const gtsam::PreintegrationParams* p,
|
||||
double imuSampleTime,
|
||||
const gtsam::imuBias::ConstantBias& bias);
|
||||
Vector gravity_n() const;
|
||||
Vector actualAngularVelocity(double t) const;
|
||||
Vector actualSpecificForce(double t) const;
|
||||
Vector measuredAngularVelocity(double t) const;
|
||||
Vector measuredSpecificForce(double t) const;
|
||||
double imuSampleTime() const;
|
||||
gtsam::PreintegratedImuMeasurements integrate(
|
||||
double T, const gtsam::imuBias::ConstantBias& estimatedBias,
|
||||
bool corrupted) const;
|
||||
gtsam::NavState predict(
|
||||
const gtsam::PreintegratedImuMeasurements& pim,
|
||||
const gtsam::imuBias::ConstantBias& estimatedBias) const;
|
||||
Matrix estimateCovariance(
|
||||
double T, size_t N,
|
||||
const gtsam::imuBias::ConstantBias& estimatedBias) const;
|
||||
Matrix estimateNoiseCovariance(size_t N) const;
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// Utilities
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -235,8 +235,8 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
|||
const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||
pose_j, vel_j, bias), _PIM_(pim) {
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
|
||||
PreintegratedImuMeasurements::Params>(pim.p());
|
||||
boost::shared_ptr<PreintegrationParams> p = boost::make_shared<
|
||||
PreintegrationParams>(pim.p());
|
||||
p->n_gravity = n_gravity;
|
||||
p->omegaCoriolis = omegaCoriolis;
|
||||
p->body_P_sensor = body_P_sensor;
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
|
|||
class ScenarioRunner {
|
||||
public:
|
||||
typedef imuBias::ConstantBias Bias;
|
||||
typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
|
||||
typedef boost::shared_ptr<PreintegrationParams> SharedParams;
|
||||
|
||||
private:
|
||||
const Scenario& scenario_;
|
||||
|
|
|
|||
|
|
@ -803,11 +803,11 @@ TEST(ImuFactor, bodyPSensorWithBias) {
|
|||
static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
|
||||
|
||||
struct ImuFactorMergeTest {
|
||||
boost::shared_ptr<PreintegratedImuMeasurements::Params> p_;
|
||||
boost::shared_ptr<PreintegrationParams> p_;
|
||||
const ConstantTwistScenario forward_, loop_;
|
||||
|
||||
ImuFactorMergeTest()
|
||||
: p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)),
|
||||
: p_(PreintegrationParams::MakeSharedU(kGravity)),
|
||||
forward_(kZero, Vector3(kVelocity, 0, 0)),
|
||||
loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
|
||||
// arbitrary noise values
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
|
|||
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
|
||||
|
||||
// Create default parameters with Z-up and above noise parameters
|
||||
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
|
||||
static boost::shared_ptr<PreintegrationParams> defaultParams() {
|
||||
auto p = PreintegrationParams::MakeSharedU(10);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
|
|
|
|||
Loading…
Reference in New Issue