01 Mar 2025

SO(3) Logarithm

AI Chat Series

[ Y-2025   SO3   AI_Assistant_Programming   ]

本文是AI Chat系列文章的第2篇，主要介绍实现SO(3)的对数运算。

Introduction

有一段SO(3)的对数操作的代码，如下所示：

/* Logrithm of a Rotation Matrix */
template <typename T>
Matrix<T, 3, 1> Log(const Matrix<T, 3, 3>& R) {
    const T tr = R.trace();
    T theta = (tr > 3.0 - 1e-6 || tr < -1.0 + 1e-6) ? 0.0 : std::acos(0.5 * (tr - 1));

    Matrix<T, 3, 1> K(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
    return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
}

我想让AI帮我检查一下，是否有潜在的Issues

Ask the AI

Me: this function is used to calculate the tangent of the Rotation manifold, so please check its validity

AI: This function calculates the logarithm map of a rotation matrix (SO(3) -> so(3)), converting a rotation matrix to its corresponding rotation vector (axis-angle representation). Let me analyze its validity:

  1. Mathematical Background:
    • For a rotation matrix R ∈ SO(3), its logarithm gives a skew-symmetric matrix in so(3)
    • The formula used is based on Rodrigues’ formula in reverse
    • The result is a 3×1 vector ω where [ω]ₓ is the skew-symmetric matrix
  2. Code Analysis: 
     // Trace is used to find rotation angle
 const T tr = R.trace();
 // Calculate rotation angle theta
 T theta = (tr > 3.0 - 1e-6 || tr < -1.0 + 1e-6) ? 0.0 : std::acos(0.5 * (tr - 1));

 // Extract the skew-symmetric part
 wlt::Matrix<T, 3, 1> K(R(2, 1) - R(1, 2),  // [R - R^T]_23
                     R(0, 2) - R(2, 0),    // [R - R^T]_31
                     R(1, 0) - R(0, 1));   // [R - R^T]_12

 // Scale based on angle
 return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
  3. Validity Check:
    • ✅ Trace formula is correct: tr(R) = 1 + 2cos(θ)
    • ✅ Special case handling when θ ≈ 0 (identity rotation)
    • ✅ Extraction of skew-symmetric part is correct
    • ✅ Scaling factor θ/(2sin(θ)) is correct for non-zero angles
  4. Potential Issues:
    • ⚠️ The threshold 1e-6 for trace comparison might be too tight for some applications
    • ⚠️ No handling for the case when θ = π (180° rotation)
    • ⚠️ Could have numerical stability issues near θ = π

Here’s an improved version:

template <typename T>
wlt::Matrix<T, 3, 1> Log(const wlt::Matrix<T, 3, 3>& R) {
    const T tr = R.trace();
    const T cos_theta = (tr - 1) * 0.5;
    
    // Clamp cos_theta to [-1,1] to handle numerical errors
    T theta = std::acos(std::max(T(-1), std::min(T(1), cos_theta)));
    
    // Extract skew-symmetric part
    wlt::Matrix<T, 3, 1> K(R(2, 1) - R(1, 2),
                          R(0, 2) - R(2, 0),
                          R(1, 0) - R(0, 1));
    
    if (std::abs(theta) < T(1e-10)) {
        // Near identity rotation
        return T(0.5) * K;
    } else if (std::abs(theta - M_PI) < T(1e-10)) {
        // 180-degree rotation case
        // Need to extract rotation axis from R+I
        wlt::Matrix<T, 3, 1> axis;
        T max_diag = R(0,0) + 1;
        int max_idx = 0;
        for (int i = 1; i < 3; ++i) {
            if (R(i,i) + 1 > max_diag) {
                max_diag = R(i,i) + 1;
                max_idx = i;
            }
        }
        axis = wlt::Matrix<T, 3, 1>(R(max_idx,0), R(max_idx,1), R(max_idx,2));
        axis += wlt::Matrix<T, 3, 1>::Unit(max_idx);
        axis.normalize();
        return theta * axis;
    } else {
        // General case
        return (T(0.5) * theta / std::sin(theta)) * K;
    }
}

The improvements include:

  1. Better handling of numerical stability near identity
  2. Added handling for 180-degree rotations
  3. Clamping of cos_theta to prevent acos domain errors
  4. More robust threshold checks

This implementation should handle all cases in SO(3) more robustly while maintaining the mathematical correctness of the logarithm map.

Me: please give me more details about the case when 180-degree rotation

AI: Let me explain the 180-degree rotation case in detail:

When θ = π (180°), we have special issues because:

  1. sin(θ) = 0, making the formula θ/(2sin(θ)) undefined
  2. The trace = -1, making it hard to determine the rotation axis

Here’s a detailed analysis:

template <typename T>
wlt::Matrix<T, 3, 1> Log(const wlt::Matrix<T, 3, 3>& R) {
    const T tr = R.trace();
    const T cos_theta = (tr - 1) * 0.5;  // For 180°, cos_theta = -1
    
    if (std::abs(cos_theta + 1) < T(1e-10)) {  // Check if it's close to 180°
        // For 180° rotation:
        // R = I + 2[k]_x^2, where k is the rotation axis
        // R + I = 2kk^T
        // Therefore, the column with largest diagonal entry in (R + I)
        // gives us the rotation axis
        
        T max_diag = R(0,0) + 1;  // Diagonal elements of R + I
        int max_idx = 0;
        
        // Find the largest diagonal element
        for (int i = 1; i < 3; ++i) {
            if (R(i,i) + 1 > max_diag) {
                max_diag = R(i,i) + 1;
                max_idx = i;
            }
        }
        
        // Extract rotation axis from the corresponding column
        wlt::Matrix<T, 3, 1> axis;
        switch (max_idx) {
            case 0:
                axis = wlt::Matrix<T, 3, 1>(
                    std::sqrt((R(0,0) + 1) * 0.5),
                    (R(0,1) + R(1,0)) * 0.25 / std::sqrt((R(0,0) + 1) * 0.5),
                    (R(0,2) + R(2,0)) * 0.25 / std::sqrt((R(0,0) + 1) * 0.5)
                );
                break;
            case 1:
                axis = wlt::Matrix<T, 3, 1>(
                    (R(1,0) + R(0,1)) * 0.25 / std::sqrt((R(1,1) + 1) * 0.5),
                    std::sqrt((R(1,1) + 1) * 0.5),
                    (R(1,2) + R(2,1)) * 0.25 / std::sqrt((R(1,1) + 1) * 0.5)
                );
                break;
            case 2:
                axis = wlt::Matrix<T, 3, 1>(
                    (R(2,0) + R(0,2)) * 0.25 / std::sqrt((R(2,2) + 1) * 0.5),
                    (R(2,1) + R(1,2)) * 0.25 / std::sqrt((R(2,2) + 1) * 0.5),
                    std::sqrt((R(2,2) + 1) * 0.5)
                );
                break;
        }
        
        // Return π times the rotation axis
        return M_PI * axis;
    }
    
    // ... rest of the code for non-180° cases ...
}

Mathematical Background:

  1. For a 180° rotation around axis k (unit vector): \(R = I + 2[k]_×^2 = I - 2(k k^T - I) = 2k k^T - I\)

  2. Therefore: \(R + I = 2k k^T\)

  3. Properties of this case:

    • Trace(R) = -1
    • R is symmetric
    • Eigenvalues are {-1, -1, 1}
    • Eigenvector with eigenvalue 1 is the rotation axis

Example:

// Test case for 180° rotation around x-axis
wlt::Matrix<double, 3, 3> R_180_x;
R_180_x << 1, 0,  0,
           0, -1, 0,
           0, 0, -1;

wlt::Matrix<double, 3, 1> log_R = Log(R_180_x);
// Expected result: [π, 0, 0]^T

Key Points:

  1. We can’t use the standard formula θ/(2sin(θ)) * K because sin(π) = 0
  2. The rotation axis is the eigenvector corresponding to eigenvalue 1
  3. We can find this axis by looking at R + I
  4. The column of R + I with the largest diagonal entry gives us the most numerically stable way to compute the axis

This improved handling of 180° rotations makes the logarithm map complete and numerically stable for all rotation matrices in SO(3).

Me: 这里补充一下，180°旋转的时候，通过上面的方法，\(R-R^T\)，是没法恢复出k的，因为\(R-R^T\)是0矩阵，所以需要通过\(R+I\)来恢复出k

下面继续…

Me: I’m confused with the switch case, it’s a bit different with the above logic “axis += wlt::Matrix<T, 3, 1>::Unit(max_idx);”

Let me explain the difference between these two approaches for finding the rotation axis in a 180° rotation:

  1. The simpler approach you mentioned: 
    axis = wlt::Matrix<T, 3, 1>(R(max_idx,0), R(max_idx,1), R(max_idx,2));
axis += wlt::Matrix<T, 3, 1>::Unit(max_idx);
axis.normalize();
  2. The switch case approach in the code: 
    case 0:  // When x-axis has largest diagonal
 axis = wlt::Matrix<T, 3, 1>(
     std::sqrt((R(0,0) + 1) * 0.5),           // = sqrt(2k_x^2) = |k_x|
     (R(0,1) + R(1,0)) * 0.25 / sqrt(...),    // = k_x*k_y / |k_x|
     (R(0,2) + R(2,0)) * 0.25 / sqrt(...)     // = k_x*k_z / |k_x|
 );

The switch case approach is mathematically more precise because:

  1. For a 180° rotation, \(R + I = 2kk^T\) where \(k\) is the unit rotation axis
  2. The diagonal elements of \(2kk^T\) are \(2k_x^2, 2k_y^2, 2k_z^2\)
  3. The off-diagonal elements are \(2k_xk_y, 2k_xk_z, 2k_yk_z\)

This gives us a way to directly compute the components of k without needing normalization.

However, both approaches should give the same result, with the simpler approach being more intuitive but potentially less numerically stable.

Me: This line (R(0,1) + R(1,0)) * 0.25 / sqrt(...), // = k_x*k_y / |k_x|, why not use the same method as obtaining the k_x to obtain the k_y

AI: Ah, you raise a good point! Let me explain why we can’t use the same method for all components:

For a 180° rotation around axis k = [k_x, k_y, k_z]:

  1. When we choose the x-component (case 0), we know: 
    R(0,0) + 1 = 2k_x^2        // Diagonal term
R(0,1) + R(1,0) = 2k_x*k_y // Off-diagonal term

  2. We could try to get k_y from \(R(1,1) + 1 = 2k_y^2\), but in this case (when x-component is largest), this value would be close to zero and numerically unstable.

  3. Instead, we use: 
    k_x = ±sqrt((R(0,0) + 1)/2)  // Get magnitude from diagonal
k_y = (R(0,1) + R(1,0))/(4*k_x) // Use off-diagonal for sign and magnitude

You’re right that theoretically we could use sqrt((R(1,1) + 1)/2) to get k_y directly, but numerically it’s more stable to use the off-diagonal elements when we’re working with the non-dominant components.

This is why we use different formulas for the dominant component (using sqrt) versus the other components (using ratios).