已经过匹配的点云变换参数估计—umeyama

当两个点云\(\{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][1]。基本思路是在目标函数中加入拉格朗日乘子限制\(\boldsymbol{R}\)是标准正交阵（\(\boldsymbol{R}\boldsymbol{R}^T=\boldsymbol{I},det(\boldsymbol{R})=1\)）。之后就是引入SVD进行一系列推导。
[1]: http://web.stanford.edu/class/cs273/refs/umeyama.pdf