前言
3D点云配准，在无人驾驶领域有一定分量。自己转行之后，开始接触这个领域。点云配准的一个非常难的点就是如何快速查找到比较可靠的匹配对。本文这里不展开讨论，在这里，仅叙述在找到匹配对后，如何反算出对应的刚体变换参数（注意，这里仅考虑一个刚体变换）。
问题
假如有一个3D点\(p_1\)，在经过如下的刚体变换后得到新的位置点\(p_2\)
(1)$$p_2=sRp_1+t $$
公式(1)中的\(s\)为缩放系数,\(R\)为旋转矩阵,\(t\)为偏移。
现在我们需要求解的问题是,已知有\(n\)组\((p_1,p_2)_i\),它们每一组的匹配对应的权重为\(w_i\)，现在需要反求解公式(1)当中的变换参数。
求解
对于求解一个优化问题的未知参数，可以按常规方法构建一个最小二乘问题
(2)$$\begin{align*}
min E &=\sum_{i=1}^{n}\left \| w_i(p_{2i} – (sRp_{1i}+t)) \right \|_2^2 \\
&=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – sRp_{1i}-t \right \|_2^2 \\
&=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1}) + \bar{p_2} -sR \bar{p_1}- t \right \|_2^2\\
&=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1})\right \|_2^2 +
\\ &\quad \sum_{i=1}^{n}w_i^2\left \| \bar{p_2} -sR \bar{p_1}- t \right \|_2^2 +
\\ &\quad 2\sum_{i=1}^{n}w_i^2 (p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1}))^T(\bar{p_2} -sR \bar{p_1}- t)
\end{align*}$$
上面公式里的\(\bar{p_1}\)、\(\bar{p_2}\)为引入的消去变量。从公式(2)可以发现最小化目标函数由3部分组成，由于前两项均为非负数，而第3项无法保证值为非负，因此，可以考虑将第3项变成一个与\(s\)、\(R\)、\(t\)无关的常量，进而达到先消去第3部分的表达式目的，这里令它等于0，即
(3)$$\begin{align*}
0 &=2\sum_{i=1}^{n}w_i^2 (p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1}))^T(\bar{p_2} -sR \bar{p_1}- t)\\
&=2[\sum_{i=1}^{n}w_i^2 (p_{2i} – \bar{p_2})- sR\sum_{i=1}^{n}w_i^2 (p_{1i}- \bar{p_1})]^T(\bar{p_2} -sR \bar{p_1}- t)\\
&=2[(\sum_{i=1}^{n}w_i^2p_{2i}-\bar{p_2}\sum_{i=1}^{n}w_i^2)-sR(\sum_{i=1}^{n}w_i^2p_{1i}-\bar{p_1}\sum_{i=1}^{n}w_i^2)]^T(\bar{p_2} -sR \bar{p_1}- t)
\end{align*}$$
要满足(3)式，很明显，对于任意的\(s\)、\(R\)、\(t\)均使得公式(3)满足时(相当于公式(3)中的值与\(s\)、\(R\)、\(t\)无关)，\(\bar{p_1}\)、\(\bar{p_2}\)值必然满足如下条件
(4)$$\sum_{i=1}^{n}w_i^2p_{2i}-\bar{p_2}\sum_{i=1}^{n}w_i^2=0,\sum_{i=1}^{n}w_i^2p_{1i}-\bar{p_1}\sum_{i=1}^{n}w_i^2=0$$
即
(5)$$\bar{p_1}=\sum_{i=1}^{n}w_i^2p_{1i} / \sum_{i=1}^{n}w_i^2,\bar{p_2}=\sum_{i=1}^{n}w_i^2p_{2i} / \sum_{i=1}^{n}w_i^2$$
当我们构造的\(\bar{p_1}\)、\(\bar{p_2}\)满足公式(5)时，这时(2)式的最小二乘的目标函数就变成了如下形式
(6)$$\begin{align*}
min E &=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1})\right \|_2^2 + \sum_{i=1}^{n}w_i^2\left \| \bar{p_2} -sR \bar{p_1}- t \right \|_2^2\\
&=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1})\right \|_2^2+ \left \| \bar{p_2} -sR \bar{p_1}- t \right \|_2^2 \sum_{i=1}^{n}w_i^2
\end{align*}$$
从公式(6)可以发现，目标最小值由两部分组成，第一部分只和\(R\)、\(s\)有关，而第二项当得到\(R\)、\(s\)后，可以马上确定出使第2项为0的最佳\(t\)，因此求解这类问题可以先优化求解第一部分，然后再根据第二部分确定出最佳\(t\)。
注意：这里之所以可以分成两部分求解，因为公式的第二部分总可以找到一个\(t\)使得其表达式值为0，对于其它可类似拆分的优化问题，如果不能确保第另一项为0，则不可采用这种方式求解，而需要采用来回迭代求解的方式进行试解。
根据公式(6)则展开第一部分有
(7)$$\begin{align*}
min E_1 &=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}- sR(p_{1i}- \bar{p_1})\right \|_2^2 \\
&= \sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}\right \|_2^2 + \sum_{i=1}^{n}w_i^2\left \| sR(p_{1i}- \bar{p_1})\right \|_2^2 – 2\sum_{i=1}^{n}w_i^2(p_{2i} – \bar{p_2})^T sR(p_{1i}- \bar{p_1}) \\
&= \sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}\right \|_2^2 + \sum_{i=1}^{n}w_i^2 s^2(p_{1i}- \bar{p_1})^TR^TR(p_{1i}- \bar{p_1}) – 2s\sum_{i=1}^{n}w_i^2(p_{2i} – \bar{p_2})^T R(p_{1i}- \bar{p_1})\\
&=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}\right \|_2^2 + \sum_{i=1}^{n}w_i^2 s^2(p_{1i}- \bar{p_1})^T(p_{1i}- \bar{p_1}) – 2s\sum_{i=1}^{n}tr(w_i^2R(p_{1i}- \bar{p_1})(p_{2i}- \bar{p_2})^T)\\
&=\sum_{i=1}^{n}w_i^2\left \| p_{2i} – \bar{p_2}\right \|_2^2 + s^2\sum_{i=1}^{n}w_i^2\left \| p_{1i} – \bar{p_1}\right \|_2^2 – 2s \times tr(R \sum_{i=1}^{n}w_i^2(p_{1i}- \bar{p_1})(p_{2i}- \bar{p_2})^T)
\end{align*}$$
公式(7)可以看出，这里求最小值又可以分成三部分，而第一部分为常数，因此实际最小化就变成了后面两部分。
这里设置一个如下的中间变量\(H\)
(8)$$ H=\sum_{i=1}^{n}w_i^2(p_{1i}- \bar{p_1})(p_{2i}- \bar{p_2})^T$$
以及变量\(a\)
(9)$$ a = \sum_{i=1}^{n}w_i^2\left \| p_{1i} – \bar{p_1}\right \|_2^2$$
则(7)式最终等价求如下函数的最小形式
(10)$$\begin{align*}
min E_1'&= as^2 – 2s \times tr(RH)\\
&= a(s-\frac{tr(RH)}{a})^2-\frac{tr(RH)^2}{a}\\
\end{align*}$$
分析公式(10)可知，要使整体表达式最小，则前面二次方的部分要最小，后面被减部分要最大，从表达式可知，总存在一个\(s\)使得二次方表达式部分取得最小的0值，这时可以得到
(11)$$s=\frac{tr(RH)}{a}$$
而对于(10)式，由于要使得最小，则后面的负数项则必须最大，即(10)要求得最小值，等价于(12)算式求得最大值
(12)$$max E= tr(RH)^2$$
如果尺度\(s\)有大小范围限制，要求最小值，其等价于(13)算式求得最大值(本文这里仅把\(s\)当做一个纯优化求解的可正可负的变量)。
(13)$$max E= sign(s)tr(RH)$$
接下来，进一步对\(H\)进行奇异值分解，即
(14)$$H=USV^T$$
则求迹时可变成如下等式
(15)$$\begin{align*}
tr(RH) &= tr(RUSV^T) \\
&= tr(V^TRUS)\\
&= tr(AS)
\end{align*}$$
上面的\(A=V^TRU\)，很明显\(A\)为一个正交矩阵，由于\(S\)由奇异值分解时求解实对称矩阵特征值而来，因此对角矩阵\(S\)上的元素均不小于0，这里假设\(S\)对角上的元素为\(\sigma_0 \)、\(\sigma_1 \)、\(\sigma_2\)，而\(A\)上的对角元素为\(a_{00}\)、\(a_{11}\)、\(a_{22}\)，由于\(A\)的正交性，这几个元素取值范围在[-1,1]之间，则最终(15)式可变成
(16)$$tr(RH)=tr(AS)=a_{00}\sigma_0 +a_{11}\sigma_1 +a_{22}\sigma_2$$
从上面可知，(12)算式要求得最大值，则公式(16)的迹必须取极大或者极小值。由于上面\(\sigma_0 \)、\(\sigma_1 \)、\(\sigma_2\)均不小于0，因此(16)式要取得极值，则必然\(a_{00}\)、\(a_{11}\)、\(a_{22}\)同号时方可取极值。明显地，当\(a_{00}\)、\(a_{11}\)、\(a_{22}\)均取最大值1或者最小值-1时，这时可以使得\(tr(RH) \)的值取得最大，这时正好有一个唯一与之对应的矩阵\(\pm E\)，由于取正负单位阵得到的结果使得(12)得到的最大值一致，因此其目前有2组可能解，即
(17)$$ V^TRU = A = \pm E \Leftrightarrow R= \pm VU^T$$
根据旋转矩阵的性质，其行列式为1，因此可以从(17)的两组解中选择出满足旋转矩阵性质的解，即
(18)$$ R=\left | UV \right | VU^T$$
结合公式(11)，则最终，当得到\(R\)、\(s\)后，可根据(6)式得到\(t\)
(19)$$t = \bar{p_2} -sR \bar{p_1}$$
需要注意，上面的求解过程认为\(s\)为没有限制的参数，当\(s \neq 0\)且为大小限制的参数时，则优化目标函数由(12)式变化成了(13)式。根据上面的方法可以得到最佳\(R\)的结果如下
(20)$$R=sign(s)VU^T$$
对于(20)的结果，如果\(\left | R \right | =1\)，则这时求得的旋转矩阵则为最终解。
当(20)式得到的\(\left | R \right | \neq 1\)时，明显这个结果不是真实的解，这时不能使用(18)算式进行修正，因为这样求解时得到的结果完全与优化目标结果相反。
当限制\(s>0\)时，通过公式(10)可以发现，要求最优值，则要求
(21)$$s=\frac{tr(RH)}{a}>0 \Rightarrow tr(RH)>0$$
然后求解满足如下条件的最大值
(22)$$maxE'=tr(RH)$$
而直接指定缩放系数\(s\)的值，并且这个\(s>0\)时，根据(10)式可知最终也是求解(22)的最大值，因为这时的\(s\)已知，因此这种情况没有(21)的限制。
对于(22)式，最终都会归结到(16)式，由于其元素值本身的性质，在(16)式中，总能找到大于0的结果，因此最终的优化结果均能满足(21)式的要求，因此不管是直接给出一个大于0的缩放系数，还是限制待求解的缩放系数为正，最终优化目标均变成(22)式，根据文献当(16)式中的\(A\)满足如下条件时，可以取得最佳值
(23)$$A=\begin{bmatrix}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & -1
\end{bmatrix}$$
这时，\(R\)结果如下
(24)$$ R=VAU^T$$
不失一般性，当限制缩放系数为正数时，则最终的旋转矩阵结果如下
(25)$$ R=VZU^T$$
其中\(Z\)取值如下
(26)$$Z=\begin{bmatrix}
1 & 0 & 0\\
0 & 1& 0\\
0&0 & \left | UV \right |
\end{bmatrix}$$
步骤
①\(s\)为无限制参数
最终求解带权重的变换矩阵的步骤如下：
第1步：根据公式(5)计算\(\bar{p_1}\)、\(\bar{p_2}\)
第2步：根据公式(8)计算\(H\)
第3步：根据公式(14)对\(H\)进行奇异值分解
第4步：根据公式(18)求得旋转矩阵\(R\)
第5步：根据公式(9)计算\(a\)，如果忽略尺度，则跳过本步骤
第6步：根据公式(11)计算尺度\(s\)，如果忽略尺度，则跳过本步骤
第7步：根据公式(19)计算\(t\)
②\(s\)限制为正数
第1步：根据公式(5)计算\(\bar{p_1}\)、\(\bar{p_2}\)
第2步：根据公式(8)计算\(H\)
第3步：根据公式(14)对\(H\)进行奇异值分解
第4步：根据公式(25)求得旋转矩阵\(R\)
第5步：根据公式(9)计算\(a\)，如果忽略尺度，则跳过本步骤
第6步：根据公式(11)计算尺度\(s\)，如果忽略尺度，则跳过本步骤
第7步：根据公式(19)计算\(t\)
PS：可以发现，权重系数均是以\(w_i^2\)形式出现，如果我们设置的权重系数\(w_i\)本身为实际权重的平方项，则这时可以直接使用\(w_i\)代替\(w_i^2\)