花生_TL007

这个屌丝很懒，什么也没留下！

热门标签

ICP知识点梳理笔记_icp非凸

作者：花生_TL007 | 2024-02-21 09:33:31

踩

icp非凸

ICP知识点梳理笔记

一、ICP是什么？
二、ICP的具体原理和实现
三、Point to Plane的ICP
四、一些ICP改进的对比

一、ICP是什么？

Iterative Closest Points迭代最近点算法，是1992年，Paul J.Besl 等人首次提出了 ICP 算法（Method for registration of 3-D shapes），以点集对点集（PSTPS）配准方法为基础，阐述了一种曲面拟合算法，用于解决基于自由形态曲面的配准问题。其基本迭代流程分为如下四个过程：
1）搜索点云与目标点云的最近点对；
2）通过最近点计算误差函数，进行 SVD(Singular Value Decomposition)分解来计算旋转矩阵R和平移矩阵t；
3）将得到的旋转矩阵R和平移矩阵t应用在点云上
4）求解应用后的点云与目标点云误差，通过阈值判断是否继续迭代。

注意：本文对原文的超链接均为Google Scholar的搜索结果，具体原文PDF请自行善用搜索工具获取（部分原文可直接通过Google Scholar获取）

二、ICP的具体原理和实现

1.搜索最近点

首先有两个点，其欧氏距离为：
$d\left( {{{\vec{r}}}_{1}},{{{\vec{r}}}_{2}} \right)=\left\| {{{\vec{r}}}_{1}}-{{{\vec{r}}}_{2}} \right\|=\sqrt{{{\left( {{x}_{2}}-{{x}_{1}} \right)}^{2}}+{{\left( {{y}_{2}}-{{y}_{1}} \right)}^{2}}+{{\left( {{z}_{2}}-{{z}_{1}} \right)}^{2}}}$
其中 ${{\vec{r}}_{1}}=\left( {{x}_{1}},{{y}_{1}},{{z}_{1}} \right)$ ， ${{\vec{r}}_{2}}=\left( {{x}_{2}},{{y}_{2}},{{z}_{2}} \right)$
那么对于一个点与一个点集的欧氏距离为：
$d\left( \vec{p},A \right)=\underset{i\in \left\{ 1,\ldots ,{{N}_{a}} \right\}}{\mathop{\min }}\,d\left( \vec{p},{{{\vec{a}}}_{i}} \right)$
此时有一个点 ${{\vec{a}}}_{j}$ 满足 $d\left( \vec{p},{{{\vec{a}}}_{j}} \right)=d\left( \vec{p},A \right)$
那么此时 $\vec{p}$ 和 ${{\vec{a}}}_{j}$ 为一对最近点。由此可见进行粗匹配对最近点的迭代效率至关重要。

注意：Mathtype中转换成LaTeX格式时，两侧默认为[和]，这是Display Style，要改成Inline Style，即将[和]都改成$。
注意：文章中不仅讨论点与点之间的距离，也讨论了点与线，点与三角形之间的距离，以及点到参数实体距离和点到隐式实体距离

2.对应点集配准

SVD：singular value decomposition奇异值分解，可以参考https://zhuanlan.zhihu.com/p/29846048。
简而言之，协方差计算需要通过特征分解求特征向量，而对一个矩阵进行特征分解，需要这个矩阵是正定的，如果不是正定的任意矩阵就需要SVD。
对于一个绕任意轴 $\vec{n}$ 旋转 $\theta$ 四元数： ${{\vec{q}}_{R}}={{\left[ {{q}_{0}},{{q}_{1}},{{q}_{2}},{{q}_{3}} \right]}^{T}}={{\left[ \cos \left( \frac{\theta }{2} \right),\sin \left( \frac{\theta }{2} \right){{{\vec{n}}}_{x}},\sin \left( \frac{\theta }{2} \right){{{\vec{n}}}_{y}},\sin \left( \frac{\theta }{2} \right){{{\vec{n}}}_{z}} \right]}^{T}}$
那么有旋转矩阵R：
$R=\left[$

\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{0} q_{3}) & 2 (q_{1} q_{3} + q_{0} q_{2}) \\ 2 (q_{1} q_{2} + q_{0} q_{3}) & q_{0}^{2} + q_{2}^{2} - q_{1}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{0} q_{1}) \\ 2 (q_{1} q_{3} - q_{0} q_{2}) & 2 (q_{2} q_{3} + q_{0} q_{1}) & q_{0}^{2} + q_{3}^{2} - q_{1}^{2} - q_{2}^{2} \end{matrix}

$\begin{matrix} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left( { {q}_{1}}{ {q}_{2}}-{ {q}_{0}}{ {q}_{3}} \right) & 2\left( { {q}_{1}}{ {q}_{3}}+{ {q}_{0}}{ {q}_{2}} \right) \\ 2\left( { {q}_{1}}{ {q}_{2}}+{ {q}_{0}}{ {q}_{3}} \right) & q_{0}^{2}+q_{2}^{2}-q_{1}^{2}-q_{3}^{2} & 2\left( { {q}_{2}}{ {q}_{3}}-{ {q}_{0}}{ {q}_{1}} \right) \\ 2\left( { {q}_{1}}{ {q}_{3}}-{ {q}_{0}}{ {q}_{2}} \right) & 2\left( { {q}_{2}}{ {q}_{3}}+{ {q}_{0}}{ {q}_{1}} \right) & q_{0}^{2}+q_{3}^{2}-q_{1}^{2}-q_{2}^{2} \\ \end{matrix}$ \right]

R = q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} 2 (q_{1} q_{2} + q_{0} q_{3}) 2 (q_{1} q_{3} - q_{0} q_{2}) 2 (q_{1} q_{2} - q_{0} q_{3}) q_{0}^{2} + q_{2}^{2} - q_{1}^{2} - q_{3}^{2} 2 (q_{2} q_{3} + q_{0} q_{1}) 2 (q_{1} q_{3} + q_{0} q_{2}) 2 (q_{2} q_{3} - q_{0} q_{1}) q_{0}^{2} + q_{3}^{2} - q_{1}^{2} - q_{2}^{2}

设平移向量

{{\vec{q}}_{T}}={{\left[ {{q}_{4}},{{q}_{5}},{{q}_{6}} \right]}^{T}}

那么有完全配准状态向量

\vec{q}={{\left[ {{{\vec{q}}}_{R}}|{{{\vec{q}}}_{T}} \right]}^{T}}

现在让点集P与点集X配准（两点集数量相等均为N，且相同编号的点为对应的最近点），即需要最小化均方误差函数（mean square objective fuction）：

f\left( {\vec{q}} \right)=\frac{1}{N}\sum\limits_{i=1}^{N}{{{\left\| {{{\vec{x}}}_{i}}-R\left( {{{\vec{q}}}_{R}} \right){{{\vec{p}}}_{i}}-{{{\vec{q}}}_{T}} \right\|}^{2}}}

考虑到两个点云的质心

{{\vec{\mu }}_{p}}=\frac{1}{N}\sum\limits_{i=1}^{N}{{{{\vec{p}}}_{i}}}

和

{{\vec{\mu }}_{x}}=\frac{1}{N}\sum\limits_{i=1}^{N}{{{{\vec{x}}}_{i}}}

那么有两点云的协方差矩阵

{{\Sigma }_{px}}

，通过一系列的推导（详见原文）可以得到对应

{{\vec{q}}_{R}}

的矩阵

Q\left( {{\Sigma }_{px}} \right)

的最大特征值就是最优旋转。同时可以求出平移向量

{{\vec{q}}_{T}}

四元数与旋转矩阵之间的关系和推导可以见https://blog.csdn.net/loongkingwhat/article/details/88427828。

3.将求得的结果应用到原点云

${{P}_{k+1}}={{\vec{q}}_{k}}\left( {{P}_{0}} \right)$

4.判断均方差是否收敛

略

三、Point to Plane的ICP

1992年，Chen等人在Object Modeling by Registration of Multiple Range Images中提出了平面（3D range image）的配准方式，把最近点对的直线距离改成最近点到对应点切平面的距离，推导可以参考https://blog.csdn.net/qq_45369294/article/details/121150122，算法实现可以参考https://blog.csdn.net/qq_36686437/article/details/109998696
2004年，Low等人在Linear least-squares optimization for point-to-plane icp surface registration中运用线性最小二乘优化，此时的最小化均方差函数变成：
${{M}_{opt}}=\arg {{\min }_{M}}\sum\limits_{i}{{{\left( \left( M\cdot {{s}_{i}}-{{d}_{i}} \right)\cdot {{n}_{i}} \right)}^{2}}}$
与前文公式的格式进行匹配后为：
在这里插入图片描述

四、一些ICP改进的对比

2001年，Szymon Rusinkiewicz等人对ICP进行了总结对比并提出了一定的改进Efficient Variants of the ICP Algorithm

作为对比的基准主要包括一下六个特性：
1.在两个网格上做随机抽样点
2.将每个选定的点与另一个网格中法线与源法线45度以内的最接近的样本进行匹配。
3.点对的均匀(常数)加权
4.拒绝包含边缘顶点的对，以及一部分具有最大点到点距离的对
5.点到平面误差度量。
6.经典的“选择-匹配-最小化”迭代，而不是对对齐转换进行其他搜索

1.点云的选取

选择具有高强度梯度的点，在变体中使用每个样本的颜色或强度来帮助对齐（Registration of 3-D partial surface models using luminance and depth information），并且同时在两个点云中进行sampling。

2.匹配点

在另一个网格中找到最近的点(A Method for Registration of 3-D Shapes)。这种计算可以使用k-d树和/或近点缓存加速(Fast and Accurate Shape-Based Registration)。

3.点的权重

对于权重的选取受点云数据本身的特性影响较大，而且对于普通的点云，权重带来的收敛性能影响很小，因此可以不考虑权重。

4.拒绝部分点对

拒绝在边界处的点对非常的有效(Zippered polygon meshes from range images)，但是按照比例去拒绝点对的话，可能会影响确定正确对齐的准确性和稳定性，并且通常不会提高收敛速度。

5.误差度量与最小化

Point to Plane明显要比Point to Point的收敛速度要快Linear least-squares optimization for point-to-plane icp surface registration

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/花生_TL007/article/detail/122573