Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration（论文笔记）（点对平面的ICP算法）

最新推荐文章于 2024-09-13 16:36:02 发布

多蕾姆

最新推荐文章于 2024-09-13 16:36:02 发布

阅读量312

点赞数 4

分类专栏：三维重建文章标签：平面计算机视觉算法图像处理论文笔记论文阅读 3d

本文链接：https://blog.csdn.net/qq_57894564/article/details/141120900

版权

三维重建专栏收录该内容

3 篇文章 0 订阅

订阅专栏

2 点对平面的ICP算法

给定源表面和目标表面，ICP算法的每次迭代首先在源表面上的点与目标表面上的点之间组成一组对应点对。例如，对于源表面上的每个点，选择目标表面上最近的点作为其对应点。ICP迭代的输出是一个3D刚体变换M，它变换源点，使对应点之间的总误差在某一选定的误差度量下最小。

图1 两个表面之间的点对平面误差

当使用点对平面的误差度量时，优化目标是在相应的目的点（见图1）每个源点和切平面之间距离的平方之和。更具体来说，如果 $s _ { i } =$ $( s _ { i x } , s _ { i y } , s _ { i z } , 1 ) ^ { T }$ 是一个源点， $d _ { i } = ( d _ { i x } , d _ { i y } , d _ { i z } , 1 ) ^ { T }$ 是对应的目标点， $n _ { i } =$ $( n _ { i x } , n _ { i y } , n _ { i z } , 0 ) ^ { T }$ 是在 $d_{i}$ 处的单位法向量，然后每个ICP迭代的目标是找到 $M_{opt}$ 使得

$M _ { o p t } = a r g \min \sum _ { i } ( ( M \cdot s _ { i } - d _ { i } ) \cdot n _ { i } ) ^ { 2 }$ （1）

其中M和 $M_{opt}$ 是 $4\times 4$ 的3D刚体变换矩阵。

一个3D刚体变换M包含一个旋转矩阵 $R ( \alpha , \beta , \gamma )$ 和一个转换矩阵 $T ( t _ { x } , t _ { y } , t _ { z } )$ ，即 $M = T ( t _ { x } , t _ { y } , t _ { z } ) \cdot R ( \alpha , \beta , \gamma )$ （2）

其中

$T ( t _ { x }, t_{y} , t_{z} ) =\begin{pmatrix} 1 & 0 & 0 & t_{x}\\ 0 & 1 & 0 & t_{y} \\ 0 & 0 & 1 & t_{z}\\ 0 & 0 & 0 & 1 \end{pmatrix}$ （3）

$R ( \alpha , \beta , \gamma ) = R _ { z } ( \gamma ) \cdot R _ { y } ( \beta ) \cdot R _ { x } ( \alpha ) =\begin{pmatrix} r_{11} & r_{12} & r_{13} & 0\\ r_{21} & r_{22} & r_{23} & 0\\ r_{31} & r_{32} & r_{33} & 0\\ 0 & 0 & 0 & 1 \end{pmatrix}$ （4）

其中

$r _ { 1 1 } = \cos \gamma \cos \beta$

$r _ { 1 2 } = - \sin \gamma \cos \alpha + \cos \gamma \sin \beta \sin \alpha$

$r _ { 1 3 } = \sin \gamma \sin \alpha + \cos \gamma \sin \beta \cos \alpha$

$r _ { 2 1 } = \sin \gamma \cos \beta$

$r _ { 2 2 } = \cos \gamma \cos \alpha + \sin \gamma \sin \beta \sin \alpha$

$r _ { 2 3 } = - \cos \gamma \sin \alpha + \sin \gamma \sin \beta \cos \alpha$ $r _ { 3 1 } = - \sin \beta$

$r _ { 3 2 } = \cos \beta \sin \alpha$

$r _ { 3 3 } = \cos \beta \cos \alpha$

$R _ { x } ( \alpha ) , R _ { y } ( \beta ) , R _ { z } ( \gamma )$ 分别是 $\alpha ,\beta ,\gamma$ 弧度绕x,y,z轴的旋转。

式1本质上是一个最小二乘优化的问题，求解它只需要确定 $\alpha , \beta , \gamma , t _ { x } , t _ { y } , t _ { z }$ 这六个参数的值。然而，由于 $\alpha ,\beta ,\gamma$ 是旋转矩阵R中非线性三角函数的参数，因此不能使用有效的线性最小二乘技术来求解。在下一节中，我们将介绍如何用线性最小二乘问题来近似这个非线性最小二乘问题。

3 线性估计

当角度 $\theta \approx 0$ 时，我们可以用近似值 $\sin \theta \approx \theta , \cos \theta \approx 1$ 。因此，当 $\alpha ,\beta ,\gamma \approx 0$ 时，

$R ( \alpha , \beta , \gamma ) \approx\begin{pmatrix} 1 & \alpha \beta -\gamma & \alpha \gamma +\beta & 0\\ \gamma & \alpha \beta \gamma +1 & \beta \gamma -\alpha & 0\\ -\beta & \alpha & 1 & 0\\ 0& 0 & 0 &1 \end{pmatrix}\approx\begin{pmatrix} 1 & -\gamma & \beta & 0\\ \gamma & 1 & -\alpha & 0\\ -\beta & \alpha & 1 & 0\\ 0 & 0 & 0 & 1 \end{pmatrix}= \hat{R}(\alpha ,\beta ,\gamma )$ （5）

然后，M被估计为

$\widehat { M } = T ( t _ { x } , t _ { y } , t _ { z } ) \cdot \widehat { R } ( \alpha , \beta , \gamma ) =\begin{pmatrix} 1 & -\gamma & \beta & t_{x}\\ \gamma & 1 & -\alpha & t_{y}\\ -\beta & \alpha & 1 & t_{z}\\ 0 & 0 & 0 & 1 \end{pmatrix}$ （6）

现在我们可以重写式1为

$\widehat { M } _ { o p t } = a r g \min ( ( \widehat { M } \cdot s _ { i } - d _ { i } ) \cdot n _ { i } ) ^ { 2 }$ （7）

式7中的每一个 $( \widehat { M } \cdot s _ { i } - d _ { i } ) \cdot n_{i}$ 都能被写作一个6个参数 $\alpha , \beta , \gamma , t _ { x } , t _ { y } , t _ { z }$ 的线性表示。

$( \widehat { M } \cdot s _ { i } - d _ { i } ) \cdot n _ { i } =\left ( \hat{M}\cdot \begin{pmatrix} s_{ix}\\ s_{iy}\\ s_{iz}\\ 1 \end{pmatrix} -\begin{pmatrix} d_{ix}\\ d_{iy}\\ d_{iz}\\ 1 \end{pmatrix}\right)\cdot \begin{pmatrix} n_{ix}\\ n_{iy}\\ n_{iz}\\ 0 \end{pmatrix}$ $= ( n _ { i z } s _ { i y } - n _ { i y } s _ { i z } ) \alpha + ( n _ { i x } s _ { i z } - n _ { i z } s _ { i x } ) \beta + ( n _ { i y } s _ { i x } - n _ { i x } s _ { i y } ) \gamma + n _ { i x } t _ { x }$ $= \left [ ( n _ { i z } s _ { i y } - n _ { i y } s _ { i z } ) \alpha + ( n _ { i x } s _ { i z } - n _ { i z } s _ { i x } ) \beta + ( n _ { i y } s _ { i x } - n _ { i x } s _ { i y } ) \gamma + n _ { i x } t _ { x } + n _ { i y } t _ { y } + n _ { i z } t _ { z } \right ]- \left [n _ { i x } d _ { i x } + n _ { i y } d _ { i y } + n _ { i z } d _ { i z } - n _ { i x } s _ { i x } - n _ { i y } s _ { i y } - n _ { i z } s _ { i z } \right ]$

给定N对对应点对，我们可以将所有的 $( \widehat { M } \cdot s _ { i } - d _ { i } ) \cdot n_{i}$ 排列为一个矩阵表达 $Ax-b$

注意到

$\min \sum _ { i } ( ( \widehat { M } \cdot s _ { i } - d _ { i } ) \cdot n _ { i } ) ^ { 2 } = \min { x } \left| { A x - b } \right| ^ { 2 }$ （11）

因此，我们可以首先解出

$x _ { o p t } = a r g \min _ { x } | A x - b ^ { 2 } |$ （12）

这是一个标准的线性最小二乘问题，可以使用SVD（奇异值分解）来解决。设 $A = U \sum V ^ { T }$ 是A的SVD。A的伪逆定义为矩阵 $A ^ { + } = V \sum ^ { + } U ^ { T }$ ，其中 $\sum ^ { + }$ 是取的 $\sum$ 非零元素的逆（零元素保持不变）。则最小二乘问题的解为

$x _ { o p t } = A ^ { + } b$ （13）

假设解为 $x _ { o p t } = ( \alpha _ { o p t } , \beta _ { o p t } , \gamma _ { o p t } , t _ { x o p t } , t _ { y o p t } , t _ { z o p t } )$ ，注意到由于 $\widehat { R } ( \alpha _ { o p t } , \beta _ { o p t } , \gamma _ { o p t } )$ 可能不是一个有效的旋转矩阵，我们不应该使用结果 $T ( t _ { x o p t } , t _ { y o p t } , t _ { z o p t } ) \cdot \widehat { R } ( \alpha _ { o p t } , \beta _ { o p t } , \gamma _ { o p t } )$ 。相反，我们应该使用 $T ( t _ { x o p t } , t _ { y o p t } , t _ { z o p t } ) \cdot R ( \alpha _ { o p t } , \beta _ { o p t } , \gamma _ { o p t } )$ ，即使它不等于式7中定义的 $\widehat { M } _ { o p t }$ 。