def test_values(self):
values = Values()
E = EssentialMatrix(Rot3(), Unit3())
tol = 1e-9
values.insert(0, Point2(0, 0))
values.insert(1, Point3(0, 0, 0))
values.insert(2, Rot2())
values.insert(3, Pose2())
values.insert(4, Rot3())
values.insert(5, Pose3())
values.insert(6, Cal3_S2())
values.insert(7, Cal3DS2())
values.insert(8, Cal3Bundler())
values.insert(9, E)
values.insert(10, imuBias_ConstantBias())
# Special cases for Vectors and Matrices
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
# floating point numbers in column-major (Fortran style) storage order,
# whereas by default, numpy.array is in row-major order and the type is
# in whatever the number input type is, e.g. np.array([1,2,3])
# will have 'int' type.
#
# The wrapper will automatically fix the type and storage order for you,
# but for performance reasons, it's recommended to specify the correct
# type and storage order.
# for vectors, the order is not important, but dtype still is
vec = np.array([1., 2., 3.])
values.insert(11, vec)
mat = np.array([[1., 2.], [3., 4.]], order='F')
values.insert(12, mat)
# Test with dtype int and the default order='C'
# This still works as the wrapper converts to the correct type and order for you
# but is nornally not recommended!
mat2 = np.array([[1, 2, ], [3, 5]])
values.insert(13, mat2)
self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol))
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
self.assertTrue(values.atimuBias_ConstantBias(
10).equals(imuBias_ConstantBias(), tol))
# special cases for Vector and Matrix:
actualVector = values.atVector(11)
self.assertTrue(np.allclose(vec, actualVector, tol))
actualMatrix = values.atMatrix(12)
self.assertTrue(np.allclose(mat, actualMatrix, tol))
actualMatrix2 = values.atMatrix(13)
self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
def n2g(numpy_arr, obj):
if obj == "Quaternion":
x, y, z, w = numpy_arr
return gtsam.Rot3.Quaternion(w, x, y, z)
elif obj == "Euler":
roll, pitch, yaw = numpy_arr
return gtsam.Rot3.Ypr(yaw, pitch, roll)
elif obj == "Point2":
x, y = numpy_arr
return gtsam.Point2(x, y)
elif obj == "Pose2":
x, y, yaw = numpy_arr
return gtsam.Pose2(x, y, yaw)
elif obj == "Point3":
x, y, z = numpy_arr
return gtsam.Point3(x, y, z)
elif obj == "Pose3":
x, y, z, roll, pitch, yaw = numpy_arr
return gtsam.Pose3(gtsam.Rot3.Ypr(yaw, pitch, roll),
gtsam.Point3(x, y, z))
elif obj == "imuBiasConstantBias":
imu_bias = numpy_arr
return gtsam.imuBias_ConstantBias(np.array(imu_bias[:3]),
np.array(imu_bias[3:]))
elif obj == "Vector":
return np.array(numpy_arr)
else:
raise NotImplementedError("Not implemented from numpy to " + obj)
def __init__(self, twist=None, bias=None, dt=1e-2):
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
# setup interactive plotting
plt.ion()
# Setup loop as default scenario
if twist is not None:
(W, V) = twist
else:
# default = loop with forward velocity 2m/s, while pitching up
# with angular velocity 30 degree/sec (negative in FLU)
W = np.array([0, -math.radians(30), 0])
V = np.array([2, 0, 0])
self.scenario = gtsam.ConstantTwistScenario(W, V)
self.dt = dt
self.maxDim = 5
self.labels = list('xyz')
self.colors = list('rgb')
# Create runner
self.g = 10 # simple gravity constant
self.params = self.defaultParams(self.g)
if bias is not None:
self.actualBias = bias
else:
accBias = np.array([0, 0.1, 0])
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)
def __init__(self):
self.velocity = np.array([2, 0, 0])
self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Choose one of these twists to change scenario:
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)
accBias = np.array([-0.3, 0.1, 0.2])
gyroBias = np.array([0.1, 0.3, -0.1])
bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
dt = 1e-2
super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
def test_loop_runner(self):
# Forward velocity 2m/s
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
v = 2
w = math.radians(6)
W = np.array([0, -w, 0])
V = np.array([v, 0, 0])
scenario = gtsam.ConstantTwistScenario(W, V)
dt = 0.1
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
bias = gtsam.imuBias_ConstantBias()
runner = gtsam.ScenarioRunner(scenario, params, dt, bias)
# Test specific force at time 0: a is pointing up
t = 0.0
a = w * v
np.testing.assert_almost_equal(np.array([0, 0, a + self.g]),
runner.actualSpecificForce(t))
def Optimize(self, meas_time, imu_samples): """Perform optimization""" # IMU preintegration until measurement time predicted_nav_state = self.ImuUpdate(imu_samples) # Add estimates self.new_initial_ests.insert(self.poseKey, predicted_nav_state.pose()) self.new_initial_ests.insert(self.velKey, predicted_nav_state.velocity()) self.new_initial_ests.insert(self.biasKey, self.current_bias) # Add IMU bias factor bias_factor = gtsam.BetweenFactorConstantBias( self.biasKey-1, self.biasKey, gtsam.imuBias_ConstantBias(), self.bias_cov) self.new_factors.add(bias_factor) # Compute result result = self.Isam2Update() if result: self.current_time = meas_time self.current_global_pose = result.atPose3(self.poseKey) self.current_global_vel = result.atVector(self.velKey) self.current_bias = result.atimuBias_ConstantBias(self.biasKey)
if __name__ == '__main__':
imu_measurements, _, _, true_poses = np.load('circle_gold.npy')
############################## Algorithm parameters ############################
import argparse
params = argparse.Namespace()
# Time step length.
# In real world, it will be different at each measurement,
# and you'll have to take dtᵢ from data.
params.dt = 1e-2
# IMU bias
params.accelerometer_bias = np.array([0, 0.1, 0])
params.gyroscope_bias = np.array([0, 0, 0])
params.IMU_bias = gtsam.imuBias_ConstantBias(params.accelerometer_bias,
params.gyroscope_bias)
# 'circle_gold.npy' simulates a loop with forward velocity 2 m/s,
# while pitching up with angular velocity 30 degrees/sec.
params.initial_velocity = np.array([2, 0, 0])
params.initial_angular_vel = np.array([0, -math.radians(30),
0]) # not used here
params.initial_pose = gtsam.Pose3()
params.initial_state = gtsam.NavState(params.initial_pose,
params.initial_velocity)
# IMU preintegration algorithm parameters
# "U" means "Z axis points up"; "MakeSharedD" would mean "Z axis points along the gravity"
preintegration_params = gtsam.PreintegrationParams.MakeSharedU(
10) # 10 is the gravity force
# Realistic noise parameters
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# 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
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values()
# 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
noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
newgraph.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 = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t # simulation time
if i == 0: # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))
elif i >= 2: # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))
if i > 0:
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = gtsam.BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()
def __init__(self, params):
""" Initialize variables """
self.imu_meas_predict = [] # IMU measurements for pose prediction
self.imu_opt_meas = [] # IMU measurements for pose prediction between measurement updates
self.imu_samples = [] # imu samples
self.opt_meas = [] # optimizes measurements
self.__opt_meas_buffer_time = params['opt_meas_buffer_time']
""" IMU Preintegration """
# IMU preintegration parameters
self.imu_preint_params = gtsam.PreintegrationParams(np.asarray(params['g']))
self.imu_preint_params.setAccelerometerCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2))
self.imu_preint_params.setGyroscopeCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2))
self.imu_preint_params.setIntegrationCovariance(np.eye(3) * params['int_cov_sigma']**2)
self.imu_preint_params.setUse2ndOrderCoriolis(params['setUse2ndOrderCoriolis'])
self.imu_preint_params.setOmegaCoriolis(np.array(params['omega_coriolis']))
""" Initialize Parameters """
# ISAM2 initialization
isam2_params = gtsam.ISAM2Params()
isam2_params.setRelinearizeThreshold(params['relinearize_th'])
isam2_params.setRelinearizeSkip(params['relinearize_skip'])
isam2_params.setFactorization(params['factorization'])
# self.isam2 = gtsam.ISAM2(isam2_params)
self.isam2 = gtsam.ISAM2()
self.new_factors = gtsam.NonlinearFactorGraph()
self.new_initial_ests = gtsam.Values()
self.min_imu_sample = 2 # minimum imu sample count needed for integration
# ISAM2 keys
self.poseKey = gtsam.symbol(ord('x'), 0)
self.velKey = gtsam.symbol(ord('v'), 0)
self.biasKey = gtsam.symbol(ord('b'), 0)
""" Set initial variables """
# Initial state
self.last_opt_time = self.current_time = 0
self.current_global_pose = numpy_pose_to_gtsam(
params['init_pos'],
params['init_ori'])
self.current_global_vel = np.asarray(
params['init_vel'])
self.current_bias = gtsam.imuBias_ConstantBias(
np.asarray(params['init_acc_bias']),
np.asarray(params['init_gyr_bias']))
self.imu_accum = gtsam.PreintegratedImuMeasurements(self.imu_preint_params)
# uncertainty of the initial state
self.init_pos_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_ori_cov'], params['init_ori_cov'], params['init_ori_cov'],
params['init_pos_cov'], params['init_pos_cov'], params['init_pos_cov']]))
self.init_vel_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_vel_cov'], params['init_vel_cov'], params['init_vel_cov']]))
self.init_bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_acc_bias_cov'],
params['init_gyr_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov']]))
# measurement noise
self.dvl_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray(
params['dvl_cov']))
self.bar_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray(
params['bar_cov']))
self.bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.concatenate((
params['sigma_acc_bias_evol'],
params['sigma_gyr_bias_evol'])))
""" Set initial state """
# Initial factors
prior_pose_factor = gtsam.PriorFactorPose3(
self.poseKey,
self.current_global_pose,
self.init_pos_cov)
self.new_factors.add(prior_pose_factor)
prior_vel_factor = gtsam.PriorFactorVector(
self.velKey,
self.current_global_vel,
self.init_vel_cov)
self.new_factors.add(prior_vel_factor)
prior_bias_factor = gtsam.PriorFactorConstantBias(
self.biasKey,
self.current_bias,
self.init_bias_cov)
self.new_factors.add(prior_bias_factor)
# Initial estimates
self.new_initial_ests.insert(self.poseKey, self.current_global_pose)
self.new_initial_ests.insert(self.velKey, self.current_global_vel)
self.new_initial_ests.insert(self.biasKey, self.current_bias)
