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

update ImuFactorExample2.py

release/4.3a0
Varun Agrawal 2020-07-27 21:00:44 -05:00
parent 858f5d42d3
commit 0b550b355f
1 changed files with 57 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
python/gtsam_py/python/examples/ImuFactorExample2.py
View File

@ -1,6 +1,6 @@
"""
iSAM2 example with ImuFactor.
Author: Robert Truax (C++), Frank Dellaert (Python)
Author: Frank Dellaert, Varun Agrawal
"""
# pylint: disable=invalid-name, E1101
@ -8,17 +8,18 @@ from __future__ import print_function
import math
import gtsam
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import numpy as np
import gtsam
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
from gtsam.gtsam.symbol_shorthand import B, V, X
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
from gtsam.utils import plot
def vector3(x, y, z):
@ -26,35 +27,13 @@ def vector3(x, y, z):
return np.array([x, y, z], dtype=np.float)
def ISAM2_plot(values, fignum=0):
"""Plot poses."""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
i = 0
min_bounds = 0, 0, 0
max_bounds = 0, 0, 0
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
position = pose_i.translation()
min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
# max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
i += 1
# draw
axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
plt.pause(1)
# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
g = 9.81
n_gravity = vector3(0, 0, -g)
def preintegration_parameters():
# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
I = np.eye(3)
PARAMS.setAccelerometerCovariance(I * 0.1)
@ -67,29 +46,44 @@ BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
Point3(0.05, -0.10, 0.20))
return PARAMS, BIAS_COVARIANCE, DELTA
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
def get_camera(radius):
up = Point3(0, 0, 1)
target = Point3(0, 0, 0)
position = Point3(radius, 0, 0)
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose()
return camera
# Create the set of ground-truth landmarks and poses
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
def get_scenario(radius, pose_0, angular_velocity, delta_t):
"""Create the set of ground-truth landmarks and poses"""
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
return scenario
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
camera = get_camera(radius)
pose_0 = camera.pose()
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity = math.radians(180) # rad/sec
scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
# Create a factor graph
newgraph = NonlinearFactorGraph()
graph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = ISAM2()
@ -102,21 +96,21 @@ def IMU_example():
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = B(0)
biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
biasnoise)
newgraph.push_back(biasprior)
graph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
graph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
@ -138,7 +132,7 @@ def IMU_example():
biasKey += 1
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
graph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
@ -150,21 +144,24 @@ def IMU_example():
# Add Imu Factor
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
graph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
isam.update(graph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
plot.plot_incremental_trajectory(0, result,
start=i, scale=3, time_interval=0.01)
# reset
newgraph = NonlinearFactorGraph()
graph = NonlinearFactorGraph()
initialEstimate.clear()
plt.show()
if __name__ == '__main__':
IMU_example()