当两个点云$\{x_i\},\{y_i\}, i=1,2,...n$各点已经经过匹配,现求两个点云间的相似变换 $s,\boldsymbol{R},\boldsymbol{t}$。


推导从纯旋转开始,而相似变换在纯旋转变换的求法已知后可以很简单地推导得到。

纯旋转

问题定义

已知两个点云$\{x_i\},\{y_i\}, i=1,2,...n$各点已经经过匹配,现求$\boldsymbol{R}$使得

$$ \boldsymbol{R} = arg\min_{\boldsymbol{R}}||\boldsymbol{A}-\boldsymbol{RB}||^2 $$

解法(定理)

记$\boldsymbol{A}$和$\boldsymbol{B}$都为$m\times n$矩阵,$\boldsymbol{R}$为$m\times m$的旋转矩阵($3\times 3$)。记$\boldsymbol{AB^T}$的SVD分解为
$$\boldsymbol{AB^T} = \boldsymbol{UDV^T}$$
其中$\boldsymbol{UU^T}=\boldsymbol{VV^T}=\boldsymbol{I},\boldsymbol{D}=diag(d_1,d_2,...d_m),d_1 \ge d_2 \ge ... \ge d_m$
那么$||\boldsymbol{A}-\boldsymbol{RB}||^2$的最小值满足:

$$ \min_{\boldsymbol{R}}||\boldsymbol{A}-\boldsymbol{RB}||^2 = ||\boldsymbol{A}||^2 + ||\boldsymbol{B}||^2 -2tr(\boldsymbol{DS}) $$

其中

$$ \boldsymbol{S}= \left\{ \begin{align} &\boldsymbol{I} & if & & det(\boldsymbol{AB^T}) \ge 0 \\ &diag(1,1,...,1,-1) & if & & det(\boldsymbol{AB^T}) \lt 0 \end{align} \right. $$

当$det(\boldsymbol{AB^T})=0, i.e. rank(\boldsymbol{AB^T})=m-1$,$\boldsymbol{S}$的需要经过如下判断决定:

$$ \boldsymbol{S}= \left\{ \begin{align} &\boldsymbol{I} & if & & det(\boldsymbol{U})det(\boldsymbol{V}) = 1 \\ &diag(1,1,...,1,-1) & if & & det(\boldsymbol{U})det(\boldsymbol{V}) = -1 \end{align} \right. $$

相似变换

问题定义

已知两个点云$\boldsymbol{X}=\{x_i\},\boldsymbol{Y}=\{y_i\}, i=1,2,...n$各点已经经过匹配,现求$c,\boldsymbol{R,t}$使得

$$ \epsilon^2 = \min e^2(c,\boldsymbol{R,t}) = \min_{c,\boldsymbol{R,t}} \frac{1}{n} \sum_{i=1}^n ||\boldsymbol{y}_i-c(\boldsymbol{Rx_i+t})||^2 $$

解法(定理)

误差最小值为

$$ \epsilon^2 = \sigma_{y_2}-\frac{\boldsymbol{DS}^2}{\sigma_{x_2}} $$

其中$\sigma_{x_2},\sigma_{y_2}$分别为两点云$\boldsymbol{X}=\{x_i\},\boldsymbol{Y}=\{y_i\}, i=1,2,...n$的方差,去,且$\boldsymbol{D}$为点云的协方差矩阵$\sum_{xy}$的SVD分解$\sum_{xy}=\boldsymbol{UDV^T}$中的$\boldsymbol{D}$,并且

$$ \boldsymbol{S}= \left\{ \begin{align} &\boldsymbol{I} & if & & det(\sum_{xy}) \ge 0 \\ &diag(1,1,...,1,-1) & if & & det(\sum_{xy}) \lt 0 \end{align} \right. $$

记$\boldsymbol{\mu}_x,\boldsymbol{\mu}_y$分别为两个点云的均值,
当$rank(\sum_{xy}) = m$,

$$ \begin{align} \boldsymbol{R} &= \boldsymbol{USV}^T \\ \boldsymbol{t} &= \boldsymbol{\mu}_y - c \boldsymbol{R\mu_x} \\ c &= \frac{1}{\sigma_x^2}tr(\boldsymbol{DS}) \end{align} $$

当$rank(\sum_{xy}) = m-1$,

$$ \boldsymbol{S}= \left\{ \begin{align} &\boldsymbol{I} & if & & det(\boldsymbol{U})det(\boldsymbol{V}) = 1 \\ &diag(1,1,...,1,-1) & if & & det(\boldsymbol{U})det(\boldsymbol{V}) = -1 \end{align} \right. $$

证明

证明参考umeyama,1991。基本思路是在目标函数中加入拉格朗日乘子限制$\boldsymbol{R}$是标准正交阵($\boldsymbol{R}\boldsymbol{R}^T=\boldsymbol{I},det(\boldsymbol{R})=1$)。之后就是引入SVD进行一系列推导。

标签: none

添加新评论