Compute Fréchet Mean of Rotation Matrices on SO(k)

Calculates the Fréchet mean (geometric mean) of a list of k x k rotation matrices. The Fréchet mean is the rotation matrix R_bar that minimizes the sum of squared geodesic distances to all rotation matrices in the list: R_bar = argmin_R sum_i d(R, R_i)^2. This implementation uses an iterative algorithm involving logarithmic and exponential maps on SO(k).

Usage

.so_logm_closed_form(R)

Arguments

R_list

A list of k x k matrices, expected to be rotation matrices (in SO(k) or O(k)). The function attempts to work with matrices close to SO(k).

k_dim

Integer, the dimension of the rotation matrices (e.g., 3 for SO(3)). If NULL (default), it's inferred from the first valid matrix in `R_list`.

max_iter

Integer, the maximum number of iterations for the algorithm (default: 50).

tol

Numeric, the tolerance for convergence. The algorithm stops when the Frobenius norm of the mean tangent vector is below this tolerance (default: 1e-7).

initial_mean

Optional. A k x k matrix to use as the initial estimate for the mean. If NULL, the first valid matrix in `R_list` is used, or an identity matrix if none are valid.

project_to_SOk

Logical. If TRUE (default), after each update, the new mean estimate is projected to the closest matrix in SO(k) using SVD. This helps maintain numerical stability and ensures the result is indeed in SO(k).

Value

A k x k matrix representing the Fréchet mean of the input rotation matrices. Returns an identity matrix of appropriate dimension if `R_list` is empty, contains no valid matrices, or if `k_dim` cannot be determined.

Examples

# Example for SO(3)
if (requireNamespace("expm", quietly = TRUE)) {
  R1 <- matrix(c(1,0,0, 0,cos(0.1),-sin(0.1), 0,sin(0.1),cos(0.1)), 3, 3)
  R2 <- matrix(c(cos(0.2),-sin(0.2),0, sin(0.2),cos(0.2),0, 0,0,1), 3, 3)
  R_list_so3 <- list(R1, R2)
  # frechet_mean_so_k(R_list_so3) # k_dim inferred
  # frechet_mean_so_k(R_list_so3, k_dim = 3)
}

# Example for SO(2)
if (requireNamespace("expm", quietly = TRUE)) {
  theta1 <- pi/4
  R1_so2 <- matrix(c(cos(theta1), -sin(theta1), sin(theta1), cos(theta1)), 2, 2)
  theta2 <- pi/3
  R2_so2 <- matrix(c(cos(theta2), -sin(theta2), sin(theta2), cos(theta2)), 2, 2)
  # frechet_mean_so_k(list(R1_so2, R2_so2))
}