PoseRotationPrior - GTSAM Docs

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

X = symbol_shorthand.X

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

Error: $\text{Log}(\text{measured}^{-1} \cdot \text{pose.rotation}())$

This is useful when you have information about the absolute orientation of a pose but little or no information about its translation.

Creating a PoseRotationPrior¶

Provide the key of the pose variable, the measured prior rotation (Rot3 for Pose3, Rot2 for Pose2), and a noise model defined on the rotation manifold’s dimension (e.g., 3 for Rot3).

pose_key = X(0)
measured_rotation = Rot3.Yaw(np.pi / 4) # Prior belief about orientation

# Noise model on rotation (3 dimensions for Rot3)
rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev

# Factor type includes the Pose type, e.g. PoseRotationPrior3D
factor = PoseRotationPrior3D(pose_key, measured_rotation, rotation_noise)
factor.print("PoseRotationPrior: ")

# Alternative constructor: extract rotation from a full Pose3 prior
full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored
factor_from_pose = PoseRotationPrior3D(pose_key, full_pose_prior, rotation_noise)
# factor_from_pose.print("\nFrom Pose Prior: ") # Should be identical

PoseRotationPrior: PoseRotationPrior  keys = { x0 }
isotropic dim=3 sigma=0.05
Measured Rotation [
	0.707107, -0.707107, 0;
	0.707107, 0.707107, 0;
	0, 0, 1
]

Evaluating the Error¶

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

values = Values()

# Pose with correct rotation but different translation
pose_val1 = Pose3(measured_rotation, Point3(1, 2, 3))
values.insert(pose_key, pose_val1)
error1 = factor.error(values)
print(f"Error with correct rotation: {error1} (Should be near zero)")

# Pose with incorrect rotation
pose_val2 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(1, 2, 3))
values.update(pose_key, pose_val2)
error2 = factor.error(values)
print(f"Error with incorrect rotation: {error2} (Should be non-zero)")

# Check that translation change doesn't affect error
pose_val3 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(100, 200, 300))
values.update(pose_key, pose_val3)
error3 = factor.error(values)
print(f"Error with different translation: {error3} (Should be same as error2)")
assert np.allclose(error2, error3)

Error with correct rotation: 0.0 (Should be near zero)
Error with incorrect rotation: 1.9999999999999951 (Should be non-zero)
Error with different translation: 1.9999999999999951 (Should be same as error2)