Computes the geodesic distance on SO(k) between two k x k rotation matrices, representing the angle of rotation needed to align one to the other. The result is given in degrees.
Arguments
- R_est
A k x k estimated rotation matrix (should be orthogonal with determinant +1).
- R_true
A k x k true rotation matrix (should be orthogonal with determinant +1).
- method
Character string, the method to use. Default is "geodesic". Currently, "geodesic" uses the matrix logarithm. A fallback to a simpler trace-based formula (most accurate for SO(3)) is used if `expm::logm` fails or the `expm` package is not installed.
Value
The misalignment angle in degrees. Returns 0 for identical matrices. Returns NA if dimensions are mismatched or matrices are not square.
Details
The primary method ("geodesic") calculates the angle as: `||logm(R_true^T R_est)||_F / sqrt(2)`, where `logm` is the matrix logarithm and `||.||_F` is the Frobenius norm. This is a standard geodesic distance on the special orthogonal group SO(k).
If the `expm` package is not available, or if `expm::logm` fails (e.g., due to numerical issues if matrices are not perfectly orthogonal), the function falls back to a simpler formula derived from the trace: `acos((trace(R_true^T R_est) - 1) / 2)`. This fallback formula is exact for SO(3) and provides a dissimilarity measure for other k, being 0 for perfect alignment.
Examples
if (requireNamespace("expm", quietly = TRUE)) {
R1 <- diag(3)
theta <- pi/4
R2 <- matrix(c(cos(theta), -sin(theta), 0,
sin(theta), cos(theta), 0,
0, 0, 1), nrow=3, byrow=TRUE)
misalign_deg(R2, R1) # Should be 45 degrees
# Example for k=2
R_true_2d <- matrix(c(1,0,0,1),2,2)
angle_rad_2d <- pi/6 # 30 degrees
R_est_2d <- matrix(c(cos(angle_rad_2d), -sin(angle_rad_2d),
sin(angle_rad_2d), cos(angle_rad_2d)), 2, 2)
misalign_deg(R_est_2d, R_true_2d)
}
#> [1] 30