def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
"""Add a prior on the navigation state at time `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))
def test_PriorFactor(self):
values = gtsam.Values()
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
factor = gtsam.PriorFactorPose3(key, priorPose3, model)
values.insert(key, priorPose3)
self.assertEqual(factor.error(values), 0)
key = 3
priorVector = np.array([0., 0., 0.])
model = gtsam.noiseModel_Unit.Create(3)
factor = gtsam.PriorFactorVector(key, priorVector, model)
values.insert(key, priorVector)
self.assertEqual(factor.error(values), 0)
############################### Build the factor graph ####################################
factor_graph = gtsam.NonlinearFactorGraph()
# A technical hack for defining variable names in GTSAM python bindings
def symbol(letter, index):
return int(gtsam.symbol(ord(letter), index))
# Add a prior factor on the initial position
factor_graph.push_back(
gtsam.PriorFactorPose3(symbol('x', 0), params.initial_pose,
params.initial_pose_covariance))
factor_graph.push_back(
gtsam.PriorFactorVector(symbol('v', 0), params.initial_velocity,
params.initial_velocity_covariance))
# Add IMU factors (or "motion model"/"transition" factors).
# Ideally, we would add factors between every pair (xᵢ₋₁, xᵢ). But, to save computations,
# we will add factors between pairs (x₀, xₖ), (xₖ, x₂ₖ) etc., and as an IMU "measurement"
# between e.g. x₀ and xₖ we will use combined (pre-integrated) measurements `0, 1, ..., k-1`.
# Below, `k == PREINTEGRATE_EVERY_STEPS`.
PREINTEGRATE_EVERY_STEPS = 25
# For code generalization, create pairs (0, k), (k, 2k), (2k, 3k), ..., (mk, N-1)
preintegration_steps = list(
range(0, len(imu_measurements), PREINTEGRATE_EVERY_STEPS))
if preintegration_steps[-1] != len(
imu_measurements) - 1: # don't miss the last measurements
preintegration_steps.append(len(imu_measurements) - 1)
# An iterator over those pairs
vv2.insert(4, np.array([4., 2., 1]))
vv2.print_(b"vv2:")
vv.print_(b"vv:")
vv.insert(4, np.array([1., 2., 4.]))
vv.print_(b"vv:")
vv3 = vv.add(vv2)
vv3.print_(b"vv3:")
values = gtsam.Values()
values.insert(1, gtsam.Point3())
values.insert(2, gtsam.Rot3())
values.print_(b"values:")
factor = gtsam.PriorFactorVector(1, np.array([1., 2., 3.]), diag)
print "Prior factor vector: ", factor
keys = gtsam.KeyVector()
keys.push_back(1)
keys.push_back(2)
print 'size: ', keys.size()
print keys.at(0)
print keys.at(1)
noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
noise.print_('noise:')
print 'noise print:', noise
f = gtsam.JacobianFactor(7, np.ones([2, 2]), model=noise, b=np.ones(2))
print 'JacobianFactor(7):\n', f
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 addPrior(self, i, graph):
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))
def add_imu_measurements(self,
measured_poses,
measured_acc,
measured_omega,
measured_vel,
delta_t,
n_skip,
initial_poses=None):
n_frames = measured_poses.shape[0]
# Check if sizes are correct
assert measured_poses.shape[0] == n_frames
assert measured_acc.shape[0] == n_frames
assert measured_vel.shape[0] == n_frames
# Pose prior
pose_key = X(0)
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2]))
pose_0 = gtsam.Pose3(measured_poses[0])
self.graph.push_back(
gtsam.PriorFactorPose3(pose_key, pose_0, pose_noise))
self.initial_estimate.insert(pose_key, gtsam.Pose3(measured_poses[0]))
# IMU prior
bias_key = B(0)
bias_noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.5)
self.graph.push_back(
gtsam.PriorFactorConstantBias(bias_key,
gtsam.imuBias.ConstantBias(),
bias_noise))
self.initial_estimate.insert(bias_key, gtsam.imuBias.ConstantBias())
# Velocity prior
velocity_key = V(0)
velocity_noise = gtsam.noiseModel.Isotropic.Sigma(3, .5)
velocity_0 = measured_vel[0]
self.graph.push_back(
gtsam.PriorFactorVector(velocity_key, velocity_0, velocity_noise))
self.initial_estimate.insert(velocity_key, velocity_0)
# Preintegrator
accum = gtsam.PreintegratedImuMeasurements(self.IMU_PARAMS)
# Add measurements to factor graph
for i in range(1, n_frames):
accum.integrateMeasurement(measured_acc[i], measured_omega[i],
delta_t[i - 1])
if i % n_skip == 0:
pose_key += 1
DELTA = gtsam.Pose3(
gtsam.Rot3.Rodrigues(0, 0, 0.1 * np.random.randn()),
gtsam.Point3(4 * np.random.randn(), 4 * np.random.randn(),
4 * np.random.randn()))
self.initial_estimate.insert(
pose_key,
gtsam.Pose3(measured_poses[i]).compose(DELTA))
velocity_key += 1
self.initial_estimate.insert(velocity_key, measured_vel[i])
bias_key += 1
self.graph.add(
gtsam.BetweenFactorConstantBias(
bias_key - 1, bias_key, gtsam.imuBias.ConstantBias(),
self.BIAS_COVARIANCE))
self.initial_estimate.insert(bias_key,
gtsam.imuBias.ConstantBias())
# Add IMU Factor
self.graph.add(
gtsam.ImuFactor(pose_key - 1, velocity_key - 1, pose_key,
velocity_key, bias_key, accum))
# Reset preintegration
accum.resetIntegration()
except StopIteration:
pass
# Accumulate the current measurement
current_preintegrated_IMU.integrateMeasurement(measured_acceleration,
measured_angular_vel,
dt[i])
# Add a prior factor on the initial state
factor_graph.push_back(
gtsam.PriorFactorPose3(symbol('x', preintegration_steps[0]),
params.initial_state.pose(),
params.initial_pose_covariance))
factor_graph.push_back(
gtsam.PriorFactorVector(symbol('v', preintegration_steps[0]),
params.initial_state.velocity(),
params.initial_velocity_covariance))
factor_graph.push_back(
gtsam.PriorFactorConstantBias(symbol('b', 0), params.IMU_bias,
params.IMU_bias_covariance))
if USE_VISUAL_FACTORS:
factor_graph.push_back(
gtsam.PriorFactorPoint3(symbol('m', 0), gtsam.Point3(1, 0, 0),
gtsam.noiseModel_Isotropic.Sigma(3, 0.05)))
# Other example priors, e.g. a constraint that the intial and the final states should be roughly identical:
factor_graph.push_back(
gtsam.PriorFactorPose3(symbol('x', preintegration_steps[-1]),
params.initial_state.pose(),
params.initial_pose_covariance))
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)