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||T_1 - T_2||_F^2.

  • FrobeniusBetweenFactorNL<T>: Penalizes the Frobenius norm difference between the predicted measurement T2^{-1} * T1 and the actual measurement T12_measured. Error is ∣∣I−T2−1T1⋅T~12∣∣F2||I - T_2^{-1} T_1 \cdot \tilde T_{12}||_F^2. Use this with any Matrix Lie group.

  • FrobeniusBetweenFactor<T>: Penalizes the Frobenius norm difference between the predicted rotation T2 and the expected rotation T1 * T12_measured. Error is ∣∣T1⋅T~12−T2∣∣F2||T_1 \cdot \tilde T_{12} - T_2||_F^2. Uses this - easier to optimize factor - if the norm is invariant to multiplying with T2, as is the case for Rot3, Pose3, etc, but not, say, Similarity3.

The helper function function ConvertModel ensures the noise model used by the various Frobenius factors has the correct dimension. You can either provide:

  • an isotropic noise model of the manifold dimension of the type T (e.g., 3 for Rot3)

  • any noise model of the full vectorized matrix dimension (N*N, where N is the matrix size, e.g., 9 for Rot3).

  • If you provide an isotropic model of the manifold dimension, ConvertModel will automatically convert it to the correct dimension for the Frobenius factor.

Open In Colab

import gtsam
from gtsam import (SL4, FrobeniusBetweenFactorNLSL4,
                   FrobeniusBetweenFactorRot3, FrobeniusFactorRot3,
                   FrobeniusPriorRot3, Rot3, Values, 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)

# Option 1: Provide an isotropic noise model of the manifold dimension (3 for Rot3)
rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)
# ConvertModel will expand this to 9D for Frobenius factors

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

# Option 2: Provide any noise model of the full vectorized dimension (9 for Rot3)
frobenius_noise_model = gtsam.noiseModel.Isotropic.Sigma(9, 0.01)
prior_fro_full = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model)
prior_fro_full.print("FrobeniusPriorRot3 (full 9D noise): ")

# Evaluate error
values = Values()
values.insert(key, Rot3.Yaw(0.11))
error_prior = prior_fro.error(values)
print(f"\nFrobeniusPrior error (vectorized matrix diff): {error_prior}")
FrobeniusPriorRot3:   keys = { r0 }
isotropic dim=9 sigma=0.01
FrobeniusPriorRot3 (full 9D noise):   keys = { r0 }
isotropic dim=9 sigma=0.01

FrobeniusPrior error (vectorized matrix diff): 0.9999916666944463

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)
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 }
isotropic dim=9 sigma=0.02

FrobeniusFactor error (vectorized matrix diff): 0.062499869791775416

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: FrobeniusBetweenFactorNL<gtsam::Rot3>(r0,r1)
  T12:  [
	0.999988, -0.00499998, 0;
	0.00499998, 0.999988, 0;
	0, 0, 1
]
isotropic dim=9 sigma=0.005

FrobeniusBetweenFactor error: 7.703719777548943e-30

4. FrobeniusBetweenFactorNL<SL4>

Acts like BetweenFactor<SL4> but minimizes ∣∣T2−1T1⋅T12−I∣∣F2||T_2^{-1} T_1 \cdot T_{12} - I||_F^2, for use with groups that do not satisfy the invariance property needed to use FrobeniusBetweenFactor.

# Create two SL4 elements (4x4 special linear group matrices)
sl4_1 = SL4.Expmap([0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09, 0.1, 0.11, 0.12, 0.13, 0.14, 0.15])
sl4_2 = SL4.Expmap([0.015, 0.025, 0.035, 0.045, 0.055, 0.065, 0.075, 0.085, 0.095, 0.105, 0.115, 0.125, 0.135, 0.145, 0.155])
measured_sl4 = SL4.Expmap([0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005])

# Create keys for the variables
key_sl4_1 = X(0)
key_sl4_2 = X(1)

# Noise model for 16D (4x4 matrix)
sl4_noise_model = gtsam.noiseModel.Isotropic.Sigma(16, 0.01)

# Construct the FrobeniusBetweenFactorNL for SL4
fbf_sl4 = FrobeniusBetweenFactorNLSL4(key_sl4_1, key_sl4_2, measured_sl4, sl4_noise_model)

# Insert values into Values container
values_sl4 = Values()
values_sl4.insert(key_sl4_1, sl4_1)
values_sl4.insert(key_sl4_2, sl4_2)

# Print factor and evaluate error
fbf_sl4.print("\nFrobeniusBetweenFactorNL<SL4>: ")
error_sl4 = fbf_sl4.error(values_sl4)
print(f"\nFrobeniusBetweenFactorNL<SL4> error: {error_sl4}")

FrobeniusBetweenFactorNL<SL4>: FrobeniusBetweenFactorNL<gtsam::SL4>(x0,x1)
  T12:    1.00505 0.00503769 0.00503769 0.00502517
0.00503769    1.00004 0.00502515 0.00501265
0.00503769 0.00502515    1.00004 0.00501265
0.00502517 0.00501265 0.00501265    0.99505
isotropic dim=16 sigma=0.01

FrobeniusBetweenFactorNL<SL4> error: 0.003941370929502147
GTSAM Docs
EssentialMatrixFactor Variants
GTSAM Docs
GeneralSFMFactor