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

clean up and refactoring to use custom preintegration params

release/4.3a0
Varun Agrawal 2021-09-10 11:02:14 -04:00
parent e5bad525a6
commit 19850425b8
3 changed files with 56 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/navigation.i
View File

@ -88,6 +88,8 @@ virtual class PreintegratedRotationParams {
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
gtsam::Vector n_gravity;
static gtsam::PreintegrationParams* MakeSharedD(double g);
static gtsam::PreintegrationParams* MakeSharedU(double g);
static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81

48
python/gtsam/examples/ImuFactorExample.py
View File

@ -10,20 +10,19 @@ A script validating and demonstrating the ImuFactor inference.
Author: Frank Dellaert, Varun Agrawal
"""
# pylint: disable=no-name-in-module,unused-import,arguments-differ
# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order
from __future__ import print_function
import argparse
import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.symbol_shorthand import B, V, X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
from PreintegrationExample import POSES_FIG, PreintegrationExample
@ -51,12 +50,23 @@ class ImuFactorExample(PreintegrationExample):
gyroBias = np.array([0.1, 0.3, -0.1])
bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
g = 9.81
params = gtsam.PreintegrationParams.MakeSharedU(g)
# Some arbitrary noise sigmas
gyro_sigma = 1e-3
accel_sigma = 1e-3
I_3x3 = np.eye(3)
params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
params.setIntegrationCovariance(1e-7**2 * I_3x3)
dt = 1e-2
super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
bias, dt)
bias, params, dt)
def addPrior(self, i, graph):
"""Add priors at time step `i`."""
"""Add a prior on the navigation state at time `i`."""
state = self.scenario.navState(i)
graph.push_back(
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
@ -71,21 +81,27 @@ class ImuFactorExample(PreintegrationExample):
result = optimizer.optimize()
return result
def plot(self, result):
"""Plot resulting poses."""
def plot(self,
values,
title="Estimated Trajectory",
fignum=POSES_FIG + 1,
show=False):
"""Plot poses in values."""
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG + 1, pose_i, 1)
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
plot_pose3(fignum, pose_i, 1)
i += 1
plt.title("Estimated Trajectory")
plt.title(title)
gtsam.utils.plot.set_axes_equal(POSES_FIG + 1)
gtsam.utils.plot.set_axes_equal(fignum)
print("Bias Values", result.atConstantBias(BIAS_KEY))
print("Bias Values", values.atConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
if show:
plt.show()
def run(self, T=12, compute_covariances=False, verbose=True):
"""Main runner."""
@ -173,7 +189,7 @@ class ImuFactorExample(PreintegrationExample):
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
self.plot(result)
self.plot(result, show=True)
if __name__ == '__main__':

38
python/gtsam/examples/PreintegrationExample.py
View File

@ -5,9 +5,13 @@ All Rights Reserved
See LICENSE for the license information
A script validating the Preintegration of IMU measurements
A script validating the Preintegration of IMU measurements.
Authors: Frank Dellaert, Varun Agrawal.
"""
# pylint: disable=invalid-name,unused-import,wrong-import-order
import math
import gtsam
@ -21,22 +25,20 @@ POSES_FIG = 2
class PreintegrationExample(object):
"""Base class for all preintegration examples."""
@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, float))
params.setAccelerometerCovariance(
kAccelSigma ** 2 * np.identity(3, float))
params.setIntegrationCovariance(
0.0000001 ** 2 * np.identity(3, float))
params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float))
params.setAccelerometerCovariance(kAccelSigma**2 *
np.identity(3, float))
params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float))
return params
def __init__(self, twist=None, bias=None, dt=1e-2):
def __init__(self, twist=None, bias=None, params=None, dt=1e-2):
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
# setup interactive plotting
@ -58,9 +60,11 @@ class PreintegrationExample(object):
self.labels = list('xyz')
self.colors = list('rgb')
# Create runner
self.g = 10 # simple gravity constant
self.params = self.defaultParams(self.g)
if params:
self.params = params
else:
# Default params with simple gravity constant
self.params = self.defaultParams(g=10)
if bias is not None:
self.actualBias = bias
@ -69,13 +73,15 @@ class PreintegrationExample(object):
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)
# Create runner
self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt,
self.actualBias)
fig, self.axes = plt.subplots(4, 3)
fig.set_tight_layout(True)
def plotImu(self, t, measuredOmega, measuredAcc):
"""Plot IMU measurements."""
plt.figure(IMU_FIG)
# plot angular velocity
@ -109,7 +115,7 @@ class PreintegrationExample(object):
ax.set_xlabel('specific force ' + label)
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
# plot ground truth pose, as well as prediction from integrated IMU measurements
"""Plot ground truth pose, as well as prediction from integrated IMU measurements."""
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, scale)
t = actualPose.translation()
@ -122,7 +128,7 @@ class PreintegrationExample(object):
plt.pause(time_interval)
def run(self, T=12):
# simulate the loop
"""Simulate the loop."""
for i, t in enumerate(np.arange(0, T, self.dt)):
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)