This notebook compares two conventional GTSAM formulations of the same 20-pose localization problem. Both optimize poses , both reuse the loaded BetweenFactorPose3 measurements, and both treat the loaded world landmarks as known.
The first graph retains the dataset’s bearing-range factors and fixes every with a hard prior. The second replaces each bearing-range factor with KnownLandmarkFactorPose3, so each landmark becomes fixed factor data rather than an optimized variable.
See the KnownLandmarkFactor API notebook for both pose conventions, wrapper names, and the exact QCQP distinction.
import numpy as np
import gtsam
import plotly.graph_objects as goLoad conventional and data¶
The g2o file stores Cartesian landmark observations as bearing and range. For measured bearing , range , and Unit3 tangent basis , we recover
The covariance conversion is exact at the measurement linearization point. Cartesian and bearing-range Gaussian models are not globally identical, so their optimized answers can differ slightly.
data_file = gtsam.findExampleDataFile(
"known_landmark_localization_20.g2o"
)
source_graph, dataset_values = gtsam.readG2o(data_file, is3D=True)
pose_keys, landmark_keys = set(), set()
for index in range(source_graph.size()):
factor = source_graph.at(index)
keys = tuple(factor.keys())
if isinstance(factor, gtsam.BearingRangeFactor3D):
k, l = keys
pose_keys.add(k)
landmark_keys.add(l)
elif isinstance(factor, gtsam.BetweenFactorPose3):
pose_keys.update(keys)
else:
raise TypeError(f"Unexpected factor type: {type(factor).__name__}")
pose_keys, landmark_keys = sorted(pose_keys), sorted(landmark_keys)
print(f"loaded factors: {source_graph.size()}")
print(f"loaded poses: {len(pose_keys)}")
print(f"loaded landmarks: {len(landmark_keys)}")loaded factors: 99
loaded poses: 20
loaded landmarks: 4
Build the two conventional graphs¶
KnownLandmarkFactorPose3 follows the standard GTSAM convention: its state is , and its prediction is
In code, this prediction is wTk.transformTo(wL). The bearing-range graph keeps variables but pins each one to its loaded value. The known-landmark graph contains only variables. Every relative-pose factor is reused unchanged in both graphs.
bearing_range_graph = gtsam.NonlinearFactorGraph()
known_landmark_graph = gtsam.NonlinearFactorGraph()
kPCovariances = []
for index in range(source_graph.size()):
factor = source_graph.at(index)
keys = tuple(factor.keys())
if isinstance(factor, gtsam.BearingRangeFactor3D):
k, l = keys
bearing_range_graph.add(factor)
measured_kBearingRange = factor.measured()
kRange = measured_kBearingRange.range()
kBearing = measured_kBearingRange.bearing()
measured_kP = kRange * kBearing.unitVector()
DkP_dbearingRange = np.column_stack(
(kRange * kBearing.basis(), kBearing.unitVector())
)
kPCovariance = (
DkP_dbearingRange
@ factor.noiseModel().covariance()
@ DkP_dbearingRange.T
)
known_landmark_graph.add(
gtsam.KnownLandmarkFactorPose3(
k,
dataset_values.atPoint3(l),
measured_kP,
gtsam.noiseModel.Gaussian.Covariance(kPCovariance),
)
)
kPCovariances.append(kPCovariance)
elif isinstance(factor, gtsam.BetweenFactorPose3):
bearing_range_graph.add(factor)
known_landmark_graph.add(factor)
hard_point_model = gtsam.noiseModel.Constrained.All(3)
for l in landmark_keys:
bearing_range_graph.add(
gtsam.PriorFactorPoint3(l, dataset_values.atPoint3(l), hard_point_model)
)
eigenvalues = np.linalg.eigvalsh(kPCovariances[0])
anisotropicity = np.sqrt(eigenvalues[-1] / eigenvalues[0])
print(f"bearing-range graph factors: {bearing_range_graph.size()}")
print(f"known-landmark graph factors: {known_landmark_graph.size()}")
print(f"sqrt covariance condition number: {anisotropicity:.1f}")
assert np.isclose(anisotropicity, 10.0)bearing-range graph factors: 103
known-landmark graph factors: 99
sqrt covariance condition number: 10.0
Optimize both formulations¶
Both graphs start from the same perturbed values. The bearing-range branch also carries the exact loaded values required by its binary factors and hard priors.
perturbation = np.array([0.02, -0.015, 0.01, 0.08, -0.05, 0.06])
ground_truth_wTks = gtsam.Values()
initial_wTks = gtsam.Values()
initial_bearing_range = gtsam.Values(dataset_values)
for index, k in enumerate(pose_keys):
wTk = dataset_values.atPose3(k)
initial_wTk = wTk.retract((index + 1) * perturbation)
ground_truth_wTks.insert(k, wTk)
initial_wTks.insert(k, initial_wTk)
initial_bearing_range.update(k, initial_wTk)
parameters = gtsam.GaussNewtonParams()
parameters.setMaxIterations(100)
parameters.setRelativeErrorTol(1e-12)
bearing_range_result = gtsam.GaussNewtonOptimizer(
bearing_range_graph, initial_bearing_range, parameters
).optimize()
known_landmark_result_wTks = gtsam.GaussNewtonOptimizer(
known_landmark_graph, initial_wTks, parameters
).optimize()
def pose_errors(actual_wTks):
return np.array([
np.linalg.norm(
dataset_values.atPose3(k).localCoordinates(actual_wTks.atPose3(k))
)
for k in pose_keys
])
bearing_range_errors = pose_errors(bearing_range_result)
known_landmark_errors = pose_errors(known_landmark_result_wTks)
result_differences = np.array([
np.linalg.norm(
bearing_range_result.atPose3(k).localCoordinates(
known_landmark_result_wTks.atPose3(k)
)
)
for k in pose_keys
])
bearing_range_initial_error = bearing_range_graph.error(initial_bearing_range)
bearing_range_final_error = bearing_range_graph.error(bearing_range_result)
known_landmark_initial_error = known_landmark_graph.error(initial_wTks)
known_landmark_final_error = known_landmark_graph.error(
known_landmark_result_wTks
)
print(
f"bearing-range: initial={bearing_range_initial_error:.3f}, "
f"final={bearing_range_final_error:.3f}, "
f"max pose error={bearing_range_errors.max():.4f} m"
)
print(
f"known landmark: initial={known_landmark_initial_error:.3f}, "
f"final={known_landmark_final_error:.3f}, "
f"max pose error={known_landmark_errors.max():.4f} m"
)
print(f"maximum difference between results: {result_differences.max():.6f}")
assert bearing_range_final_error < bearing_range_initial_error
assert known_landmark_final_error < known_landmark_initial_error
assert bearing_range_errors.max() < 0.05
assert known_landmark_errors.max() < 0.05
assert result_differences.max() < 0.001bearing-range: initial=1049464.261, final=108.696, max pose error=0.0228 m
known landmark: initial=1327741.839, final=109.066, max pose error=0.0229 m
maximum difference between results: 0.000632
def world_translations(wTks):
return np.vstack([wTks.atPose3(k).translation() for k in pose_keys])
ground_truth_wPs = world_translations(ground_truth_wTks)
initial_wPs = world_translations(initial_wTks)
bearing_range_wPs = world_translations(bearing_range_result)
known_landmark_wPs = world_translations(known_landmark_result_wTks)
wLs = np.vstack([dataset_values.atPoint3(l) for l in landmark_keys])
fig = go.Figure()
fig.add_scatter(
x=initial_wPs[:, 0], y=initial_wPs[:, 1], mode="lines+markers",
line={"dash": "dash"}, name="initial wTk",
)
fig.add_scatter(
x=ground_truth_wPs[:, 0], y=ground_truth_wPs[:, 1],
mode="lines+markers", line={"color": "black"}, name="ground truth wTk",
)
fig.add_scatter(
x=bearing_range_wPs[:, 0], y=bearing_range_wPs[:, 1],
mode="lines+markers", name="bearing-range result",
)
fig.add_scatter(
x=known_landmark_wPs[:, 0], y=known_landmark_wPs[:, 1],
mode="lines+markers", marker={"symbol": "x"},
name="known-landmark result",
)
fig.add_scatter(
x=wLs[:, 0], y=wLs[:, 1], mode="markers",
marker={"symbol": "star", "size": 12}, name="known wL",
)
fig.update_layout(
title="Conventional localization with known landmarks",
xaxis_title="world x [m]", yaxis_title="world y [m]",
template="plotly_white", width=750, height=520,
)
fig.update_yaxes(scaleanchor="x", scaleratio=1)
fig.show()Reading the comparison¶
Both formulations recover nearly the same conventional trajectory. Their small difference is expected: the Cartesian covariance is the bearing-range covariance pushed forward at the measured bearing and range, but the two nonlinear Gaussian likelihoods are not globally identical.