def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values,
key: int):
"""Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2."""
# Print the current estimates computed using iSAM2.
print("*"*50 + f"\nInference after State {key+1}:\n")
print(current_estimate)
# Compute the marginals for all states in the graph.
marginals = gtsam.Marginals(graph, current_estimate)
# Plot the newly updated iSAM2 inference.
fig = plt.figure(0)
axes = fig.gca()
plt.cla()
i = 1
while current_estimate.exists(i):
gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i))
i += 1
plt.axis('equal')
axes.set_xlim(-1, 5)
axes.set_ylim(-1, 3)
plt.pause(1)
def error_point(measurement, calibration: gtsam.Cal3_S2,
this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
x_key = this.keys()[0]
l_key = this.keys()[1]
pose = values.atPose3(x_key)
pw = values.atPoint3(l_key)
cam = PinholeCameraCal3_S2(pose, calibration)
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
error = (pi - measurement)
# print(pi, pi_line, error)
if jacobians is not None:
# print(gtsam.DefaultKeyFormatter(x_key), gtsam.DefaultKeyFormatter(l_key))
d = 1. / q[2]
Rt = cam.pose().rotation().transpose()
Dpose = cal_Dpose(pn, d)
Dpoint = cal_Dpoint(pn, d, Rt)
Dpi_pn = np.array([[320., 0], [0, 320.]], dtype=float)
Dpose = np.matmul(Dpi_pn, Dpose)
Dpoint = np.matmul(Dpi_pn, Dpoint)
jacobians[0] = Dpose
jacobians[1] = Dpoint
return error
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 values_to_gtsfm_data(values: Values, initial_data: GtsfmData) -> GtsfmData:
"""Cast results from the optimization to GtsfmData object.
Args:
values: results of factor graph optimization.
initial_data: data used to generate the factor graph; used to extract information about poses and 3d points in
the graph.
Returns:
optimized poses and landmarks.
"""
result = GtsfmData(initial_data.number_images())
# add cameras
for i in initial_data.get_valid_camera_indices():
result.add_camera(i, values.atPinholeCameraCal3Bundler(C(i)))
# add tracks
for j in range(initial_data.number_tracks()):
input_track = initial_data.get_track(j)
# populate the result with optimized 3D point
result_track = SfmTrack(values.atPoint3(P(j)))
for measurement_idx in range(input_track.number_measurements()):
i, uv = input_track.measurement(measurement_idx)
result_track.add_measurement(i, uv)
result.add_track(result_track)
return result
def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
wRc_values: gtsam.Values) -> gtsam.Values:
"""Estimate poses given rotations and normalized translation directions between cameras.
Args:
i_iZj_list: List of normalized translation direction measurements between camera pairs,
Z here refers to measurements. The measurements are of camera j with reference
to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit
vector to j in i's frame and is not a transformation.
wRc_values: Rotations of the cameras in the world frame.
Returns:
gtsam.Values: Estimated poses.
"""
# Convert the translation direction measurements to world frame using the rotations.
w_iZj_list = gtsam.BinaryMeasurementsUnit3()
for i_iZj in i_iZj_list:
w_iZj = gtsam.Unit3(
wRc_values.atRot3(i_iZj.key1()).rotate(i_iZj.measured().point3()))
w_iZj_list.append(
gtsam.BinaryMeasurementUnit3(i_iZj.key1(), i_iZj.key2(), w_iZj,
i_iZj.noiseModel()))
# Remove the outliers in the unit translation directions.
w_iZj_inliers = filter_outliers(w_iZj_list)
# Run the optimizer to obtain translations for normalized directions.
wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
wTc_values = gtsam.Values()
for key in wRc_values.keys():
wTc_values.insert(
key, gtsam.Pose3(wRc_values.atRot3(key), wtc_values.atPoint3(key)))
return wTc_values
def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
key: int, xy_tol=0.6, theta_tol=17) -> int:
"""Simple brute force approach which iterates through previous states
and checks for loop closure.
Args:
odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame.
current_estimate: The current estimates computed by iSAM2.
key: Key corresponding to the current state estimate of the robot.
xy_tol: Optional argument for the x-y measurement tolerance, in meters.
theta_tol: Optional argument for the theta measurement tolerance, in degrees.
Returns:
k: The key of the state which is helping add the loop closure constraint.
If loop closure is not found, then None is returned.
"""
if current_estimate:
prev_est = current_estimate.atPose2(key+1)
rotated_odom = prev_est.rotation().matrix() @ odom[:2]
curr_xy = np.array([prev_est.x() + rotated_odom[0],
prev_est.y() + rotated_odom[1]])
curr_theta = prev_est.theta() + odom[2]
for k in range(1, key+1):
pose_xy = np.array([current_estimate.atPose2(k).x(),
current_estimate.atPose2(k).y()])
pose_theta = current_estimate.atPose2(k).theta()
if (abs(pose_xy - curr_xy) <= xy_tol).all() and \
(abs(pose_theta - curr_theta) <= theta_tol*np.pi/180):
return k
def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results."""
plot_vals = gtsam.Values()
for i in range(scene_data.numberCameras()):
plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
for j in range(scene_data.numberTracks()):
plot_vals.insert(P(j), result.atPoint3(P(j)))
plot.plot_3d_points(0, plot_vals, linespec="g.")
plot.plot_trajectory(0, plot_vals, title="SFM results")
plt.show()
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
# print(f"{this = },\n{v = },\n{len(H) = }")
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
if len(H) > 0:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
def create_initial_estimate(n):
"""Create initial estimate for n events."""
initial = Values()
zero = Event()
for key in range(n):
TOAFactor.InsertEvent(key, zero, initial)
return initial
def values_to_gtsfm_data(values: Values, initial_data: GtsfmData,
shared_calib: bool) -> GtsfmData:
"""Cast results from the optimization to GtsfmData object.
Args:
values: results of factor graph optimization.
initial_data: data used to generate the factor graph; used to extract information about poses and 3d points in
the graph.
shared_calib: flag indicating if calibrations were shared between the cameras.
Returns:
optimized poses and landmarks.
"""
result = GtsfmData(initial_data.number_images())
is_fisheye_calibration = isinstance(initial_data.get_camera(0),
PinholeCameraCal3Fisheye)
if is_fisheye_calibration:
cal3_value_extraction_lambda = lambda i: values.atCal3Fisheye(
K(0 if shared_calib else i))
else:
cal3_value_extraction_lambda = lambda i: values.atCal3Bundler(
K(0 if shared_calib else i))
camera_class = PinholeCameraCal3Fisheye if is_fisheye_calibration else PinholeCameraCal3Bundler
# add cameras
for i in initial_data.get_valid_camera_indices():
result.add_camera(
i,
camera_class(values.atPose3(X(i)),
cal3_value_extraction_lambda(i)),
)
# add tracks
for j in range(initial_data.number_tracks()):
input_track = initial_data.get_track(j)
# populate the result with optimized 3D point
result_track = SfmTrack(values.atPoint3(P(j)))
for measurement_idx in range(input_track.numberMeasurements()):
i, uv = input_track.measurement(measurement_idx)
result_track.addMeasurement(i, uv)
result.add_track(result_track)
return result
def test_call(self):
expected_pose = Pose2(1, 1, 0)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]) -> np.ndarray:
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = ge.CustomFactor(noise_model, gtsam.KeyVector([0]), error_func)
v = Values()
v.insert(0, Pose2(1, 0, 0))
e = cf.error(v)
self.assertEqual(e, 0.5)
def test_optimize(self):
"""Do trivial test with three optimizer variants."""
fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
optimal_values = Values()
optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, fg.error(optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
initial_values = Values()
initial_values.insert(KEY1, x0)
self.assertEqual(9.0, fg.error(initial_values), 1e-3)
# optimize parameters
ordering = Ordering()
ordering.push_back(KEY1)
# Gauss-Newton
gnParams = GaussNewtonParams()
gnParams.setOrdering(ordering)
actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
self.assertAlmostEqual(0, fg.error(actual1))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(ordering)
actual2 = LevenbergMarquardtOptimizer(fg, initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual2 = LevenbergMarquardtOptimizer(fg, initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)
actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
self.assertAlmostEqual(0, fg.error(actual3))
# Graduated Non-Convexity (GNC)
gncParams = GncLMParams()
actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize()
self.assertAlmostEqual(0, fg.error(actual4))
def test_call(self):
"""Test if calling the factor works (only error)"""
expected_pose = Pose2(1, 1, 0)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]) -> np.ndarray:
"""Minimal error function with no Jacobian"""
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0], error_func)
v = Values()
v.insert(0, Pose2(1, 0, 0))
e = cf.error(v)
self.assertEqual(e, 0.5)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""
the custom error function. One can freely use variables captured
from the outside scope. Or the variables can be acquired by indexing `v`.
Jacobian is passed by modifying the H array of numpy matrices.
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
def test_optimization(self):
"""Tests if a factor graph with a CustomFactor can be properly optimized"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)
fg = gtsam.NonlinearFactorGraph()
fg.add(cf)
fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
v = Values()
v.insert(0, Pose2(0, 0, 0))
v.insert(1, Pose2(0, 0, 0))
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params)
result = optimizer.optimize()
self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5)
self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> float:
"""Odometry Factor error function
:param measurement: Odometry measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key1 = this.keys()[0]
key2 = this.keys()[1]
pos1, pos2 = values.atVector(key1), values.atVector(key2)
error = measurement - (pos1 - pos2)
if jacobians is not None:
jacobians[0] = I
jacobians[1] = -I
return error
def test_no_jacobian(self):
"""Tests that we will not calculate the Jacobian if not requested"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""
Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
self.assertTrue(
H is None) # Should be true if we only request the error
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
e_cf = cf.error(v)
e_bf = bf.error(v)
np.testing.assert_allclose(e_cf, e_bf)
def test_jacobian(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)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""
the custom error function. One can freely use variables captured
from the outside scope. Or the variables can be acquired by indexing `v`.
Jacobian is passed by modifying the H array of numpy matrices.
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
gf = cf.linearize(v)
gf_b = bf.linearize(v)
J_cf, b_cf = gf.jacobian()
J_bf, b_bf = gf_b.jacobian()
np.testing.assert_allclose(J_cf, J_bf)
np.testing.assert_allclose(b_cf, b_bf)
def error_point_landmarks( measurement: np.ndarray,
calibration: gtsam.Cal3_S2,
sems: List,
this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
x_key = this.keys()[0]
l_key = this.keys()[1]
# print(sems)
# s_key = this.keys()[2]
pose = values.atPose3(x_key)
pw = values.atPoint3(l_key)
# conf = values.atDouble(s_key)
cam = PinholeCameraCal3_S2(pose, calibration)
# pos = cam.project(point)
q = cam.pose().transformTo(pw)
pn = cam.Project(q)
pi = cam.calibration().uncalibrate(pn)
error = pi - measurement
if jacobians is not None:
# print(gtsam.DefaultKeyFormatter(x_key), gtsam.DefaultKeyFormatter(l_key))
s = sems[l_key-L0]
d = 1. / q[2]
Rt = cam.pose().rotation().transpose()
Dpose = cal_Dpose(pn, d)
Dpoint = cal_Dpoint(pn, d, Rt)
Dpi_pn = np.array([[320., 0], [0, 320.]], dtype=float)
Dpose = np.matmul(Dpi_pn, Dpose)
Dpoint = np.matmul(Dpi_pn, Dpoint)
jacobians[0] = Dpose
jacobians[1] = Dpoint
error*=s
# pos = cam.project(point)
return error
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
self.fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
self.optimal_values = Values()
self.optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
self.initial_values = Values()
self.initial_values.insert(KEY1, x0)
self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
# optimize parameters
self.ordering = Ordering()
self.ordering.push_back(KEY1)
def plot_incremental_trajectory(fignum: int,
values: Values,
start: int = 0,
scale: float = 1,
marginals: Optional[Marginals] = None,
time_interval: float = 0.0) -> None:
"""
Incrementally plot a complete 3D trajectory using poses in `values`.
Args:
fignum: Integer representing the figure number to use for plotting.
values: Values dict containing the poses.
start: Starting index to start plotting from.
scale: Value to scale the poses by.
marginals: Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
time_interval: Time in seconds to pause between each rendering.
Used to create animation effect.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
poses = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(poses.keys())
for key in keys[start:]:
if values.exists(key):
pose_i = values.atPose3(key)
plot_pose3(fignum, pose_i, scale)
# Update the plot space to encompass all plotted points
axes.autoscale()
# Set the 3 axes equal
set_axes_equal(fignum)
# Pause for a fixed amount of seconds
plt.pause(time_interval)
def test_jacobian(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)
expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
# print(f"{this = },\n{v = },\n{len(H) = }")
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
if len(H) > 0:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = ge.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
v = Values()
v.insert(0, gT1)
v.insert(1, gT2)
bf = gtsam.BetweenFactorPose2(0, 1, Pose2(0, 0, 0), noise_model)
gf = cf.linearize(v)
gf_b = bf.linearize(v)
J_cf, b_cf = gf.jacobian()
J_bf, b_bf = gf_b.jacobian()
np.testing.assert_allclose(J_cf, J_bf)
np.testing.assert_allclose(b_cf, b_bf)
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> float:
"""Landmark Factor error function
:param measurement: Landmark measurement, to be filled with `partial`
:param this: gtsam.CustomFactor handle
:param values: gtsam.Values
:param jacobians: Optional list of Jacobians
:return: the unwhitened error
"""
key = this.keys()[0]
pos = values.atVector(key)
error = pos - measurement
if jacobians is not None:
jacobians[0] = I
return error
def estimate_poses_given_rot(factors: gtsam.BetweenFactorPose3s,
rotations: gtsam.Values,
d: int = 3):
""" Estimate Poses from measurements, given rotations. From SfmProblem in shonan.
Arguments:
factors -- data structure with many BetweenFactorPose3 factors
rotations {Values} -- Estimated rotations
Returns:
Values -- Estimated Poses
"""
I_d = np.eye(d)
def R(j):
return rotations.atRot3(j) if d == 3 else rotations.atRot2(j)
def pose(R, t):
return gtsam.Pose3(R, t) if d == 3 else gtsam.Pose2(R, t)
graph = gtsam.GaussianFactorGraph()
model = gtsam.noiseModel.Unit.Create(d)
# Add a factor anchoring t_0
graph.add(0, I_d, np.zeros((d,)), model)
# Add a factor saying t_j - t_i = Ri*t_ij for all edges (i,j)
for factor in factors:
keys = factor.keys()
i, j, Tij = keys[0], keys[1], factor.measured()
measured = R(i).rotate(Tij.translation())
graph.add(j, I_d, i, -I_d, measured, model)
# Solve linear system
translations = graph.optimize()
# Convert to Values.
result = gtsam.Values()
for j in range(rotations.size()):
tj = translations.at(j)
result.insert(j, pose(R(j), tj))
return result
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.
vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is
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(), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(), 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 error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]) -> np.ndarray:
"""Minimal error function with no Jacobian"""
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
class TestScenario(GtsamTestCase):
"""Do trivial test with three optimizer variants."""
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
self.fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
self.optimal_values = Values()
self.optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
self.initial_values = Values()
self.initial_values.insert(KEY1, x0)
self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
# optimize parameters
self.ordering = Ordering()
self.ordering.push_back(KEY1)
def test_gauss_newton(self):
gnParams = GaussNewtonParams()
gnParams.setOrdering(self.ordering)
actual = GaussNewtonOptimizer(self.fg, self.initial_values,
gnParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_levenberg_marquardt(self):
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(self.ordering)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_levenberg_marquardt_pcg(self):
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_dogleg(self):
dlParams = DoglegParams()
dlParams.setOrdering(self.ordering)
actual = DoglegOptimizer(self.fg, self.initial_values,
dlParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_graduated_non_convexity(self):
gncParams = GncLMParams()
actual = GncLMOptimizer(self.fg, self.initial_values,
gncParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_iteration_hook(self):
# set up iteration hook to track some testable values
iteration_count = 0
final_error = 0
final_values = None
def iteration_hook(iter, error_before, error_after):
nonlocal iteration_count, final_error, final_values
iteration_count = iter
final_error = error_after
final_values = optimizer.values()
# optimize
params = LevenbergMarquardtParams.CeresDefaults()
params.setOrdering(self.ordering)
params.iterationHook = iteration_hook
optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
params)
actual = optimizer.optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
self.gtsamAssertEquals(final_values, actual)
self.assertEqual(self.fg.error(actual), final_error)
self.assertEqual(optimizer.iterations(), iteration_count)
def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]) -> np.ndarray:
key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose)
return error
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
Each variable in the system (poses and landmarks) must be identified with a unique key.
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
Here we will use Symbols
In GTSAM, measurement functions are represented as 'factors'. Several common factors
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
Here we will use Projection factors to model the camera's landmark observations.
Also, we will initialize the robot at some location using a Prior factor.
When the factors are created, we will add them to a Factor Graph. As the factors we are using
are nonlinear factors, we will need a Nonlinear Factor Graph.
Finally, once all of the factors have been added to our factor graph, we will want to
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
to update the linearization point. This happens repeatedly until the solver converges
to a consistent set of variable values. This requires us to specify an initial guess
for each variable, held in a Values container.
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1 * np.random.randn(6, 1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1 * np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
