Skip to article frontmatterSkip to article content

Karcher Mean Factors

The KarcherMeanFactor.h header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values. The Karcher mean Rˉ\bar{R} of a set of rotations {Ri}\{R_i\} is the rotation that minimizes the sum of squared geodesic distances on the manifold:

Rˉ=arg⁡min⁡R∑id2(R,Ri)=arg⁡min⁡R∑i∣∣Log(Ri−1R)∣∣2\bar{R} = \arg \min_R \sum_i d^2(R, R_i) = \arg \min_R \sum_i || \text{Log}(R_i^{-1} R) ||^2
(1)

Functions/Classes:

  • FindKarcherMean: Computes the Karcher mean of a std::vector of rotations (or other suitable manifold type T). It solves the minimization problem above using a small internal optimization.
  • KarcherMeanFactor<T>: A factor that enforces a constraint related to the Karcher mean. It does not constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the sum of tangent space updates applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables.

Open In Colab

import gtsam
import numpy as np
from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values
from gtsam.symbol_shorthand import R

1. FindKarcherMean

Computes the Karcher mean of a list of rotations.

# Create a list of Rot3 objects
rotations = gtsam.Rot3Vector()
rotations.append(Rot3.Yaw(0.1))
rotations.append(Rot3.Yaw(0.15))
rotations.append(Rot3.Yaw(0.05))
rotations.append(Rot3.Yaw(0.12))

# Compute the Karcher mean
karcher_mean = FindKarcherMeanRot3(rotations)

print("Input Rotations (Yaw angles):")
for r in rotations: print(f"  {r.yaw():.3f}")

print(f"\nComputed Karcher Mean (Yaw angle): {karcher_mean.yaw():.3f}")
# Note: For yaw rotations, the Karcher mean yaw is close to the arithmetic mean (0.105)
Input Rotations (Yaw angles):
  0.100
  0.150
  0.050
  0.120

Computed Karcher Mean (Yaw angle): 0.105

2. KarcherMeanFactor<Rot3>

Creates a factor that constrains the rotational average of a set of Rot3 variables. It acts as a soft gauge constraint. When linearized, it yields a Jacobian factor where each block corresponding to a variable is βI3x3\sqrt{\beta} I_{3x3}, and the error vector is zero. The beta parameter (optional, defaults to 1) controls the strength (precision) of the constraint.

keys = [R(0), R(1), R(2)]
beta = 100.0 # Strength of the constraint

k_factor = KarcherMeanFactorRot3(keys, 3, beta)
k_factor.print("KarcherMeanFactorRot3: ")

# Linearization example
values = Values()
values.insert(R(0), Rot3.Yaw(0.1))
values.insert(R(1), Rot3.Yaw(0.2))
values.insert(R(2), Rot3.Yaw(0.3))

linearized_factor = k_factor.linearize(values)

# Check the Jacobian blocks (should be sqrt(beta)*Identity)
sqrt_beta = np.sqrt(beta)
A = linearized_factor.getA()
assert A.shape == (3, 9), f"Unexpected shape for A: {A.shape}"
A0 = A[:, :3]
A1 = A[:, 3:6]
A2 = A[:, 6:9]
b = linearized_factor.getb()

print(f"sqrt(beta): {sqrt_beta}")
print(f"\nJacobian for R(0):\n{A0}")
print(f"Jacobian for R(1):\n{A1}")
print(f"Jacobian for R(2):\n{A2}")
print(f"Error vector b:\n{b}")
KarcherMeanFactorRot3:   keys = { r0 r1 r2 }
sqrt(beta): 10.0

Jacobian for R(0):
[[10.  0.  0.]
 [ 0. 10.  0.]
 [ 0.  0. 10.]]
Jacobian for R(1):
[[10.  0.  0.]
 [ 0. 10.  0.]
 [ 0.  0. 10.]]
Jacobian for R(2):
[[10.  0.  0.]
 [ 0. 10.  0.]
 [ 0.  0. 10.]]
Error vector b:
[0. 0. 0.]
GTSAM Docs
InitializePose3
GTSAM Docs
LAGO (Linear Approximation for Graph Optimization)