点云数据
点云数据通常表示为N行,至少3列的矩阵,其中N表示点的数量,每一行代表一个点。通常3列分别是点在空间中(x,y,z)的坐标。如果点云数据有除空间中坐标外的附加信息,如来自LIDAR传感器的点云数据,那么它可能具有每个点的附加值,例如“反射率”,其是在该位置中障碍物反射多少激光光束的量度。 在这种情况下,点云数据可能是N×4阵列。
三维点云配准
点云的配准过程,就是求两个点云之间的一个旋转平移矩阵,源点云通过旋转平移矩阵后,变换成和目标点云矩阵相同的矩阵。
可以表示为以下方程:
其中,pt,ps分别是目标点云和源点云中的一对对应点。
R,T就是我们要求的旋转平移矩阵。
但是,我们并不知道两个点云中,点与点之间的对应关系,这也是配准的核心问题。
三维点云配准分为粗配准与精配准两步。
粗配准就是在完全不清楚两个点云的相对位置关系的情况下,找到一个这两个点云近似的旋转平移矩阵(不一定很精确,但是已经大概是对的了)。
精配准就是在已知一个旋转平移矩阵的初值的情况下(这个初值大概已经是正确的了),进一步计算得到更加精确的旋转平移矩阵。
配准的评价标准:比较通用的是LCP(Largetst Common Pointset)(最大公共点集)。给定两个点集P,Q,找到一个变换T,使得变换后的T(P)与Q的重叠度最大。在变换后的P内任意一点,如果在容差范围内有另外一个Q的点,则认为该点是重合点。重合点占所有点数量的比例就是重叠度。
(一)粗配准
(1)暴力配准
粗配准中,最简单粗暴的一个方法就是暴力配准。找到一个刚体变换需要3对对应点,即在点集P中选取3个点,如果我们能找到它们在点集Q中的对应点,即能根据它们的坐标求出旋转平移矩阵。(具体怎么求:pass)
暴力配准步骤:
- 随机在点集P中选取3个点,随机在点集Q中选取3个点。
- 计算旋转平移矩阵T
- 计算 ||T(P)- Q||<的点的个数(点集P经过旋转平移矩阵后,点集中与点集Q的点的距离小于给定的阈值的点的个数)
- 遍历所有的可能性,选取最高的作为最后的结果
暴力配准的缺点:时间复杂度高。
假设点集P的大小为n,点集Q的大小为m,分别随机选取3个点组成关联对,则在选取3个点时就分别有和种可能,点集P中选取的任意一种可能,均需遍历点集Q中的所有可能。因此,完成整个算法共需遍历种可能。因此,暴力配准的时间复杂度为。(当n,m足够大时,-1,-2等都失去了意义)
(2)4PCS(4-Points Congruent Sets)(4点全等集)
4PCS的核心原理在于,在刚性变换中,同一平面上的4个点,在经过刚性变换后,其某些比率是稳定不变的。
如图,{a,b,c,d}四点共面,其中两条直线的交点为e。在刚性变换中,r1,r2是稳定不变的。
4PCS步骤:
- 在点集P中选取共面四点对作为base B={a,b,c,d}
- 计算比率,
- 点集Q(n个点)中,每两个点组成一对(),对于每一对点,,计算他们的中间点
- 如图,每一对点都可以计算出两个中间点
- 若点集Q中,任意两对点,一对由计算得到的中间点和另一对由计算得到的中间点的距离在允许范围内,那么可以认为这两对点可能是base B={a,b,c,d}的仿射对应点。
- 如图,此时可认为点集{,,,}是base B={a,b,c,d}的仿射对应点
- 可能存在k个这样的点集,这k个点集再作如下处理:
(1)根据每个点集与base B,计算出旋转平移矩阵T
(2)计算 ||T(P)- Q||<的点的个数(点集P经过旋转平移矩阵后,点集T(P)与点集Q,距离小于给定的阈值的点的个数)
(3)遍历所有k个点集,选取最高的作为最后的结果
因此,4PCS算法的时间复杂度包括两部分,点集Q(n个点)中任意选取两个点组成一对,计算,需作,在找出的个可能是base B的仿射对应点的四点集中,遍历检查,获得最好的旋转平移矩阵T。
(二)精配准
精配准的模式基本已固定为使用ICP算法(Iterative Closest Point)(最近点迭代法)及其各种变种。
ICP算法的步骤:
- (1)ICP算法的核心是最小化一个目标函数:
是一对对应点,总共有对对应点,这个目标函数实际上是找到旋转平移矩阵R,T,使得所有对应点之间的欧式距离的平方和最小。 - (2)寻找对应点。起初并不知道有哪些对应点,因此,在有粗配准获得的旋转平移矩阵的初值下,用此旋转平移矩阵对源点云进行变换,得到一个变换后的点云,将这个变换后的点云与目标点云进行比较,只要两个点云中存在距离小于一定阈值(ICP算法的一个参数)的点,就认为这两个点是对应点。(最邻近点)
- (3)R,T优化。有了对应点后,根据目标函数优化R,T。
- (4)迭代。优化得到了新的R,T,导致变换后的点云的一些点的位置发生了变化,一些最近邻点对也相应地发生了变化,回到步骤(2)中重新寻找对应点。迭代(2)(3)步骤,直到满足迭代终止条件,如R,T的变化量小于一定值,或目标函数的变化小于一定值,或最近邻点对不再改变等(ICP算法的另一个参数)
- (5)迭代终止后,最终获得精配准的R,T
ICP算法的参数:
(1)距离阈值。两个点云中距离小于距离阈值的点,才算作对应点。
(2)迭代的终止条件。
至此,完成点云数据的配准。
参考文献:
[1] Aiger D, Mitra N J, Cohen-Or D. 4-points congruent sets for robust pairwise surface registration[J]. Acm Transactions on Graphics, 2008, 27(3):85.