自适应点云配准（RANSAC、ICP）

点云配准完整代码记得给个star~实验目标 编译运行环境需编写项目将配准包括 ICP 配准予以实现, 此过程会用到 numpy 库。运行 main.py src.ply tgt.ply .ply , 即读取源点云.src.ply 获取原点点云 ,从目标点云.tgt.ply取对象作为目标点云 , 把其保存路径设定为.ply。运行该步骤后会显示三张图 , 就是首先为原始点云 , 其中源点云按要求做红色标注 , 目标点云按要求做绿色标注 , 接着是粗配准结果 , 最后是精配准结果。算法设计先对点云进行降采样, 再对点云进行滤波, 之后计算点云特征信息, 其中包括法向量, 还有特征直方图, 以及后续特征匹配需要的若干特征, 任务一和任务二的主要思路一致, 接着利用提取的特征使用某种方法进行粗配准, 最后使用ICP的方法进行精配准。特征提取【PCL学习笔记】中, 关于快速点特征直方图FPFH, 涉及到pcl::, 还有Eba_。进行 FPFHFast Point算法的运用, PFHPoint算法把没给点邻域内两两点对间几何特征某些角度信息提取出来, 去绘制直方图, FPFH 保留了 PFH 的多数信息, 然而忽略了相邻点之间的运算, 却是依照距离的反比把一个邻域内点的直方图加权从而得到 33 维向量, 效率更为高些, 尝试运用 PCA 来进行降维进而提升后续特征匹配速度, 发觉作用不大, 于是舍弃。粗配准采取的办法就是, 针对源点云中的这三个点, 去尝试推测他们和目标点云中的哪些点处于对应关系, 接着计算变换矩阵, 对其优劣状况予以检验。简要流程如下:于源点云中随机挑选3个点, 分别去查找它们于目标点云中特征向量最为接近的点, 此处的“接近”乃在欧式距离的意义范围得以确定的, 查询数据结构运用的是KD树。针对两个三角形之间的变换矩阵展开计算, 实际上就是依照ICP精配准的流程前进, 求得源于最小二乘法意义方向进行定义的最优变换矩阵。在求出源点云经过此变换之后所形成的点云后, 计算它与目标点云之间的重合程度, 此重合度定义为源点云中处于阈值半径内存在目标点的点的数量。再回到1的步骤, 重复开展若干次, 维护并返回重合度最高的变换。按照实际进行编码的情况来看, 鉴于在4里KD树的查询的速度较为缓慢于33维空间中近乎是O(n)O(n)O(n), 所以说提高在1、2当中选择三角形对的质量的这种行为是一种不错的做法。在此处, 三角形对的质量一方面指的是它们相互匹配的可能性, 另一方面指的是经由它们计算得出的变换矩阵是不是准确。能够想到的方法拥有许多, 例如三角形的边不可以太短、两个三角形的对应边长之间的差值不可以太大、优先去选择特征向量远离均值的点等等。在我的实现过程里采用的是前面的两种。此外在降采样后的点云上配准也可以提高效率。精配准【点云精配准】 PointICP- Ding具体推导过程就不赘述了。深度图融合上方所提及的算法, 能够较为出色地达成针对形状以及大小一致的点云配准工作。然而, 要是直接将其运用于深度图之上, 便会出现无法匹配成功的状况接下来呈现的是 -depth 匹配结果的俯视图:这是由于, ICP针对每个源点云中的点, 都会于目标点云中寻觅一个距离最近的点来与之匹配, 但在深度图里, 两个点云存有诸多原本就不匹配的点, 要是强硬地让它们匹配, 那么在最小二乘法的意义范畴内, 最优解便如上图那般, 一个点云被挪动到了另一个的“中央”位置。修正方式为, 针对源点云中的某一个点, 要是在目标点云中不存在与它的间距小于某个限定值的点, 那么在这次迭代期间就不将该点纳入考量。并且, 这个限定值会随着迭代的持续而持续减小存在下限, 如此一来就致使配准愈发精细。关键代码实现中还涉及两个细节。首要的是, 匹配阈值设定为多大才适宜呢? 鉴于不同的数据有着各异的大小规格, 因而我们期望获取点子云于某个表面之上的平均距离。在此之处, 运用了一种极为粗暴的方式, 思索体素往下采样, 采样至一定阶段后, 点子云密集部位的平均距离便得以确定。基于此状况, 把的匹配阈值设定成2.5*, 进而得出相应结果, 结论成立。第二点是, 怎样去判定两个三角形之间的差异呢? 在代码里, 当对应边的长度差值大于某个长度的平均值时。据此, 50次开展有效采样, 便能够计算得出很好的转移矩阵, 由下面所呈现的效果能够看出, 哪怕是粗配准所产生的效果亦是颇为良好的。def RANSAC(): maxCount 0 jisuan0 j 0 print(RANSACing...) while True: j 1 // 随机选取三个点 srcCorr random.sample(range(srcNum), 3) // 如果某两个点距离太近舍弃 if not notTooClose3([srcPoints[x] for x in srcCorr]): continue // KD 树特征匹配 tgtCorr [] for id in srcCorr: k, idx, dis2 tgtFpfhKDT.search_knn_vector_xd(srcFpfh[id], knn1) tgtCorr.append(random.choice(idx[0])) // 如果两个三角形差距太大也舍弃 if True in [farAway(srcPoints[i[0]] - srcPoints[j[0]], tgtPoints[i[1]] - tgtPoints[j[1]]) for i in zip(srcCorr, tgtCorr) for j in zip(srcCorr, tgtCorr)]: continue jisuan 1 // 得到变换矩阵 R, T calculateTrans(np.array([srcPoints[i] for i in srcCorr]), np.array([tgtPoints[i] for i in tgtCorr])) A np.transpose((R srcPoints.T) np.tile(T, (1, srcNum))) //计算匹配数 count 0 for point in range(0, srcNum, 1): k, idx, dis2 tgtKDT.search_hybrid_vector_3d(A[point], radiusfitThreshold, max_nn1) count k // 更新最大匹配 if count maxCount: maxCount count bestR, bestT R, T if jisuan 50 and j 1000: break print(RANSAC calculated %d times, maximum matches: %d % (jisuan, maxCount)) return bestR, bestTICPdef ICP(src, tgt): limit fitThreshold retR np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) retT np.array([[0], [0], [0]]) trace [] for _ in range(400): tgtCorr [] srcCorr [] // 筛选距离近的点对 for point in src: k, idx, dis2 tgtKDT.search_knn_vector_3d(point, knn1) if dis2[0] (limit)**2: srcCorr.append(point) tgtCorr.append(tgt[idx[0]]) trace.append([limit, len(srcCorr)]) R, T calculateTrans(np.array(srcCorr), np.array(tgtCorr)) retR R retR retT R retT T src np.transpose((R src.T) np.tile(T, (1, srcNum))) // 阈值的衰减 limit (limit - fitThreshold/1.5) * 0.95 fitThreshold/1.5 // 自适应过程如果匹配点对不变化说明算法收敛退出迭代 if len(trace) 50 and len(set([x[1] for x in trace[-20:]])) 1: break print(ICP trace is:, trace) return retR, retT效果针对所有测试数据而言, 算法均可获取令人满意之结果, 于此选取 、 以及 -depth 这三组数据予以展示。除此之外, 倘若径直对原始数据实施配准, 将会致使源点云和目标点云全然重合仅会呈现单一颜色, 不利于观察效果。故而算法针对每组数据皆做了适量下采样之后再施行配准, 所以能够看出是两个点云匹配之结果。:配准前粗配准精配准:配准前粗配准精配准-depth:配准前降采样后粗配准降采样后精配准