Skip to article frontmatterSkip to article content

Frobenius Norm Factors

This header defines factors that operate directly on the entries of rotation matrices (Rot3 or generally SO(n)) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.

These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard BetweenFactor or PriorFactor on rotations.

  • FrobeniusPrior<T>: Penalizes the Frobenius norm difference between a variable rotation T and a fixed target matrix M. Error is ∣∣T−M∣∣F2||T - M||_F^2.
  • FrobeniusFactor<T>: Penalizes the Frobenius norm difference between two variable rotations T1 and T2. Error is ∣∣T1−T2∣∣F2||T1 - T2||_F^2.
  • FrobeniusBetweenFactor<T>: Penalizes the Frobenius norm difference between the predicted rotation T2 and the expected rotation T1 * T12_measured. Error is ∣∣T1⋅T12−T2∣∣F2||T1 \cdot T_{12} - T2||_F^2. Note: The noise models for these factors operate on the vectorized rotation matrix (e.g., 9 elements for Rot3). The helper function ConvertNoiseModel attempts to convert standard rotation noise models (like those for BetweenFactor<Rot3>) into an appropriate isotropic noise model for the Frobenius factor. It expects the input noise model to be isotropic.

Open In Colab

import gtsam
import numpy as np
from gtsam import Rot3, Pose3, Values
from gtsam import FrobeniusPriorRot3, FrobeniusFactorRot3, FrobeniusBetweenFactorRot3
from gtsam import symbol_shorthand

X = symbol_shorthand.X
R = symbol_shorthand.R # Using 'R' for Rot3 keys

1. FrobeniusPrior<Rot3>

Constrains a Rot3 variable R(0) to be close to a target matrix M in the Frobenius norm sense.

target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)
key = R(0)

# Create a standard isotropic noise model for rotation (3 dimensional)
rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)

# Convert it for the Frobenius factor (9 dimensional)
frobenius_noise_model_prior = gtsam.noiseModel.Isotropic.Sigma(9, 0.01) # Or use ConvertNoiseModel

prior_fro = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model_prior)
prior_fro.print("FrobeniusPriorRot3: ")

# Evaluate error
values = Values()
values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation
error_prior = prior_fro.error(values)
print(f"\nFrobeniusPrior error (vectorized matrix diff): {error_prior}")
FrobeniusPriorRot3:   keys = { r0 }
  noise model: unit (9) 

FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05

1. FrobeniusFactor<Rot3>

Constrains two Rot3 variables R(0) and R(1) to be close to each other in the Frobenius norm sense.

values = Values()

key1 = R(0)
key2 = R(1)
# Use same noise model dimension (9)
frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)

factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)
factor_fro.print("\nFrobeniusFactorRot3: ")

# Evaluate error
values.insert(key1, Rot3.Yaw(0.11))
values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0
error_factor = factor_fro.error(values)
print(f"\nFrobeniusFactor error (vectorized matrix diff): {error_factor}")

FrobeniusFactorRot3:   keys = { r0 r1 }
  noise model: unit (9) 

FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05

3. FrobeniusBetweenFactor<Rot3>

Acts like BetweenFactor<Rot3> but minimizes ∣∣R1⋅R12−R2∣∣F2||R_1 \cdot R_{12} - R_2||_F^2 instead of using the Log map error.

measured_R12 = Rot3.Yaw(0.005)
# Use same noise model dimension (9)
frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)

between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)
between_fro.print("\nFrobeniusBetweenFactorRot3: ")

# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))
error_between = between_fro.error(values)
print(f"\nFrobeniusBetweenFactor error: {error_between}")

FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<class gtsam::Rot3>(r0,r1)
  T12:  [
	0.999988, -0.00499998, 0;
	0.00499998, 0.999988, 0;
	0, 0, 1
]
  noise model: unit (9) 

FrobeniusBetweenFactor error: 1.925929944387236e-34
GTSAM Docs
EssentialMatrixFactor Variants
GTSAM Docs
GeneralSFMFactor