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

Sync ImuFactorExample

release/4.3a0
Fan Jiang 2020-07-27 10:56:09 -04:00
parent 9216934ca8
commit 2bda74950a
2 changed files with 197 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
python/gtsam_py/python/examples/ImuFactorExample.py
View File

@ -5,32 +5,29 @@ All Rights Reserved
See LICENSE for the license information
A script validating the ImuFactor inference.
A script validating and demonstrating the ImuFactor inference.
Author: Frank Dellaert, Varun Agrawal
"""
from __future__ import print_function
import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand
B = symbol_shorthand.B
V = symbol_shorthand.V
X = symbol_shorthand.X
from gtsam.utils.plot import plot_pose3
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('b', 0))
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol('x', key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol('v', key)
BIAS_KEY = B(0)
np.set_printoptions(precision=3, suppress=True)
@ -76,8 +73,14 @@ class ImuFactorExample(PreintegrationExample):
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())
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
pose = state_i.pose().compose(poseNoise)
velocity = state_i.velocity() + np.random.randn(3)*0.1
initial.insert(X(i), pose)
initial.insert(V(i), velocity)
# simulate the loop
i = 0 # state index
@ -88,6 +91,12 @@ class ImuFactorExample(PreintegrationExample):
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
actual_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
@ -133,7 +142,10 @@ class ImuFactorExample(PreintegrationExample):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1
print(result.atimuBias.ConstantBias(BIAS_KEY))
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atConstantBias(BIAS_KEY))
plt.ioff()
plt.show()

200
python/gtsam_py/python/utils/plot.py
View File

@ -13,8 +13,9 @@ def set_axes_equal(fignum):
Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
Args:
fignum (int): An integer representing the figure number for Matplotlib.
"""
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
@ -34,7 +35,21 @@ def set_axes_equal(fignum):
def ellipsoid(xc, yc, zc, rx, ry, rz, n):
"""Numpy equivalent of Matlab's ellipsoid function"""
"""
Numpy equivalent of Matlab's ellipsoid function.
Args:
xc (double): Center of ellipsoid in X-axis.
yc (double): Center of ellipsoid in Y-axis.
zc (double): Center of ellipsoid in Z-axis.
rx (double): Radius of ellipsoid in X-axis.
ry (double): Radius of ellipsoid in Y-axis.
rz (double): Radius of ellipsoid in Z-axis.
n (int): The granularity of the ellipsoid plotted.
Returns:
tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
"""
u = np.linspace(0, 2*np.pi, n+1)
v = np.linspace(0, np.pi, n+1)
x = -rx * np.outer(np.cos(u), np.sin(v)).T
@ -47,9 +62,18 @@ def ellipsoid(xc, yc, zc, rx, ry, rz, n):
def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
"""
Plots a Gaussian as an uncertainty ellipse
Based on Maybeck Vol 1, page 366
k=2.296 corresponds to 1 std, 68.26% of all probability
k=11.82 corresponds to 3 std, 99.74% of all probability
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
origin (gtsam.Point3): The origin in the world frame.
P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
scale (float): Scaling factor of the radii of the covariance ellipse.
n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
alpha (float): Transparency value for the plotted surface in the range [0, 1].
"""
k = 11.82
U, S, _ = np.linalg.svd(P)
@ -73,10 +97,19 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
"""Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
"""
Plot a 2D pose on given axis `axes` with given `axis_length`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
origin = pose.translation()
t = pose.translation()
origin = np.array([t.x(), t.y()])
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
@ -101,34 +134,89 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
np.rad2deg(angle), fill=False)
axes.add_patch(e1)
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
"""Plot a 2D pose on given figure with given 'axis_length'."""
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 2D pose on given figure with given `axis_length`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
"""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca()
plot_pose2_on_axes(axes, pose, axis_length, covariance)
plot_pose2_on_axes(axes, pose, axis_length=axis_length,
covariance=covariance)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig
def plot_point3_on_axes(axes, point, linespec, P=None):
"""Plot a 3D point on given axis 'axes' with given 'linespec' and optional covariance 'P'."""
axes.plot([point[0]], [point[1]], [point[2]], linespec)
"""
Plot a 3D point on given axis `axes` with given `linespec`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
axes.plot([point.x()], [point.y()], [point.z()], linespec)
if P is not None:
plot_covariance_ellipse_3d(axes, point.vector(), P)
plot_covariance_ellipse_3d(axes, point, P)
def plot_point3(fignum, point, linespec, P=None):
"""Plot a 3D point on given figure with given 'linespec' and optional covariance 'P'."""
def plot_point3(fignum, point, linespec, P=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 3D point on given figure with given `linespec`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
Returns:
fig: The matplotlib figure.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_point3_on_axes(axes, point, linespec, P)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
def plot_3d_points(fignum, values, linespec="g*", marginals=None):
return fig
def plot_3d_points(fignum, values, linespec="g*", marginals=None,
title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plots the Point3s in 'values', with optional covariances.
Plots the Point3s in `values`, with optional covariances.
Finds all the Point3 objects in the given Values object and plots them.
If a Marginals object is given, this function will also plot marginal
covariance ellipses for each point.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dictionary consisting of points to be plotted.
linespec (string): String representing formatting options for Matplotlib.
marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
keys = values.keys()
@ -139,21 +227,30 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
key = keys.at(i)
point = values.atPoint3(key)
if marginals is not None:
P = marginals.marginalCovariance(key);
covariance = marginals.marginalCovariance(key)
else:
P = None
covariance = None
plot_point3(fignum, point, linespec, P)
fig = plot_point3(fignum, point, linespec, covariance,
axis_labels=axis_labels)
except RuntimeError:
continue
# I guess it's not a Point3
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
"""
Plot a 3D pose on given axis 'axes' with given 'axis_length'.
Optional 'scale' the pose and plot the covariance 'P'.
Plot a 3D pose on given axis `axes` with given `axis_length`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
@ -172,6 +269,7 @@ def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
# plot the covariance
if P is not None:
# covariance matrix in pose coordinate frame
pPp = P[3:6, 3:6]
@ -180,16 +278,49 @@ def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
plot_covariance_ellipse_3d(axes, origin, gPp)
def plot_pose3(fignum, pose, P, axis_length=0.1):
"""Plot a 3D pose on given figure with given 'axis_length'."""
def plot_pose3(fignum, pose, axis_length=0.1, P=None,
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a 3D pose on given figure with given `axis_length`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
pose (gtsam.Pose3): 3D pose to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
axis_labels (iterable[string]): List of axis labels to set.
Returns:
fig: The matplotlib figure.
"""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)
plot_pose3_on_axes(axes, pose, P=P,
axis_length=axis_length)
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig
def plot_trajectory(fignum, values, scale=1, marginals=None):
pose3Values = gtsam.allPose3s(values)
def plot_trajectory(fignum, values, scale=1, marginals=None,
title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a complete 3D trajectory using poses in `values`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dict containing the poses.
scale (float): Value to scale the poses by.
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
pose3Values = gtsam.utilities_allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastIndex = None
@ -209,11 +340,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
pass
if marginals:
P = marginals.marginalCovariance(lastKey)
covariance = marginals.marginalCovariance(lastKey)
else:
P = None
covariance = None
plot_pose3(fignum, lastPose, P, scale)
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
lastIndex = i
@ -223,11 +355,15 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
P = marginals.marginalCovariance(lastKey)
covariance = marginals.marginalCovariance(lastKey)
else:
P = None
covariance = None
plot_pose3(fignum, lastPose, P, scale)
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
except:
pass
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())