数值方法解出的近似旋转矩阵转化为正交的标准旋转矩阵 旋转矩阵标准化 正交矩阵

参考链接：https://zhuanlan.zhihu.com/p/104735380

参考视频课：https://www.bilibili.com/video/BV1ax411R7Hd?p=20

在谭平老师（SFU）在浙江大学的计算机视觉的公开课（强烈推荐讲的很好，链接如上）中听到了这个问题，在这里记录一下：

在求解涉及旋转的优化问题过程中，通过解线性方程组或者采用优化方法得到的旋转矩阵的数值解，往往没有考虑到正交约束，会得到一个存在误差的近似旋转矩阵，也就是说这个矩阵并不是一个严格正交的标准正交矩阵，不满足。因此，需要将矩阵近似为最接近的正交矩阵。

处理方法：采用SVD分解近似旋转矩阵，然后将奇异值矩阵用单位阵替代即可，得到

Matlab简单的示例如下

% Standard Rotation Matrix
R = [0.0115185,  0.999321,    0.0351458;
     -0.998647,   0.00971046,  0.051195;
      0.0508185, -0.0356881,   0.998074];
% S is identity matrix
[U, S, V] = svd(R);

% Rotation Matrix Solved without Constraint (Error)
R1 = [0.01,  0.99,   0.03;
     -0.99,   0.01,  0.05;
      0.05, -0.03,   0.99];

% S1 is not identity matrix
[U1, S1, V1] = svd(R1);


% Approximate Matrix
R2 = U1 * V1';

% S2 is identity matrix
[U2, S2, V2] = svd(R2);

这里还要注意一个问题，之前也遇到过。处理完以后得到的 R 不一定就是旋转矩阵，因为旋转矩阵是行列式值为1的正交矩阵，而正交矩阵的行列式值为。因此还要检验 R 的行列式是否为1，如果是-1，则得到了一个Reflection，需要将奇异值矩阵（单位阵）的最后一列的1置为-1，这样得到的 R 就是旋转矩阵了，这个处理方式参考了https://zhuanlan.zhihu.com/p/104735380，以及Arun 的经典文章 Least-Squares Fitting of Two 3-D Point Sets，这种情况在求解两组三维点相对位姿时有时会出现。