halcon中的三维匹配大致分为两类,一类是基于形状的(Shape-Based),一类是基于表面的(Surface-Based)。基于形状的匹配可用于单个2D图像中定位复杂的3D物体,3D物体模型必须是CAD模型,且几何边缘清晰可见,使用的相机也要预先进行校准。基于表面的匹配可用于3D场景中快速定位复杂的3D物体,比如在点云中寻找物体,模型可以从CAD或3D传感器中获得,可以包含光滑的表面,基于表面的匹配也称为“体积匹配”。
halcon中的surface matching
halcon中surface matching参考文献为:Model Globally, Match Locally: Efficient and Robust 3D Object Recognition,该方法已申请专利。通常我们只要知道模型中的三个点,以及其在场景中对应的三个点就可以确定位姿,如果每个点都有一个方向,那么只需要一组对应点和转动角度就能确定位姿。文献中定义了PPF(point pair feature)这样的特征矢量,在创建模板时计算每个特征点与其他特征点的PPF,构建哈希表,将具有相同 feature 的 point pair 放在一起,在匹配时从场景中选取一部分关键点,将每个关键点与其他场景点计算PPF,根据存储好的哈希表对模型点和转动角度进行反向投票,超过设定分数就认为找到了模型的point pair,根据模型点和转动角度算出变换矩阵得到初始位姿,再用ICP求精。
基于PPF的surface matching网上有很多文章,GitHub上有很多开源实现,opencv中也实现了该算法,对于算法的原理我不再详细展开,感兴趣的可以自己查找相关资料。这里只谈一下我的实现过程及所遇到的问题。
模型点云
首先是模型训练,最主要的问题是点云降采样,降采样可以加速计算,并且避免一些很接近的点,在点云匹配过程也需要对场景进行降采样,点云的降采样有两种办法,一种是使用kdtree,一种是使用octree,目前两种方法我还在对比中,降采样之后的PPF计算,需要限制两个点的距离和法向量夹角,夹角超过30度都会保留,计算好后保存至本地。
模型点云(降采样后)
第二步是模型匹配,先对场景点降采样,之后选取一定比例的场景点作为关键点,然后计算每个关键点与其他场景点的PPF,根据模型训练时保存的哈希表对模型点和转动角度进行投票,如果票数高于设定值,保存关键点、模型点与转动角度,并计算变换矩阵,该矩阵即为模型点对应关键点的位姿。值得注意的是计算PPF时要以关键点为圆心,选取指定半径范围内的场景点,因为距离太远的两个点不可能处在同一物体上,在Going Further with Point Pair Features这篇文章就指出使用一大一小两个voting ball做两轮投票,大小半径由bounding box确定。
第三步是位姿聚类,上一步中得到了很多位姿,这里我们要进行两次聚类,第一次是针对一个参考点对应多个位姿,第二次是针对一个位姿对应多个参考点,两种情况下都需要合并,第一次聚类将位姿转换为axis-angle+translate的形式,通过旋转角度和平移量判断是否为同类,第二次聚类将位姿转换为rotation+translate的形式,直接对模型的box center进行旋转+平移,如果变换后的点非常接近,直接合并。
第四步是点云配准,即ICP pose refinement,以第三步得到的位姿作为初始值,在场景点中寻找模型点的最近点,根据距离和方向确定对应关系,由新的点对计算新的位姿,再以新的位姿进行下一次计算,迭代至收敛或发散。对于收敛的结果重新进行评分,落在模型上的点越多,分数越高。在配准过程中找到正确的对应点以及使用稳健的icp非常关键,对应点的查找我们借助kdtree或者octree,icp使用robust symmetric icp。
目前的速度和精度还可以,可视化是在cloudcompare中完成,后续我会发布测试demo,对于位姿筛选、ICP算法、投票过程,我会继续深入研究,也欢迎大家与我讨论。
匹配结果1
匹配结果2