2019-05-24

Registration / 0.compute transform

25 分钟读完 (大约 3717 个字)

compute-transform

计算刚体变换矩阵

问题：已知有对应点对(correspondence; point pairs)P、Q集合，求解PQ之间的刚体变换矩阵

参考1: 求解刚体旋转平移关系–配准

参考2: SVD Least-Squares Rigid Motion Using SVD 3D3D算变换矩阵

1.待定系数法

容易得到超定方程组，利用最小二乘法求解超定方程组
由于输入数据的误差，导致算出来不是一个正交的旋转矩阵

每一个一一对应点满足：

$\left( \begin{array}{c} \bar{q}_x \\ \bar{q}_y \\ \bar{q}_z \\ \end{array} \right)_i=\left( \begin{array}{cccc} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{array} \right)\left( \begin{array}{c} \bar{p}_x \\ \bar{p}_y \\ \bar{p}_z \\ \end{array} \right)_i$

对于第 $i$ 个一一对应点得到方程组:

$\left( \begin{array}{cccccccc} \bar{p}_x^i & \bar{p}_y^i &\bar{p}_z^i&0&0&0&0&0&0 \\ 0&0&0&\bar{p}_x^i & \bar{p}_y^i &\bar{p}_z^i&0&0&0 \\ 0&0&0&0&0&0&\bar{p}_x^i & \bar{p}_y^i &\bar{p}_z^i \\ \end{array} \right)\left( \begin{array}{c} r_{11} \\ r_{12} \\ r_{13} \\ r_{21} \\ r_{22} \\ r_{23} \\ r_{31} \\ r_{32} \\ r_{33} \\ \end{array} \right)=\left( \begin{array}{c} \bar{q}_x^i \\ \bar{q}_y^i \\ \bar{q}_z^i \\ \end{array} \right)$

同理将所有一一对应点的方程组合并, 得到超定方程：

$\boldsymbol{A}_{3N\times9}\boldsymbol{r}_{9\times1}=\boldsymbol{b}_{3N\times1}$

利用最小二乘法求解

$(\boldsymbol{A}_{3N\times9}^\text{T}\boldsymbol{A}_{3N\times9})\boldsymbol{r}_{9\times1}=\boldsymbol{A}_{3N\times9}^\text{T}\boldsymbol{b}_{3N\times1}$

得到向量 $\boldsymbol{r}$ , 写为矩阵形式即 $\boldsymbol{R}$ .

缺点: 在有原始数据误差或者计算累积误差情况下, 得到的矩阵 $\boldsymbol{R}$ 不一定严格满足正交矩阵.

2.Levenberg-Marquardt方法求解（非线性）

输入变量从旋转矩阵R转为欧拉角R( , , )。
将非线性问题转化为线性优化。
使用信赖域法求极值。

旋转矩阵 $\boldsymbol{R}$ 具有3个自由度, 可以理解为3个Euler角可确定一个旋转状态, 或一个四元数(范数为1, 自由度为4-1=3)可确定一个旋转状态. 可利用 $\alpha~\beta~\gamma~$ 作为非线性拟合的参数, 旋转矩阵 $\boldsymbol{R}$ 由 $\alpha~\beta~\gamma~$ 唯一确定, 并使得在该参数下误差最小, 即:

$\alpha,~\beta,~\gamma~ = arg\min_{\alpha~\beta~\gamma} \sum_{i=1}^{N}\|{\bar{\boldsymbol{q}}_i-\boldsymbol{R}(\alpha,\beta,\gamma)}\bar{\boldsymbol{p}}_i\|_2$

得到 $\alpha~\beta~\gamma~$ 即得到旋转矩阵 $\boldsymbol{R}$ , , 该方法可保证该矩阵严格为正交矩阵.

缺点: Levenberg-Marquardt方法一方面对该优化问题的初值有一定要求, 另一方面非线性算法的效率较低.

LM算法原理

LM算法，全称为Levenberg-Marquard算法，它可用于解决非线性最小二乘问题，多用于曲线拟合等场合。

它的关键是用模型函数对待估参数向量在其邻域内做线性近似，忽略掉二阶以上的导数项，从而转化为线性最小二乘问题.
它具有收敛速度快等优点。
LM算法属于一种“信赖域法”——所谓的信赖域法，此处稍微解释一下：在最优化算法中，都是要求一个函数的极小值，每一步迭代中，都要求目标函数值是下降的，而信赖域法，顾名思义，就是从初始点开始，先假设一个可以信赖的最大位移，然后在以当前点为中心，以为半径的区域内，通过寻找目标函数的一个近似函数（二次的）的最优点，来求解得到真正的位移。在得到了位移之后，再计算目标函数值，如果其使目标函数值的下降满足了一定条件，那么就说明这个位移是可靠的，则继续按此规则迭代计算下去；如果其不能使目标函数值的下降满足一定的条件，则应减小信赖域的范围，再重新求解。

3.SVD奇异值分解方法（线性）

对协方差3X3矩阵用SVD(奇异值分解)来计算旋转矩阵R

Singular Value Decomposition*

1.计算两个点集的中心（重心）。将两个点集利用中心（重心），平移到原点处。

2.利用两个平移后的点集计算协方差矩阵S

3.用SVD奇异值分解方法对S进行分解获得旋转矩阵S

4.通过两个点集的中心和旋转矩阵S获得平移向量t

总结

方法一(待定系数求解方程组)和方法二(LM优化方法求解)并不是求解刚体旋转矩阵 $\boldsymbol{R}$ 和平移矩阵 $\boldsymbol{t}$ 的最佳方法. 方法三中只需利用SVD分解( ( $3\times3$ 规模数据矩阵)即可得到所需结果, 不仅过程简单而且求解精度很高.

经过一些列推导后, 我们得到最终的结果, 是不是很晕….如果对推导的过程不感兴趣的话, 可以直接看下面的计算步骤:

STEP0: 已知刚性运动前后的对应点分别为 $\boldsymbol{p}_i$ 和 $\boldsymbol{q}_i$ $i=1,2,\cdots N$ ), 均为 $3\times1$ 向量

STEP1: 分别计算 $\boldsymbol{p}_i$ 和 $\boldsymbol{q}_i$ 的重心(求和平均)得到 $\boldsymbol{p}_0$ 和 $\boldsymbol{q}_0$

STEP2: 计算重心平移到原点的数据 $\bar{\boldsymbol{p}}_i = \boldsymbol{p}_i-\boldsymbol{p}_0~,~~~\bar{\boldsymbol{q}}_i = \boldsymbol{q}_i-\boldsymbol{q}_0$

STEP3: 矩阵形式描述 $[~\bar{\boldsymbol{p}}_1~\bar{\boldsymbol{p}}_2~\cdots~\bar{\boldsymbol{p}}_N~]$ 记为 $\boldsymbol{X}_0$ , $[~\bar{\boldsymbol{q}}_1~\bar{\boldsymbol{q}}_2~\cdots~\bar{\boldsymbol{q}}_N~]$ 记为 $\boldsymbol{Y}_0$

STEP4: 计算数据矩阵 $\boldsymbol{X}_0\boldsymbol{Y}_0^\text{T}$ 并作SVD分解 $\boldsymbol{X}_0\boldsymbol{Y}_0^\text{T}=\boldsymbol{U}\boldsymbol{\varLambda}\boldsymbol{V}^\text{T}$

STEP5: 得到旋转矩阵 $\boldsymbol{R}=\boldsymbol{V}\boldsymbol{U}^\text{T}$

STEP6: 得到平移向量 $\boldsymbol{t} = \boldsymbol{q}_0-\boldsymbol{R}\boldsymbol{p}_0$

SVD原理

1.

$\alpha,~\beta,~\gamma~ = arg\min_{\alpha~\beta~\gamma} \sum_{i=1}^{N}\|{\bar{\boldsymbol{q}}_i-\boldsymbol{R}(\alpha,\beta,\gamma)}\bar{\boldsymbol{p}}_i\|_2$

得到 $\alpha~\beta~\gamma~$ 即得到旋转矩阵 $\boldsymbol{R}$ , 该方法可保证该矩阵严格为正交矩阵.

缺点: Levenberg-Marquardt方法一方面对该优化问题的初值有一定要求, 另一方面非线性算法的效率较低.

方法三: Singular Value Decomposition 奇异值分解方法

通过对应点求解旋转矩阵 $\boldsymbol{R}$ 可写为优化问题:

$\boldsymbol{R} = arg\min_{\boldsymbol{R}} \sum_{i=1}^{N}\|{\bar{\boldsymbol{q}}_i-\boldsymbol{R}}\bar{\boldsymbol{p}}_i\|_2^2$

可以将对应点列写为矩阵形式

3行N列

矩阵 $[~\bar{\boldsymbol{p}}_1~\bar{\boldsymbol{p}}_2~\cdots~\bar{\boldsymbol{p}}_N~]$ 记为 $\boldsymbol{X}_0$ , $[~\bar{\boldsymbol{q}}_1~\bar{\boldsymbol{q}}_2~\cdots~\bar{\boldsymbol{q}}_N~]$ 记为 $\boldsymbol{Y}_0$ , 则优化问题简化为:

$\boldsymbol{R} = arg\min_\boldsymbol{R} \|\boldsymbol{Y}_0-\boldsymbol{RX}_0\|_F^2$

在求解上面优化问题之前, 先回顾几个线性代数里的基本知识:

旋转(正交)矩阵的性质 $\boldsymbol{R}^\text{T}\boldsymbol{R}=\boldsymbol{R}\boldsymbol{R}^\text{T}=\boldsymbol{I}$ ;
$\|\cdot\|_\text{F}$ 为 Frobenius 范数, 即矩阵所有值的平方和之方根;
$\|\boldsymbol{A}\|_\text{F}^2=\text{trace}(\boldsymbol{A}^\text{T}\boldsymbol{A})$ 其中 $\text{trace}$ 为矩阵的迹;
矩阵 $\boldsymbol{A}$ 和 $\boldsymbol{B}$ 均为 $n\times m$ 阶, 则 $\text{trace}(\boldsymbol{A}^\text{T}\boldsymbol{B})=\sum_{j=1}^{m}\sum_{i=1}^{n}{a_{ij}b_{ij}} = \sum_{i=1}^{n}\sum_{j=1}^{m}{a_{ij}b_{ij}}=\text{trace}(\boldsymbol{B}\boldsymbol{A}^\text{T})$ ;
Singular Value Decomposition (SVD)分解, 矩阵 $\boldsymbol{A}_{n\times m}=\boldsymbol{U}_{n\times n}\boldsymbol{\varLambda}_{n\times m}\boldsymbol{V}_{m\times m}^\text{T}$ ,其中 $\boldsymbol{U}$ 和 $\boldsymbol{V}$ 为正交矩阵, $\boldsymbol{\varLambda}$ 为对角矩阵;

于是优化问题可等价为:

$\min_\boldsymbol{R} \|\boldsymbol{Y}_0-\boldsymbol{RX}_0\|_F^2 ~~\Leftrightarrow~~\min_\boldsymbol{R} \text{trace}\big((\boldsymbol{Y}_0-\boldsymbol{RX}_0)^\text{T}(\boldsymbol{Y}_0-\boldsymbol{RX}_0)\big)$

$~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\Leftrightarrow\min_\boldsymbol{R} \text{trace}(\boldsymbol{Y}_0^\text{T}\boldsymbol{Y}_0+\boldsymbol{X}_0^\text{T}\boldsymbol{X}_0-2\boldsymbol{Y}_0^\text{T}\boldsymbol{RX}_0)$

其中矩阵 $\boldsymbol{Y}_0^\text{T}\boldsymbol{Y}_0$ 和 $\boldsymbol{X}_0^\text{T}\boldsymbol{X}_0$ 由原始数据得到的矩阵, 与旋转矩阵 $\boldsymbol{R}$ 无关, 于是

$\min_\boldsymbol{R} \|\boldsymbol{Y}_0-\boldsymbol{RX}_0\|_F^2 ~~\Leftrightarrow~~ \max_\boldsymbol{R} \text{trace}(\boldsymbol{Y}_0^\text{T}\boldsymbol{RX}_0)$

进一步, 利用矩阵迹的性质(基本知识第4点)

$\max_\boldsymbol{R} \text{trace}(\boldsymbol{Y}_0^\text{T}\boldsymbol{RX}_0)~\Leftrightarrow~ \max_\boldsymbol{R} \text{trace}(\boldsymbol{RX}_0\boldsymbol{Y}_0^\text{T})$

使用SVD分解数据矩阵 $\boldsymbol{X}_0\boldsymbol{Y}_0^\text{T}=\boldsymbol{U}\boldsymbol{\varLambda}\boldsymbol{V}^\text{T}$ , 其中 $\boldsymbol{U}$ 和 $\boldsymbol{V}$ 为正交矩阵, 于是

$\max_\boldsymbol{R} \text{trace}(\boldsymbol{RX}_0\boldsymbol{Y}_0^\text{T})~\Leftrightarrow~\max_\boldsymbol{R} \text{trace}(\boldsymbol{R}\boldsymbol{U}\boldsymbol{\varLambda}\boldsymbol{V}^\text{T})$

再一次使用矩阵迹的性质(基本知识第4点)

$\max_\boldsymbol{R} \text{trace}(\boldsymbol{R}\boldsymbol{U}\boldsymbol{\varLambda}\boldsymbol{V}^\text{T})~\Leftrightarrow~\max_\boldsymbol{R} \text{trace}(\boldsymbol{\varLambda}\boldsymbol{V}^\text{T}\boldsymbol{R}\boldsymbol{U})$

记其中的矩阵 $\boldsymbol{V}^\text{T}\boldsymbol{R}\boldsymbol{U}:=\boldsymbol{Z}$ , 不难得知 $\boldsymbol{Z}$ 也为正交矩阵, 综上:

$\min_\boldsymbol{R} \|\boldsymbol{Y}_0-\boldsymbol{RX}_0\|_F^2 ~\Leftrightarrow~\max_\boldsymbol{R} \text{trace}(\boldsymbol{\varLambda}\boldsymbol{Z})$

由于 $\boldsymbol{\varLambda}$ 为对角矩阵

$\min_\boldsymbol{R} \|\boldsymbol{Y}_0-\boldsymbol{RX}_0\|_F^2 ~\Leftrightarrow~\max_\boldsymbol{R} \text{trace}\left(\left( \begin{array}{cccc} z_{11} & z_{12} & z_{13} \\ z_{21} & z_{22} & z_{23} \\ z_{31}& z_{31}& z_{33} \\ \end{array} \right)\left( \begin{array}{cccc} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0& 0& \sigma_3 \\ \end{array} \right)\right)$

$~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\Leftrightarrow~\max_\boldsymbol{R} (\sigma_1z_{11}+\sigma_1z_{22}+\sigma_1z_{33})$

由于 $\boldsymbol{Z}$ 矩阵为正交矩阵, 而且正常数据下对角阵 $\boldsymbol{\varLambda}$ 对角元素为数据矩阵的奇异值均大于0; 而且 $z_{11}~z_{22}~z_{33}$ 均小于等于1, 不难得知当 $z_{11}=z_{22}=z_{33}=1$ 时取最大值, 即:

$\boldsymbol{Z} = \boldsymbol{I}$

从而得到:

$\boldsymbol{V}^\text{T}\boldsymbol{R}\boldsymbol{U} = \boldsymbol{I}~\Leftrightarrow~\boldsymbol{R}=\boldsymbol{V}\boldsymbol{U}^\text{T}$

至此可以得到旋转矩阵 $\boldsymbol{R}$ , 不难得到平移向量 $\boldsymbol{t} = \boldsymbol{q}_0-\boldsymbol{R}\boldsymbol{p}_0$ .

2.

# LM, Registration, SVD