Skip to content

Commit

Permalink
Merge pull request #869 from borglab/fix/imu-factor-example
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Sep 6, 2021
2 parents e320bfa + 617014f commit e5bad52
Show file tree
Hide file tree
Showing 4 changed files with 84 additions and 60 deletions.
18 changes: 9 additions & 9 deletions gtsam/linear/linear.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,9 @@ virtual class Base {
};

virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* Information(Matrix R);
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true);
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true);

bool equals(gtsam::noiseModel::Base& expected, double tol);

Expand All @@ -37,9 +37,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
};

virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true);
static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true);
Matrix R() const;

// access to noise model
Expand Down Expand Up @@ -69,9 +69,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
};

virtual class Isotropic : gtsam::noiseModel::Diagonal {
static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma, bool smart = true);
static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace, bool smart = true);
static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision, bool smart = true);

// access to noise model
double sigma() const;
Expand Down
1 change: 0 additions & 1 deletion gtsam/navigation/tests/testImuFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -819,7 +819,6 @@ struct ImuFactorMergeTest {
loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
// arbitrary noise values
p_->gyroscopeCovariance = I_3x3 * 0.01;
p_->accelerometerCovariance = I_3x3 * 0.02;
p_->accelerometerCovariance = I_3x3 * 0.03;
}

Expand Down
114 changes: 66 additions & 48 deletions python/gtsam/examples/ImuFactorExample.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,28 +10,30 @@
Author: Frank Dellaert, Varun Agrawal
"""

# pylint: disable=no-name-in-module,unused-import,arguments-differ

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

BIAS_KEY = B(0)


np.set_printoptions(precision=3, suppress=True)


class ImuFactorExample(PreintegrationExample):

"""Class to run example of the Imu Factor."""
def __init__(self, twist_scenario="sick_twist"):
self.velocity = np.array([2, 0, 0])
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
Expand All @@ -42,9 +44,8 @@ def __init__(self, twist_scenario="sick_twist"):
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)
)
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])
Expand All @@ -55,19 +56,44 @@ def __init__(self, twist_scenario="sick_twist"):
bias, dt)

def addPrior(self, i, graph):
"""Add priors at time step `i`."""
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))
graph.push_back(
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
graph.push_back(
gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))

def optimize(self, graph, initial):
"""Optimize using Levenberg-Marquardt optimization."""
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
return result

def plot(self, result):
"""Plot resulting poses."""
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG + 1, pose_i, 1)
i += 1
plt.title("Estimated Trajectory")

gtsam.utils.plot.set_axes_equal(POSES_FIG + 1)

print("Bias Values", result.atConstantBias(BIAS_KEY))

plt.ioff()
plt.show()

def run(self, T=12, compute_covariances=False, verbose=True):
"""Main runner."""
graph = gtsam.NonlinearFactorGraph()

# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)

T = 12
num_poses = T # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
Expand All @@ -91,14 +117,13 @@ def run(self, T=12, compute_covariances=False, verbose=True):
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)

if (k+1) % int(1 / self.dt) == 0:
if (k + 1) % int(1 / self.dt) == 0:
# Plot every second
self.plotGroundTruthPose(t, scale=1)
plt.title("Ground Truth Trajectory")

# create IMU factor every second
factor = gtsam.ImuFactor(X(i), V(i),
X(i + 1), V(i + 1),
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)

Expand All @@ -108,34 +133,34 @@ def run(self, T=12, compute_covariances=False, verbose=True):

pim.resetIntegration()

rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
translationNoise = gtsam.Point3(*np.random.randn(3)*1)
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)

actual_state_i = self.scenario.navState(t + self.dt)
print("Actual state at {0}:\n{1}".format(
t+self.dt, actual_state_i))
t + self.dt, actual_state_i))

noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
actual_state_i.velocity() + np.random.randn(3) * 0.1)

initial.insert(X(i+1), noisy_state_i.pose())
initial.insert(V(i+1), noisy_state_i.velocity())
initial.insert(X(i + 1), noisy_state_i.pose())
initial.insert(V(i + 1), noisy_state_i.velocity())
i += 1

# add priors on end
self.addPrior(num_poses - 1, graph)

initial.print_("Initial values:")
initial.print("Initial values:")

# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
result = self.optimize(graph, initial)

result.print_("Optimized values:")
result.print("Optimized values:")
print("------------------")
print(graph.error(initial))
print(graph.error(result))
print("------------------")

if compute_covariances:
# Calculate and print marginal covariances
Expand All @@ -148,33 +173,26 @@ def run(self, T=12, compute_covariances=False, verbose=True):
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+1, pose_i, 1)
i += 1
plt.title("Estimated Trajectory")

gtsam.utils.plot.set_axes_equal(POSES_FIG+1)

print("Bias Values", result.atConstantBias(BIAS_KEY))

plt.ioff()
plt.show()
self.plot(result)


if __name__ == '__main__':
parser = argparse.ArgumentParser("ImuFactorExample.py")
parser.add_argument("--twist_scenario",
default="sick_twist",
choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist"))
parser.add_argument("--time", "-T", default=12,
type=int, help="Total time in seconds")
choices=("zero_twist", "forward_twist", "loop_twist",
"sick_twist"))
parser.add_argument("--time",
"-T",
default=12,
type=int,
help="Total time in seconds")
parser.add_argument("--compute_covariances",
default=False, action='store_true')
default=False,
action='store_true')
parser.add_argument("--verbose", default=False, action='store_true')
args = parser.parse_args()

ImuFactorExample(args.twist_scenario).run(
args.time, args.compute_covariances, args.verbose)
ImuFactorExample(args.twist_scenario).run(args.time,
args.compute_covariances,
args.verbose)
11 changes: 9 additions & 2 deletions python/gtsam/utils/plot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,10 @@ def set_axes_equal(fignum: int) -> None:
fignum: An integer representing the figure number for Matplotlib.
"""
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
if not fig.axes:
ax = fig.add_subplot(projection='3d')
else:
ax = fig.axes[0]

limits = np.array([
ax.get_xlim3d(),
Expand Down Expand Up @@ -339,7 +342,11 @@ def plot_pose3(
"""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
if not fig.axes:
axes = fig.add_subplot(projection='3d')
else:
axes = fig.axes[0]

plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)

axes.set_xlabel(axis_labels[0])
Expand Down

0 comments on commit e5bad52

Please sign in to comment.