从旋转矩阵估计更加精确的旋转矩阵

     当我们获得一个粗糙的旋转矩阵，想得到一个准确的旋转矩阵，但是与初步估计出来的旋转矩阵的F范数误差，参考张正友文章，用svd分解对矩阵R1进行分解，R1=USV'，分解后用UV'来表征最佳的旋转矩阵。

python中scipy的Rotation里面，旧版本的as_matrix写作as_dcm

上述算法写python代码如下：

u_, s_, vh = np.linalg.svd(rot_mat.copy(), full_matrices=True)
rot_mat= u_ @ np.diag(np.sign(s_)) @ vh

https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/tr98-71.pdf