旋转矩阵与四元数双向转换失效问题排查及解决

在Python中实现旋转矩阵与四元数的双向转换时，遇到部分矩阵转换失效的问题。已知单个四元数对应多个旋转矩阵，但往返转换后应保持相同旋转效果，不过部分矩阵不满足这一预期。

实现代码 #

基于文献实现的转换函数如下：

import torch

def batch_quaternions_to_rotation_matrix(w, x, y, z):
    # 归一化四元数
    norm = torch.sqrt(w**2 + x**2 + y**2 + z**2)
    w = w / norm
    x = x / norm
    y = y / norm
    z = z / norm
    
    # 计算旋转矩阵
    R = torch.zeros((w.shape[0], 3, 3))
    
    R[:, 0, 0] = w**2 + x**2 - y**2 - z**2
    R[:, 0, 1] = 2 * (x*y - w*z)
    R[:, 0, 2] = 2 * (x*z + w*y)
    
    R[:, 1, 0] = 2 * (x*y + w*z)
    R[:, 1, 1] = w**2 - x**2 + y**2 - z**2
    R[:, 1, 2] = 2 * (y*z - w*x)
    
    R[:, 2, 0] = 2 * (x*z - w*y)
    R[:, 2, 1] = 2 * (y*z + w*x)
    R[:, 2, 2] = w**2 - x**2 - y**2 + z**2
    
    return torch.transpose(R, 1, 2)

def batch_rotation_matrix_to_quaternions(R):
    batch_size = R.shape[0]
    K = torch.zeros((batch_size, 4, 4), dtype=R.dtype)

    K[:, 0, 0] = R[:, 0, 0] - R[:, 1, 1] - R[:, 2, 2]
    K[:, 0, 1] = R[:, 1, 0] + R[:, 0, 1]
    K[:, 0, 2] = R[:, 2, 0] + R[:, 0, 2]
    K[:, 0, 3] = R[:, 1, 2] - R[:, 2, 1]

    K[:, 1, 0] = R[:, 1, 0] + R[:, 0, 1]
    K[:, 1, 1] = R[:, 1, 1] - R[:, 0, 0] - R[:, 2, 2]
    K[:, 1, 2] = R[:, 2, 1] + R[:, 1, 2]
    K[:, 1, 3] = R[:, 2, 0] - R[:, 0, 2]

    K[:, 2, 0] = R[:, 2, 0] + R[:, 0, 2]
    K[:, 2, 1] = R[:, 2, 1] + R[:, 1, 2]
    K[:, 2, 2] = R[:, 2, 2] - R[:, 0, 0] - R[:, 1, 1]
    K[:, 2, 3] = R[:, 0, 1] - R[:, 1, 0]

    K[:, 3, 0] = R[:, 1, 2] - R[:, 2, 1]
    K[:, 3, 1] = R[:, 2, 0] - R[:, 0, 2]
    K[:, 3, 2] = R[:, 0, 1] - R[:, 1, 0]
    K[:, 3, 3] = R[:, 0, 0] + R[:, 1, 1] + R[:, 2, 2]

    K /= 3
    
    eigvals, eigvecs = torch.linalg.eig(K)
    eigvals = eigvals.real
    eigvecs = eigvecs.real

    mask = torch.logical_or(torch.isclose(eigvals,torch.ones(eigvals.shape, dtype=eigvals.dtype)),torch.isclose(eigvals,-torch.ones(eigvals.shape, dtype=eigvals.dtype)))
    indices = torch.where(mask)
    quaternions = eigvecs[indices[0], :, indices[1]]
    return quaternions[:,3], quaternions[:,0], quaternions[:,1], quaternions[:,2]

测试案例 #

测试发现多数正交旋转矩阵转换正常，例如：

tensor([[[ 0.7741, -0.3551,  0.5241],
         [ 0.2347,  0.9298,  0.2834],
         [-0.5880, -0.0964,  0.8031]]])

但部分矩阵转换后返回其负矩阵：
原矩阵：

tensor([[[-0.2643,  0.6644, -0.6991],
         [ 0.6968,  0.6327,  0.3378],
         [-0.6667,  0.3979,  0.6302]]])

转换后结果：

tensor([[[ 0.2643, -0.6644,  0.6991],
         [-0.6968, -0.6327, -0.3378],
         [ 0.6667, -0.3979, -0.6302]]])

问题根源 #

尝试修正四元数符号后无效，进一步排查发现失效矩阵的特征值包含-1而非1。问题在于当前转换函数仅支持行列式为+1的正当旋转矩阵，而失效矩阵是行列式为-1的非正当旋转矩阵（包含反射变换）。

解决方法 #

可通过以下代码将非正当旋转矩阵转换为正当旋转矩阵，保证转换正常：

rotation_matrices[rotation_matrices.det() < 0, :, 0] *= -1

内容的提问来源于stack exchange，提问作者Jusles