Rotate Factors

This header defines factors that constrain an unknown rotation (Rot3) based on how it transforms either full rotations or directions.

RotateFactor: Relates two incremental rotations measured in different frames using an unknown rotation relating the frames. If $Z = R \cdot P \cdot R^T$ , where $P = e^{[p]}$ and $Z = e^{[z]}$ are measured incremental rotations (expressed as angular velocity vectors $p$ and $z$ ), this factor constrains the unknown rotation $R$ such that $p = R z$ . The error is $Rz - p$ .
RotateDirectionsFactor: Relates two directions (unit vectors, Unit3) measured in different frames using an unknown rotation $R$ relating the frames. If $p_{world} = R \cdot z_{body}$ , this factor constrains $R$ . The error is the angular difference between the predicted $R z_{body}$ and the measured $p_{world}$ .

import gtsam
import numpy as np
from gtsam import Rot3, Point3, Unit3, Values
from gtsam import RotateFactor, RotateDirectionsFactor
from gtsam import symbol_shorthand

R = symbol_shorthand.R # For Rotation key

1. `RotateFactor`¶

Constraints an unknown Rot3 based on corresponding incremental rotation measurements $P$ (predicted/world) and $Z$ (measured/body).

# Assume P and Z are small incremental rotations
P = Rot3.Expmap(np.array([0.01, 0.02, 0.03]))
Z = Rot3.Expmap(np.array([0.03, -0.01, 0.02]))
rot_key = R(0)
noise1 = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)

factor1 = RotateFactor(rot_key, P, Z, noise1)
factor1.print("RotateFactor: ")

# Evaluate error
values = Values()
# Ground truth R would satisfy P = R*Z
# R = P * Z.inverse()
gt_R = P * (Z.inverse())
values.insert(rot_key, gt_R)
error1_gt = factor1.error(values)
print(f"\nError at ground truth R: {error1_gt}")

noisy_R = gt_R * Rot3.Expmap(np.array([0.001, -0.001, 0.001]))
values.update(rot_key, noisy_R)
error1_noisy = factor1.error(values)
print(f"Error at noisy R: {error1_noisy}")

RotateFactor:   keys = { r0 }
isotropic dim=3 sigma=0.001
RotateFactor:]
p: 0.01 0.02 0.03
z:  0.03 -0.01  0.02

Error at ground truth R: 700.0
Error at noisy R: 699.2869223608442

2. `RotateDirectionsFactor`¶

Constraints an unknown Rot3 based on corresponding direction measurements $p_{world}$ (predicted/world) and $z_{body}$ (measured/body).

p_world = Unit3(Point3(1, 0, 0)) # Direction in world frame
z_body = Unit3(Point3(0, 1, 0))  # Corresponding direction in body frame
noise2 = gtsam.noiseModel.Isotropic.Sigma(2, 0.01) # Noise on 2D tangent space

factor2 = RotateDirectionsFactor(rot_key, p_world, z_body, noise2)
factor2.print("RotateDirectionsFactor: ")

# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)
# This corresponds to a -90 deg yaw
gt_R_dir = Rot3.Yaw(-np.pi/2)
print(f"\nCheck: gt_R * z_body = \n{gt_R_dir.rotate(z_body)}")

# Evaluate error
values_dir = Values()
values_dir.insert(rot_key, gt_R_dir)
error2_gt = factor2.error(values_dir)
print(f"Error at ground truth R: {error2_gt}")

noisy_R_dir = gt_R_dir * Rot3.Expmap(np.array([0.01, 0, 0.01]))
values_dir.update(rot_key, noisy_R_dir)
error2_noisy = factor2.error(values_dir)
print(f"Error at noisy R: {error2_noisy}")

RotateDirectionsFactor:   keys = { r0 }
isotropic dim=2 sigma=0.01
RotateDirectionsFactor:
p:1
0
0
z:0
1
0

Check: gt_R * z_body = 
:          1
6.12323e-17
          0

Error at ground truth R: 1.874699728327322e-29
Error at noisy R: 0.999933335111092

1. RotateFactor¶

2. RotateDirectionsFactor¶

1. `RotateFactor`¶

2. `RotateDirectionsFactor`¶