使用奇异值分解的最小二乘刚性配准方法

最新推荐文章于 2021-08-23 04:42:04 发布

shchojj

最新推荐文章于 2021-08-23 04:42:04 发布

阅读量1.1k

点赞数 4

分类专栏： point cloud rigid registration

原文链接：https://igl.ethz.ch/projects/ARAP/svd_rot.pdf

版权

rigid registration 同时被 2 个专栏收录

22 篇文章 8 订阅

订阅专栏

point cloud

2 篇文章 0 订阅

订阅专栏

翻译：https://igl.ethz.ch/projects/ARAP/svd_rot.pdf

Least-Squares Rigid Motion Using SVD

1、问题描述

$\mathbb{P} = \left \{ p_1,p_2,...,p_n \right \}$

$\mathbb{Q} = \left \{ q_1,q_2,...,q_n \right \}$

是变换空间 $\mathbb{R}^d$ 中对应的两组点集。

我们希望得到一个刚体变换是的两组点集最小二乘结果最小

$\left ( R, t \right ) = argmin_{R\in SO\left ( d \right ), t\in \mathbb{R}^d}$ $\left ( R,t \right )=\mathop{\arg\min}_{R\in SO\left ( d \right ), t\in \mathbb{R}^d)} \sum_{i=1}^{n}w_i \left \| \left ( R p_i + t\right ) - q_i\right \|^2$ ---------------------(1)

其中R就是旋转矩阵，t是平移向量，w_i就是每对点的权重。

2、计算平移

首先假定R是固定的，求解公式就是 $F\left ( t \right )= \sum_{i=1}^{n}w_i \left \| \left ( R p_i + t\right ) - q_i\right \|^2$ 通过对F(t)求导，t的最优解就可以知道了

$0 = \frac{\partial F}{\partial t} =\sum_{i=1}^{n}2 w_i\left ( R p_i + t -q_i\right ) =2t\left ( \sum_{i=1}^{n}w_i \right ) + 2R\left ( \sum_{i=1}^{n}w_i p_i \right )-2\left ( \sum_{i=1}^{n}w_i q_i \right )$ --------------(2)

左边是0，那我们直接将除以 $\sum_{i=1}^{n}w_i$

$\overline{p} = \frac{\sum_{i=1}^{n}w_ip_i}{\sum_{i=1}^{n}w_i}$ 、 $\overline{q} = \frac{\sum_{i=1}^{n}w_iq_i}{\sum_{i=1}^{n}w_i}$ --------------------------------------------------------------------------------(3)

公式（2）就整理成了

$t = \overline{q}-R\overline{p}$ --------------------------------------------------------------------------------------------------------------(4)

最优平移t将转换后的P的加权质心映射到Q的加权质心

我们在把公式（4）带入目标函数F：

$F\left ( t \right )= \sum_{i=1}^{n}w_i \left \| \left ( R p_i +\overline{q}-R\overline{p}\right ) - q_i\right \|^2 = \sum_{i=1}^{n}w_i \left \| R \left ( p_i -\overline{p}\right ) -\left ( q_i-\overline{q} \right )\right \|^2$ ---------------------（6）

我们再次引入新的定义

$x_i :=p_i - \overline{p}, y_i:=q_i-\overline{q}$ ------------------------------------------------------------------------------------------(7)

那么最优的旋转求解就可以变成

$R=\mathop{\arg\min}_{R\in SO\left ( d \right )} \sum_{i=1}^{n}w_i \left \| R x_i - y_i\right \|^2$ -------------------------------------------------------------------------------(8)

3 计算旋转矩阵

将公式（8）展开

$\left \| R x_i - y_i\right \|^2 = \left ( Rx_i-y_i \right )^T\left ( Rx_i-y_i \right ) =\left ( x_i^TR^T-y_i^T \right )\left ( Rx_i-y_i \right ) =x_i^TR^TRx_i-y_i^TRx_i -x_i^TR^Ty_i+y_i^Ty_i =x_i^Tx_i-y_i^TRx_i -x_i^TR^Ty_i+y_i^Ty_i$ -----（9）

旋转矩阵 R^TR=I 推到的话 $\left ( R_zR_yR_x \right )^T\left ( R_zR_yR_x \right )$ 比较容易了 R_z^T R_z=I

x_i^TR^Ty_i 是一个标量，证明过程主要是根据维度来的 x_i^T 是1Xd, R^T 是dxd， y_i 是dx1,可不就是标量么。标量的转置可不就是标量本身么

$x_i^TR^Ty_i = \left ( x_i^TR^Ty_i \right )^T = y_i^TRx_i$ ---------------------------------------------------------------------------------(10)

因此公式（9）就可以展开为

$\left \| R x_i - y_i\right \|^2 =x_i^Tx_i-2y_i^TRx_i +y_i^Ty_i$ -------------------------------------------------------------------------(11)

然后我们将公式（11）带入到公式（8）

$R=\mathop{\arg\min}_{R\in SO\left ( d \right )} \sum_{i=1}^{n}w_i \left \| R x_i - y_i\right \|^2 =\mathop{\arg\min}_{R\in SO\left ( d \right )} \sum_{i=1}^{n}w_i \left ( x_i^Tx_i-2y_i^TRx_i +y_i^Ty_i \right ) =\mathop{\arg\min}_{R\in SO\left ( d \right )} \left ( \sum_{i=1}^{n}w_i x_i^Tx_i-2\sum_{i=1}^{n}w_i y_i^TRx_i +\sum_{i=1}^{n}w_i y_i^Ty_i \right ) =\mathop{\arg\min}_{R\in SO\left ( d \right )} \left (-2\sum_{i=1}^{n}w_i y_i^TRx_i \right )$ ----（12）

标量在求解最小化中没啥用啊，可以删掉，还有标量2也可以删掉，因此公式（12）最终可以表示为

$\mathop{\arg\min}_{R\in SO\left ( d \right )} \left (-2\sum_{i=1}^{n}w_i y_i^TRx_i \right )=\mathop{\arg\max}_{R\in SO\left ( d \right )} \left (\sum_{i=1}^{n}w_i y_i^TRx_i \right )$ -----------------------------------------------（13）

我们在把公式（13）中的 $\sum_{i=1}^{n}w_i y_i^TRx_i = tr\left ( WY^T RX\right )$ 进行矩阵表示

$\begin{bmatrix} w_1 & & & \\ & w_2 & & \\ & & \ddots & & \\ & & & w_n \end{bmatrix}\begin{bmatrix} - y_1^T - \\ - y_2^T - \\ - \vdots - \\ - y_n^T - \end{bmatrix}\begin{bmatrix} & & & \\ & R & \\ & & & \end{bmatrix}\begin{bmatrix} | | | | \\ x_1 x_2 \cdots x_n \\ |||| \end{bmatrix} =\begin{bmatrix} - w_1y_1^T - \\ - w_2y_2^T - \\ - \vdots - \\ - w_ny_n^T - \end{bmatrix}\begin{bmatrix} | | | | \\ Rx_1 Rx_2 \cdots Rx_n \\ |||| \end{bmatrix} =\begin{bmatrix} w_1y_1^TRx_1 & & & \\ & w_2y_2^TRx_2 & & \\ & & \ddots & & \\ & & & w_ny_n^TRx_n \end{bmatrix}$

$W=diag\left ( w_1,...,w_n \right )$ 是一个nxn对角矩阵，权重w_i是第i个对角元素。Y是dxn矩阵y_i是列向量，X是dxn矩阵x_i是列向量。方阵的迹是对角线上元素的和。

$\sum_{i=1}^{n}w_i y_i^TRx_i = tr\left ( WY^T RX\right )$ -----------------------------------------------------------------------------------------(14)

求解旋转矩阵就是最大化矩阵的迹 $tr\left ( WY^T RX\right )$

矩阵的迹可符合交换律,前提是AB的维度要可以相乘啊

$tr\left ( AB\right )=tr\left ( BA\right )$ -------------------------------------------------------------------------------------------------------（15）

$tr\left ( WY^T RX\right )=tr\left ( \left ( WY^T \right ) \left ( RX \right )\right )=tr\left ( \left ( RX \right )\left ( WY^T \right ) \right )=tr\left ( RX WY^T \right )$ -----------------------（16）

看矩阵维度X就是dxn，W是nxn，Y^T是nxd

问题来了 S=XWY^T 是一个dxd的矩阵，这么一看是不是像一个协方差矩阵“covariance”（我也不知道为什么像）

我们现在将S进行SVD：

$S=U\Sigma V^T$ -------------------------------------------------------------------------------------------------------------------(17)

在将公式（17）带入公式（16）

$tr\left ( RX WY^T \right )=tr\left ( RS \right )=tr\left ( RU\Sigma V^T \right )=tr\left ( \Sigma V^TRU \right )$ --------------------------------------------------(18)

其实V,U是正交矩阵，这个好理解R也是正交矩阵，旋转矩阵就是正交矩阵啊，所以 M=V^TRU 也是一个正交矩阵，也就是M的列是正交向量， m_j^Tm_j=1 ,m_j是M的列向量，然后更好理解了m_ij<=1

$1=m_j^Tm_j=\sum _{i=1}^{d}m_{ij}^{2}\Rightarrow m_{ij}^{2}\Rightarrow\left | m_ij \right |\leqslant 1$ ------------------------------------------------------------------------(19)

$tr\left ( \Sigma M \right )$ 最大可能是什么呢， $\Sigma$ 是非负特征值 $\sigma_1,\sigma_1,\cdots ,\sigma_d\geq 0$ 为对角线元素的对角矩阵，因此

$tr\left ( \Sigma M \right )=\begin{pmatrix} \sigma_1 & & & \\ & \sigma_2 & & \\ & & \ddots & \\ & & & \sigma_d \end{pmatrix}\begin{pmatrix} m_{11} m_{12} \cdots m_{1d} \\ m_{21} m_{22} \cdots m_{2d} \\ \vdots \vdots \vdots \vdots\\ m_{d1} m_{d2} \cdots m_{dd} \end{pmatrix} =\sum_{i=1}^{d}\sigma_i m_{ii}\leq \sum_{i=1}^{d}\sigma_i$ ---------------------------(20)

那这个 $tr\left ( \Sigma M \right )$ 迹的最大值不就是当m_ii=1的时候么，M是个正交矩阵，那M就只能是单位矩阵了

$I=M=V^TRU\Rightarrow V=RU\Rightarrow R=VU^T$ ----------------------------------------------------------------------(21)

Orientation rectification，上面的描述都是如何找到最优的正交矩阵，但是旋转矩阵中也有反射变换。这些假设其实都是认为这些点集可以通过变换很好的对齐，但如果只有旋转变换，实际上也许一个都对不齐。

检查 R=VU^T 是否为旋转矩阵，如果 $det\left ( VU^T \right )=-1$ 这就包含了反射，否则 $det\left ( VU^T \right )=1$ 。假设 $det\left ( VU^T \right )=-1$ ，那么R是一个旋转矩阵等价于M是一个反射矩阵，我们现在想找到一个反射矩阵M $tr\left ( \Sigma M \right )=\sigma_1m_{11}+\sigma_2m_{22}+\cdots +\sigma_dm_{dd}=:f\left ( m_{11},\cdots ,m_{dd} \right )$ -----------------------------------------(22)

现在f只依赖于对角矩阵M，没有其他人变量，m_ii看做是变量 $\left ( m_{11},\cdots ,m_{dd} \right )$ ，这是n阶反射矩阵的对角线的集合。n阶旋转矩阵的所有对角线集合等于值为-1的坐标个数为偶数的点集 $\left ( \pm 1,\cdots ,\pm 1 \right )$ 的凸包，由于任何反射矩阵都可以通过翻转旋转矩阵的一行符号来构造，反之亦然，因此我们所优化的点集 $\left ( \pm 1,\cdots ,\pm 1 \right )$ 凸包中-1的个数是uneven（我也不知道是个啥，应该是非偶数）。

由于定义域是一个凸多面体，线性函数f在顶点处达到极值。对角矩阵 $\left ( 1,1,\cdots ,1 \right )$ 因为它有偶数个- 1（也就是0了），所以不在定义域内，所以最简洁的形式就是 $\left ( 1,1,\cdots ,-1 \right )$ ：

$tr\left ( \Sigma M \right )=\sigma_1+\sigma_2+\cdots +\sigma_{d-1}-\sigma_{d}$ -------------------------------------------------------------------------------（23）

这个值是在定义域的一个顶点得到的，并且大于除 $\left ( 1,\cdots ,1 \right )$ 任何形式的组合 $\left ( \pm 1,\cdots ,\pm 1 \right )$ ，因为 $\sigma_{d}$ 是最小的特征值啊。

要记住我们一直是一个优化问题啊，

$M=V^TRU=\begin{pmatrix} 1 & & & & \\ & 1& & & &\\ & & \ddots & & &\\ & & & 1& & \\ & & & & 1 \end{pmatrix} \Rightarrow R = V\begin{pmatrix} 1 & & & & \\ & 1& & & &\\ & & \ddots & & &\\ & & & 1& & \\ & & & & 1 \end{pmatrix} U^T$ ----------------------------(24)

我们可以把有没有反射情况的通解写在一起，也就是 $det\left ( VU^T \right )=1$ 、 $det\left ( VU^T \right )=-1$

$R = V\begin{pmatrix} 1 & & & & \\ & 1& & & &\\ & & \ddots & & &\\ & & & 1& & \\ & & & & det\left ( VU^T \right ) \end{pmatrix} U^T$ -----------------------------------------------------------------------(25)