def step5_add_odometry_and_landmark_observations():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X2 = X(2)
X3 = X(3)
L2 = L(2)
# Update the list with the new variable keys.
pose_variables.append(X3)
landmark_variables.append(L2)
# Add the odometry measurement.
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
def plot(self,
values: gtsam.Values,
title: str = "Estimated Trajectory",
fignum: int = POSES_FIG + 1,
show: bool = False):
"""
Plot poses in values.
Args:
values: The values object with the poses to plot.
title: The title of the plot.
fignum: The matplotlib figure number.
POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure.
show: Flag indicating whether to display the figure.
"""
i = 0
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
plot_pose3(fignum, pose_i, 1)
i += 1
plt.title(title)
gtsam.utils.plot.set_axes_equal(fignum)
print("Bias Values", values.atConstantBias(BIAS_KEY))
plt.ioff()
if show:
plt.show()
def plot_3d(result, ax, seen):
ax.cla()
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3_on_axes(ax, pose_i, 8)
i += 1
# plot landmarks
if isinstance(seen, int):
seen = np.arange(seen)
for i in seen:
try:
pose_i = result.atPose3(L(i))
gtsam_plot.plot_pose3_on_axes(ax, pose_i, 4)
except:
pass
# draw
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
set_axes_equal(ax)
ax.view_init(-55, -85)
def add_point(self, pointsInitial, measurements, octave):
if pointsInitial[-1] > self.depth_threshold:
information = self.inv_lvl_sigma2[octave] * np.identity(2)
stereo_model = gt.noiseModel_Diagonal.Information(information)
huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaMono)
robust_model = gt.noiseModel_Robust(huber, stereo_model)
factor = gt.GenericProjectionFactorCal3_S2(
gt.Point2(measurements[0], measurements[2]), robust_model,
X(1), L(self.counter), self.K_mono)
self.is_stereo.append(False)
else:
information = self.inv_lvl_sigma2[octave] * np.identity(3)
stereo_model = gt.noiseModel_Diagonal.Information(information)
huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaStereo)
robust_model = gt.noiseModel_Robust(huber, stereo_model)
factor = gt.GenericStereoFactor3D(
gt.StereoPoint2(*tuple(measurements)), robust_model, X(1),
L(self.counter), self.K_stereo)
self.is_stereo.append(True)
self.graph.add(
gt.NonlinearEqualityPoint3(L(self.counter),
gt.Point3(pointsInitial)))
self.initialEstimate.insert(L(self.counter), gt.Point3(pointsInitial))
self.graph.add(factor)
self.octave.append(octave)
self.counter += 1
def visual_ISAM2_plot(result):
"""
VisualISAMPlot plots current state of ISAM2 object
Author: Ellon Paiva
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
"""
# Declare an id for the figure
fignum = 0
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
# gtsam.plot_3d_points(result, [], marginals)
gtsam_plot.plot_3d_points(fignum, result, 'rx')
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
i += 1
# draw
axes.set_xlim3d(-40, 40)
axes.set_ylim3d(-40, 40)
axes.set_zlim3d(-40, 40)
plt.pause(1)
def add_node(self, kf):
self.initialEstimate.insert(X(kf.kfID), gt.Pose3(kf.pose_matrix()))
for kf_n, rel_pose, _ in kf.neighbors:
if kf_n.kfID > kf.kfID:
continue
self.graph.add(
gt.BetweenFactorPose3(X(kf.kfID), X(kf_n.kfID),
gt.Pose3(rel_pose), self.covariance))
def test_SFMExample(self):
options = generator.Options()
options.triangle = False
options.nrCameras = 10
[data, truth] = generator.generate_data(options)
measurementNoiseSigma = 1.0
pointNoiseSigma = 0.1
poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
graph = gtsam.NonlinearFactorGraph()
# Add factors for all measurements
measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma)
for i in range(len(data.Z)):
for k in range(len(data.Z[i])):
j = data.J[i][k]
graph.add(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], measurementNoise,
X(i), P(j), data.K))
posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(X(0),
truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(P(0),
truth.points[0], pointPriorNoise))
# Initial estimate
initialEstimate = gtsam.Values()
for i in range(len(truth.cameras)):
pose_i = truth.cameras[i].pose()
initialEstimate.insert(X(i), pose_i)
for j in range(len(truth.points)):
point_j = truth.points[j]
initialEstimate.insert(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
for i in range(5):
optimizer.iterate()
result = optimizer.values()
# Marginalization
marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(P(0))
marginals.marginalCovariance(X(0))
# Check optimized results, should be equal to ground truth
for i in range(len(truth.cameras)):
pose_i = result.atPose3(X(i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(P(j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
def plot_2d(result, isam, ax, seen):
idx = np.ix_((0, -1), (0, -1))
ax.cla()
# plot cameras
i = 0
cameras = []
while result.exists(X(i)):
# get all data from pose
pose = result.atPose3(X(i))
# R = pose.rotation().matrix()
# cov = isam.marginalCovariance(X(i))[3:6,3:6]
# cov = ( R@[email protected] )[idx]
t = [result.atPose3(X(i)).x(), result.atPose3(X(i)).z()]
# do we want pose covariances?
# ax.add_patch(cov_patch(t, cov, 'b'))
cameras.append(t)
i += 1
# plot landmarks
landmarks = []
for i in seen:
# get all data from pose
pose = result.atPose3(L(i))
R = pose.rotation().matrix()
cov = isam.marginalCovariance(L(i))[3:6, 3:6]
cov = (R @ cov @ R.T)[idx]
t = [result.atPose3(L(i)).x(), result.atPose3(L(i)).z()]
ax.add_patch(cov_patch(t, cov, 'r'))
landmarks.append(t)
cameras = np.array(cameras)
landmarks = np.array(landmarks)
ax.plot(cameras[:, 0],
cameras[:, 1],
label="Camera Poses",
c='b',
marker='o')
ax.scatter(landmarks[:, 0], landmarks[:, 1], label="Landmarks", c='r')
ax.legend()
ax.set_xlabel("X")
ax.set_ylabel("Z")
ax.set_aspect('equal')
def predict(self, x, u):
"""
Should just predict next pose and add it together with the odometry factor
"""
odom_factor = gtsam.BetweenFactorPose2(X(self.kx), X(self.kx + 1),
gtsam.Pose2(u[0], u[1], u[2]),
self.odometry_noise)
self.add_factor(odom_factor)
xpred = self.f(x, u)
self.estimates.insert(X(self.kx + 1), utils.np2pose(xpred))
self.kx += 1
return x
def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int,
gps_measurements: List[GpsMeasurement]):
"""Write the results from `isam` to `output_filename`."""
# Save results to file
print("Writing results to file...")
with open(output_filename, 'w', encoding='UTF-8') as fp_out:
fp_out.write(
"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n")
result = isam.calculateEstimate()
for i in range(first_gps_pose, len(gps_measurements)):
pose_key = X(i)
vel_key = V(i)
bias_key = B(i)
pose = result.atPose3(pose_key)
velocity = result.atVector(vel_key)
bias = result.atConstantBias(bias_key)
pose_quat = pose.rotation().toQuaternion()
gps = gps_measurements[i].position
print(f"State at #{i}")
print(f"Pose:\n{pose}")
print(f"Velocity:\n{velocity}")
print(f"Bias:\n{bias}")
fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format(
gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
gps[0], gps[1], gps[2]))
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 __init__(self,
p,
q,
r,
alphas=np.array([0.001, 0.0001]),
sensor_offset=np.zeros(2)):
# Add noise models
self.odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(q)
self.measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(r)
self.alphas = alphas
self.sensor_offset = sensor_offset
# Create graph and initilize newest pose
self.graph = gtsam.NonlinearFactorGraph()
self.poses = gtsam.Values()
# To enumerate all poses and landmarks
self.kx = 1
self.kl = 1
self.landmarks = np.empty(0)
# Initilize graph with prior
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(p)
self.graph.add(
gtsam.PriorFactorPose2(X(self.kx), gtsam.Pose2(0.0, 0.0, 0.0),
prior_noise))
def create_graph():
"""Create a basic linear factor graph for testing"""
graph = gtsam.GaussianFactorGraph()
x0 = X(0)
x1 = X(1)
x2 = X(2)
BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE)
graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
return graph, (x0, x1, x2)
def __init__(self):
# Initialize the class
cls_un_model_fake_1d_conf.initialize_python_objects()
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
def __init__(self, model_noise):
# The instances differ from each other by model noise
self.model_noise = model_noise
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
def test_printing(self):
"""Tests if the factor result matches the GTSAM Pose2 unit test"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
def error_func(this: CustomFactor, v: gtsam.Values,
_: List[np.ndarray]):
"""Minimal error function stub"""
return np.array([1, 0, 0])
noise_model = gtsam.noiseModel.Unit.Create(3)
from gtsam.symbol_shorthand import X
cf = CustomFactor(noise_model, [X(0), X(1)], error_func)
cf_string = """CustomFactor on x0, x1
noise model: unit (3)
"""
self.assertEqual(cf_string, repr(cf))
def __init__(self):
# Model initialization
joint_lambda_pose_factor_fake_1d.initialize_python_objects()
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
def __init__(self, exp_add_1, exp_add_2, exp_add_3, exp_add_4, exp_add_5,
rinf_add_1, rinf_add_2, rinf_add_3, rinf_add_4, rinf_add_5):
# Initialize the class
cls_un_model_4d.initialize_python_objects(exp_add_1, exp_add_2,
exp_add_3, exp_add_4,
exp_add_5, rinf_add_1,
rinf_add_2, rinf_add_3,
rinf_add_4, rinf_add_5)
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
def __init__(self,
geo_model,
da_model,
lambda_model,
prior_mean,
prior_noise,
lambda_prior_mean=(0., 0.),
lambda_prior_noise=((0.5, 0.), (0., 0.5)),
cls_enable=True):
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
# Enable Lambda inference
self.cls_enable = cls_enable
# Camera pose prior
self.graph = gtsam.NonlinearFactorGraph()
self.graph.add(
gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise))
# Setting initial values
self.initial = gtsam.Values()
self.initial.insert(self.X(0), prior_mean)
self.prev_step_camera_pose = prior_mean
self.daRealization = list()
# Setting up models
self.geoModel = geo_model
self.daModel = da_model
self.lambdaModel = lambda_model
# Setting up ISAM2
params2 = gtsam.ISAM2Params()
#params2.relinearize_threshold = 0.01
#params2.relinearize_skip = 1
self.isam = gtsam.ISAM2(params2)
self.isam.update(self.graph, self.initial)
# Set custom lambda
if type(lambda_prior_mean) is np.ndarray:
self.lambda_prior_mean = lambda_prior_mean
else:
self.lambda_prior_mean = np.array(lambda_prior_mean)
self.lambda_prior_cov = gtsam.noiseModel.Gaussian.Covariance(
np.matrix(lambda_prior_noise))
self.num_cls = len(self.lambda_prior_mean) + 1
# Initialize object last lambda database
self.object_lambda_dict = dict()
def __init__(self, exp_add_1, exp_add_2, exp_add_3, exp_add_4, exp_add_5,
rinf_add_1, rinf_add_2, rinf_add_3, rinf_add_4, rinf_add_5):
# Model initialization
joint_lambda_pose_factor_2d.initialize_python_objects(
exp_add_1, exp_add_2, exp_add_3, exp_add_4, exp_add_5, rinf_add_1,
rinf_add_2, rinf_add_3, rinf_add_4, rinf_add_5)
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
def __init__(self,
class_probability_prior,
geo_model,
da_model,
cls_model,
prior_mean,
prior_noise,
cls_enable=True):
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
# Camera pose prior
self.graph = gtsam.NonlinearFactorGraph()
self.graph.add(
gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise))
# Class realization and prior probabilities. if one of the class probabilities is 0 it will
# set the logWeight to -inf
self.cls_enable = cls_enable
# Setting initial values
self.initial = gtsam.Values()
self.initial.insert(self.X(0), prior_mean)
self.prev_step_camera_pose = prior_mean
self.daRealization = list()
# self.initial.print()
# Setting up models
self.geoModel = geo_model
self.daModel = da_model
self.clsModel = cls_model
# self.classifierModel = classifierModel
# Setting up ISAM2 TODO: make ISAM2 work. As of now, it doesn't take the initial values at optimize_isam2 method
params2 = gtsam.ISAM2Params()
# params2.relinearize_threshold = 0.01
# params2.relinearize_skip = 1
self.isam = gtsam.ISAM2(params2)
self.isam.update(self.graph, self.initial)
# Setting up weight memory
self.weight_memory = list()
self.normalized_weight = 0
# Setting up class realization
self.classRealization = dict()
self.classProbabilityPrior = class_probability_prior
def add_pose(self, R, t):
# Add measurements
# pose 1
# graph.add(gt.GenericStereoFactor3D(gt.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
# graph.add(gt.GenericStereoFactor3D(gt.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
# graph.add(gt.GenericStereoFactor3D(gt.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
# pose 2
# graph.add(gt.GenericStereoFactor3D(gt.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
# graph.add(gt.GenericStereoFactor3D(gt.StereoPoint2(70, 20, 490), stereo_model, x2, l2, K))
# graph.add(gt.GenericStereoFactor3D(gt.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
# self.initialEstimate.insert(X(1), gt.Rot3(pose[0]), gt.Point3(pose[1]))
t = t.reshape((3, 1))
self.initialEstimate.insert(X(1),
gt.Pose3(np.concatenate((R, t), axis=1)))
def __init__(self):
# Create graph container and add factors to it
self.graph = gt.NonlinearFactorGraph()
# Create initial estimate for camera poses and landmarks
self.initialEstimate = gt.Values()
sigmas = np.array([
5 * np.pi / 180, 5 * np.pi / 180, 5 * np.pi / 180, 0.05, 0.05, 0.05
])
self.covariance = gt.noiseModel.Diagonal.Sigmas(sigmas)
self.graph.add(gt.NonlinearEqualityPose3(X(0), gt.Pose3(np.eye(4))))
self.result = None
self.marginals = None
def step3_add_pose_from_odometry():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X1 = X(1)
X2 = X(2)
# Update the list with the new pose variable key.
pose_variables.append(X2)
# Add an odometry measurement.
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
def step4_add_new_landmark_observation():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X2 = X(2)
L1 = L(1)
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Update ISAM2 with the new factor graph.
# There are no new variables this update, so no new initial values.
isam.update(graph, gtsam.Values())
def step2_add_landmark_observation():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X1 = X(1)
L1 = L(1)
# Update the list with the new landmark variable key.
landmark_variables.append(L1)
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), measurement_noise))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
def step1_initialize():
# Create an empty nonlinear factor graph.
# We will need to do this for every update.
graph = gtsam.NonlinearFactorGraph()
# Create a key for the first pose.
X1 = X(1)
# Update the list with the new pose variable key.
pose_variables.append(X1)
# Add a prior on pose X1 at the origin.
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0),
prior_noise))
# Set an initial estimate for the first pose.
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
# Update ISAM2 with the initial factor graph.
isam.update(graph, initial_estimate)
# Graph initialization
graph_1 = gtsam.NonlinearFactorGraph()
# Add prior lambda
prior_noise = gtsam.noiseModel.Diagonal.Covariance(
np.array([[3.2, 0.0, 0.0, 0.0], [0.0, 3.2, 0.0, 0.0], [0.0, 0.0, 3.2, 0.0],
[0.0, 0.0, 0.0, 3.2]]))
prior_noise_pose = gtsam.noiseModel.Diagonal.Variances(
np.array([0.003, 0.003, 0.001, 0.003, 0.003, 0.003]))
geo_noise = gtsam.noiseModel.Diagonal.Variances(
np.array([0.003, 0.003, 0.001, 0.002, 0.002, 0.003]))
graph_1.add(
lambda_prior_factor.LambdaPriorFactor(L(0), np.array([0., 0., 0., 0.]),
prior_noise))
graph_1.add(
gtsam.PriorFactorPose3(X(0), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)),
prior_noise_pose))
graph_1.add(
gtsam.BetweenFactorPose3(X(0), XO(1),
gtsam.Pose3(gtsam.Pose2(1.0, 0.0, -0 * 3.14 / 2)),
geo_noise))
graph_1.add(
joint_lambda_pose_factor_fake_2d.JLPFactor(
X(0), XO(1), Lambda(0), Lambda(1), [1.0, 0.0, 0.0, 0.0],
[1.1, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 1.1, 0.0, 1.1]))
print(graph_1)
initial = gtsam.Values()
initial.insert(Lambda(0), [0.0, 0.0, 0.0, 0.0])
initial.insert(Lambda(1), [0.0, 0.0, 0.0, 0.0])
initial.insert(
def batch_factorgraph_example():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create the keys for the poses.
X1 = X(1)
X2 = X(2)
X3 = X(3)
pose_variables = [X1, X2, X3]
# Create keys for the landmarks.
L1 = L(1)
L2 = L(2)
landmark_variables = [L1, L2]
# Add a prior on pose X1 at the origin.
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise))
# Add odometry factors between X1,X2 and X2,X3, respectively
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1,
0.1]))
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add Range-Bearing measurements to two different landmarks L1 and L2
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Create an optimizer.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
# Solve the MAP problem.
result = optimizer.optimize()
# Calculate marginal covariances for all variables.
marginals = gtsam.Marginals(graph, result)
# Extract marginals
pose_marginals = []
for var in pose_variables:
pose_marginals.append(
MultivariateNormalParameters(result.atPose2(var),
marginals.marginalCovariance(var)))
landmark_marginals = []
for var in landmark_variables:
landmark_marginals.append(
MultivariateNormalParameters(result.atPoint2(var),
marginals.marginalCovariance(var)))
# You can extract the joint marginals like this.
joint_all = marginals.jointMarginalCovariance(
gtsam.KeyVector(pose_variables + landmark_variables))
print("Joint covariance over all variables:")
print(joint_all.fullMatrix())
# Plot the marginals.
plot_result(pose_marginals, landmark_marginals)
def optimize(gps_measurements: List[GpsMeasurement],
imu_measurements: List[ImuMeasurement],
sigma_init_x: gtsam.noiseModel.Diagonal,
sigma_init_v: gtsam.noiseModel.Diagonal,
sigma_init_b: gtsam.noiseModel.Diagonal,
noise_model_gps: gtsam.noiseModel.Diagonal,
kitti_calibration: KittiCalibration, first_gps_pose: int,
gps_skip: int) -> gtsam.ISAM2:
"""Run ISAM2 optimization on the measurements."""
# Set initial conditions for the estimated trajectory
# initial pose is the reference frame (navigation frame)
current_pose_global = Pose3(gtsam.Rot3(),
gps_measurements[first_gps_pose].position)
# the vehicle is stationary at the beginning at position 0,0,0
current_velocity_global = np.zeros(3)
current_bias = gtsam.imuBias.ConstantBias() # init with zero bias
imu_params = getImuParams(kitti_calibration)
# Set ISAM2 parameters and create ISAM2 solver object
isam_params = gtsam.ISAM2Params()
isam_params.setFactorization("CHOLESKY")
isam_params.relinearizeSkip = 10
isam = gtsam.ISAM2(isam_params)
# Create the factor graph and values object that will store new factors and
# values to add to the incremental graph
new_factors = gtsam.NonlinearFactorGraph()
# values storing the initial estimates of new nodes in the factor graph
new_values = gtsam.Values()
# Main loop:
# (1) we read the measurements
# (2) we create the corresponding factors in the graph
# (3) we solve the graph to obtain and optimal estimate of robot trajectory
print("-- Starting main loop: inference is performed at each time step, "
"but we plot trajectory every 10 steps")
j = 0
included_imu_measurement_count = 0
for i in range(first_gps_pose, len(gps_measurements)):
# At each non=IMU measurement we initialize a new node in the graph
current_pose_key = X(i)
current_vel_key = V(i)
current_bias_key = B(i)
t = gps_measurements[i].time
if i == first_gps_pose:
# Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global)
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
new_factors.addPriorPose3(current_pose_key, current_pose_global,
sigma_init_x)
new_factors.addPriorVector(current_vel_key,
current_velocity_global, sigma_init_v)
new_factors.addPriorConstantBias(current_bias_key, current_bias,
sigma_init_b)
else:
t_previous = gps_measurements[i - 1].time
# Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = gtsam.PreintegratedImuMeasurements(
imu_params, current_bias)
while (j < len(imu_measurements)
and imu_measurements[j].time <= t):
if imu_measurements[j].time >= t_previous:
current_summarized_measurement.integrateMeasurement(
imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope, imu_measurements[j].dt)
included_imu_measurement_count += 1
j += 1
# Create IMU factor
previous_pose_key = X(i - 1)
previous_vel_key = V(i - 1)
previous_bias_key = B(i - 1)
new_factors.push_back(
gtsam.ImuFactor(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key,
current_summarized_measurement))
# Bias evolution as given in the IMU metadata
sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([
np.sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma
] * 3 + [
np.sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma
] * 3))
new_factors.push_back(
gtsam.BetweenFactorConstantBias(previous_bias_key,
current_bias_key,
gtsam.imuBias.ConstantBias(),
sigma_between_b))
# Create GPS factor
gps_pose = Pose3(current_pose_global.rotation(),
gps_measurements[i].position)
if (i % gps_skip) == 0:
new_factors.addPriorPose3(current_pose_key, gps_pose,
noise_model_gps)
new_values.insert(current_pose_key, gps_pose)
print(f"############ POSE INCLUDED AT TIME {t} ############")
print(gps_pose.translation(), "\n")
else:
new_values.insert(current_pose_key, current_pose_global)
# Add initial values for velocity and bias based on the previous
# estimates
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
# Update solver
# =======================================================================
# We accumulate 2*GPSskip GPS measurements before updating the solver at
# first so that the heading becomes observable.
if i > (first_gps_pose + 2 * gps_skip):
print(f"############ NEW FACTORS AT TIME {t:.6f} ############")
new_factors.print()
isam.update(new_factors, new_values)
# Reset the newFactors and newValues list
new_factors.resize(0)
new_values.clear()
# Extract the result/current estimates
result = isam.calculateEstimate()
current_pose_global = result.atPose3(current_pose_key)
current_velocity_global = result.atVector(current_vel_key)
current_bias = result.atConstantBias(current_bias_key)
print(f"############ POSE AT TIME {t} ############")
current_pose_global.print()
print("\n")
return isam