引言

本文主要是针对《Least-Squares Fitting of Two 3-D Point Sets》论文SVD求解旋转矩阵中推导过程中使用到的一些线性代数相关的内容做一些说明，具体算法实现不是很复杂，也有很多其他博客可以参考，比如参考中第2条SVD分解求变换矩阵(C++版)。

论文整体算法

这里直接贴论文中算法截图了（只截取了部分截图），算法过程这一部分不是本文重点，之后有需要再详细补充。本文主要是为了解决《Least-Squares Fitting of Two 3-D Point Sets》第3部分B.Derivation中的一些困惑，为什么SVD可以求解旋转矩阵，为什么使用了SVD。
论文是用最小二乘的方式求解R和T，通过误差方程最小来获得最优的R和T。

• 第一部分引出对于两组输入的点集$p_i,p_i^{\prime }$，如何用最小二乘求解R和T；
• 第二部分，主要是讲解如何将旋转矩阵R和平移T分离，先求解R，再求解T。假设$\hat{R}$和$\hat{T}$是最小二乘的解，那$p_i^{\prime \prime }\triangleq \hat{R}{p}_i+\hat{T}$表示点集$p_i$变换后的结果。变换后的$p_i^{\prime \prime }$和$p_i^{\prime }$的质心理论上应该是相等的。通过减去质心把两组输入点集的平移T的影响去除，只剩下R。减去质心相当于两组点集都是以原点作为质心，也可以理解为将两组点集平移到了原点（这里的理解不一定对）。最终推导出公式(9)，利用公式(9)可以求解得到R。
• 第三部分，就是讲解如果用SVD求解旋转矩阵R，本文的重点就是第三部分中B小节B.Derivation；

论文中一些标识说明，其中$p_i,p_i^{\prime }$是输入的两组三维点集，$p_i$是$3\times 1$的列向量。$p$表示输入点集$p_i$的质心，$p^{\prime }$表示输入点集$p_i^{\prime }$的质心，$p^{\prime \prime }$表示输入点集$p_i^{\prime \prime }$的质心。

SVD求解R

论文中III部分B小节是SVD可以计算得到R的推导，本文主要是针对该部分。

• 式(14)的理解。推导过程前面部分还可以看懂，到式(14)就不明白是为什么了。由于线性代数已经差不多都忘记了，因此最初看没有看懂，查阅了线性代数SVD相关资料，最初是完全按照矩阵乘法(不考虑求和)展开$q_i^{\prime t}R{q}_i$和$Trace(R{q}_i{q}_i^{\prime t})$，两个结果相等，但是这个逻辑是不正确的，是有了等式后面的东西才能推到出来，假如只有$q_i^{\prime t}R{q}_i$怎么得到$Trace(R{q}_i{q}_i^{\prime t})$，在搜索矩阵的迹的时候，看到了二次型（参考中的4），发现$q_i^{\prime t}R{q}_i$其实就是二次型的形式。
  二次型的迹有如下性质：
  $$x^{\prime }Ax=tr(x^{\prime }Ax)=tr(Ax{x}^{\prime })=tr(x{x}^{\prime }A)$$
  至此式(14)已经明白是怎么得到了，就是利用了二次型的迹相关的一些性质，这里就由求式(9)的${\sum }^2$的最小转变成求式(14)F的最大值，即矩阵迹的最大值。
  

图1. 式(14)

• 式14中的迹在什么情况下最大。此时用到了SVD分解，当R也即下图2中的X为$V{U}^t$时，可以得到最大的迹，那么为什么这个迹最大呢？里面说是由论文中的一个Lemma得到,如图3所示。

图2. SVD分解求迹最大

在Lemma中证明当一个矩阵式正定矩阵时，任何正交矩阵乘该矩阵的迹都比矩阵自身的迹要小。而$XH=V\Lambda V^t$就是一个对称的正定矩阵。而对称正定矩阵（正定对称矩阵）可以分解为$A{A}^T$，或者$A^{T}A$，例如使用Cholesky分解，可以分解为一个下三角矩阵和一个上三角矩阵的乘积。可知$XH$满足Lemma，可以证明在$X=V{U}^t$时，$Trace(XH)$最大，即R=X时$Trace(RH)$最大，由此可以由SVD分解得到R。

图3. Lemma

知识点

参考

1. K. S. Arun, T. S. Huang and S. D. Blostein, “Least-Squares Fitting of Two 3-D Point Sets,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. PAMI-9, no. 5, pp. 698-700, Sept. 1987, doi: 10.1109/TPAMI.1987.4767965.
2. SVD分解求变换矩阵(C++版)
3. O. Sorkine, M. Rabinovich, Least-squares rigid motion using svd. Technical notes, 1–6 (2009).
4. 矩阵运算_迹的相关性质
5. 怎样将一个对称正定矩阵分解成两个相同矩阵的乘积
6. https://www.zhihu.com/question/335611169