PoseTranslationPrior

import gtsam
import numpy as np
from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPrior3D
from gtsam import symbol_shorthand

X = symbol_shorthand.X

PoseTranslationPrior<POSE> is a unary factor that applies a prior constraint only to the translation component of a POSE variable (e.g., Pose2 or Pose3). It ignores the rotation component of the pose variable during error calculation. The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for Point2 or Point3).

This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement).

Creating a PoseTranslationPrior¶

Provide the key of the pose variable, the measured prior translation (Point3 for Pose3, Point2 for Pose2), and a noise model defined on the translation space dimension (e.g., 3 for Point3).

pose_key = X(0)
measured_translation = Point3(10.0, 20.0, 5.0) # Prior belief about position

# Noise model on translation (3 dimensions for Point3)
translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev

# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3
factor = PoseTranslationPrior3D(pose_key, measured_translation, translation_noise)
factor.print("PoseTranslationPrior: ")

# Alternative constructor: extract translation from a full Pose3 prior
full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored
factor_from_pose = PoseTranslationPrior3D(pose_key, full_pose_prior, translation_noise)
# factor_from_pose.print("\nFrom Pose Prior: ") # Should be identical

PoseTranslationPrior: PoseTranslationPrior  keys = { x0 }
isotropic dim=3 sigma=0.5
Measured Translation[
	10;
	20;
	5
]

Evaluating the Error¶

The error depends only on the translation part of the Pose3 value in the Values object.

values = Values()

# Pose with correct translation but different rotation
pose_val1 = Pose3(Rot3.Roll(0.5), measured_translation)
values.insert(pose_key, pose_val1)
error1 = factor.error(values)
print(f"Error with correct translation: {error1} (Should be near zero)")

# Pose with incorrect translation
pose_val2 = Pose3(Rot3.Roll(0.5), Point3(10.2, 19.9, 5.1))
values.update(pose_key, pose_val2)
error2 = factor.error(values)
print(f"Error with incorrect translation: {error2} (Should be non-zero)")

# Check that rotation change doesn't affect unwhitened error
pose_val3 = Pose3(Rot3.Yaw(1.0), Point3(10.2, 19.9, 5.1))
values.update(pose_key, pose_val3)
error3 = factor.error(values)
unwhitened2 = factor.unwhitenedError(values)
print(f"Error with different rotation: {error3} (Should reflect Jacobian change)")
print(f"Unwhitened error with different rotation: {unwhitened2} (Should be [0.2, -0.1, 0.1])")
# assert np.allclose(error2, error3) # Whitened error WILL change due to Jacobian
assert np.allclose(unwhitened2, np.array([0.2, -0.1, 0.1]))

Error with correct translation: 0.0 (Should be near zero)
Error with incorrect translation: 0.11999999999999986 (Should be non-zero)
Error with different rotation: 0.11999999999999986 (Should reflect Jacobian change)
Unwhitened error with different rotation: [ 0.2 -0.1  0.1] (Should be [0.2, -0.1, 0.1])