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

Sync ImuFactorExample2

release/4.3a0
Fan Jiang 2020-07-27 11:01:59 -04:00
parent 2bda74950a
commit 48b0c845dc
1 changed files with 33 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -8,22 +8,20 @@ from __future__ import print_function
import math import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam import gtsam
import gtsam.utils.plot as gtsam_plot import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
def X(key): from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
"""Create symbol for pose key.""" ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
return gtsam.symbol('x', key) PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
def V(key): from gtsam import symbol_shorthand
"""Create symbol for velocity key.""" B = symbol_shorthand.B
return gtsam.symbol('v', key) V = symbol_shorthand.V
X = symbol_shorthand.X
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def vector3(x, y, z): def vector3(x, y, z):
@ -69,20 +67,19 @@ PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1) BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20)) Point3(0.05, -0.10, 0.20))
def IMU_example(): def IMU_example():
"""Run iSAM 2 example with IMU factor.""" """Run iSAM 2 example with IMU factor."""
ZERO_BIAS = gtsam.imuBias.ConstantBias()
# Start with a camera on x-axis looking at origin # Start with a camera on x-axis looking at origin
radius = 30 radius = 30
up = gtsam.Point3(0, 0, 1) up = Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0) target = Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0) position = Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose() pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses # Create the set of ground-truth landmarks and poses
@ -91,37 +88,37 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0) angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario( scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0) angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph # Create a factor graph
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver # Create (incremental) ISAM2 solver
isam = gtsam.ISAM2() isam = ISAM2()
# Create the initial estimate to the solution # Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth # Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values() initialEstimate = Values()
# Add a prior on pose x0. This indirectly specifies where the origin is. # Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel.Diagonal.Sigmas( noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors # Add imu priors
biasKey = gtsam.symbol('b', 0) biasKey = B(0)
biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, ZERO_BIAS, biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
biasnoise) biasnoise)
newgraph.push_back(biasprior) newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, ZERO_BIAS) initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity # Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0) n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) velprior = PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior) newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity) initialEstimate.insert(V(0), n_velocity)
@ -142,10 +139,10 @@ def IMU_example():
# Add Bias variables periodically # Add Bias variables periodically
if i % 5 == 0: if i % 5 == 0:
biasKey += 1 biasKey += 1
factor = gtsam.BetweenFactorConstantBias( factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, ZERO_BIAS, BIAS_COVARIANCE) biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor) newgraph.add(factor)
initialEstimate.insert(biasKey, ZERO_BIAS) initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame # Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix() nRb = scenario.rotation(t).matrix()
@ -155,8 +152,7 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor # Add Imu Factor
imufac = gtsam.ImuFactor( imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac) newgraph.add(imufac)
# insert new velocity, which is wrong # insert new velocity, which is wrong
@ -169,7 +165,7 @@ def IMU_example():
ISAM2_plot(result) ISAM2_plot(result)
# reset # reset
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
initialEstimate.clear() initialEstimate.clear()