空中操作仅通过距离映射对遮挡目标进行鲁棒定位（RAL2022）

作者丨fishmarch@知乎

来源丨https://zhuanlan.zhihu.com/p/457168226

编辑丨3D视觉工坊

介绍一下我们已经被RA-L接受的一篇文章。这篇文章不是做SLAM，但和SLAM密切相关，还是挺有意思的。在这里仅介绍总体思路，详细内容还请查阅论文。

论文链接：https://shiyuzhao.westlake.edu.cn/style/2022RAL_XiaoyuZhang.pdf

视频链接：https://www.youtube.com/watch?v=t6-0zaRuFIY；

https://www.bilibili.com/video/BV11T4y1m7tN?share_source=copy_web&vd_source=534ab035a008ff26525503ac9b890e83

背景

在SLAM中，我们希望实现传感器（机器人）的自身定位，为此需要建立基于空间点等几何特征的环境地图；通过地图特征与传感器观测量的几何约束关系求解传感器位姿状态。

而在这项工作中，我们希望实现目标的定位，也就是获得目标点相对于传感器坐标系的三维空间位置。该工作的背景是空中操作机器人，其中目标的遮挡、丢失等问题较多。因此我们希望能够实现对遮挡物体的定位，也就是即使没有看到目标，也可知道其相对位置。

这一问题也可以借助SLAM的方法实现，例如将目标点建立到环境地图中，便可以容易地计算其到相机的相对位置。但这一方法的精度会受到目标点位置精度，相机定位精度等因素的影响。

在这篇论文中，我们希望对目标的定位不依赖于相机的自身定位。因此，我们仿照SLAM提出了新的目标定位方法，为此我们建立了新的地图形式，最终实现的是目标点的相对位置计算，该过程中不进行相机的定位。

方法

该方法基于RGB-D相机实现，也可拓展到双目相机。目标位置以点的形式计算和保存，需要在第一帧图像中给定。在具体实现中，我们用特征点表示目标点，因此文中观测到目标点指的是与目标点匹配成功。

目标定位的核心算法其实比较简单，就是在观测到目标点时，保存目标点与周围特征点的空间关系；没有观测到目标点时，利用匹配的特征点与目标点的空间关系计算目标点的位置。这里的空间关系可以有不同形式，例如方向向量等。但我们选用的是距离。因为距离是与坐标系无关的，因此在进行目标定位的过程中不需要已知相机的自身位姿。基于距离的目标定位其实也比较简单，类似于GPS等三点定位（可参考：iBeacon定位-三点定位实现）。在空间中，已知不在同一平面上的四个点，以及它们到目标点的距离，即可计算出唯一的目标点位置。

具体来讲，我们在每帧图像中提取ORB特征点，其深度值可由深度图或双目匹配计算得到，因此可获得特征点在相机坐标系下的三维坐标：

对目标点做同样处理，因此可在一帧图像中计算目标点到特征点的距离：

系统

根据上述方法，我们仿照SLAM搭建了一个目标定位系统：

pipeline

可以看出来，这里与SLAM是非常像的，也是分为了建图和定位两部分，但含义不同。建图中，我们仅保存特征点到目标点的距离，并不保存点的三维坐标，在论文中，该地图被称为target-centered range-only map。定位中，实现的是求解目标点在当前相机坐标系中的三维空间位置，核心是构建上述的优化方程，难点是特征点匹配。

仿照SLAM系统，在特征点匹配时也采用了与上一帧匹配，在局部地图中匹配等多个过程。计算求解时使用上一帧的结果作为初值。从关键帧中构建新的地图点，更新之前的地图点等。还有其他一些实现细节可以在论文中查看，基本上是和SLAM系统一一对应的，就差一个回环检测了，其实也可以加（但我懒）。整个系统并不复杂，特别是对熟悉SLAM的同学来说，所以就不在此赘述了。

实验

实验选取了两组数据，一组来自ICL-NUIM数据集，因为它可以提供目标点的真值；另一组是我们自己使用realsense采集的数据，并用VICON提供了真值。在两组实验中，我们都和SLAM（ORB-SLAM3）做了精度对比。

其中一组实验数据如上图所示，在第一帧中标记瓶盖处为目标，随着无人机的运动，目标会被机械臂遮挡，或移出相机画面，但整个过程中，我们的方法都会提供目标位置。定位误差如下图所示，蓝色曲线表示是否匹配到了目标点，换色曲线为定位误差，可以看出无论目标是否被遮挡，目标都可以定位到，且误差几乎不变。其中有一小段由于特征匹配等原因失败了，但后面可以恢复回来。

SLAM方法中误差较大，除了累积误差等。我觉得是因为，SLAM是将所有的特征点位置、相机位姿放在一个优化方程中求解一个整体误差最小的结果，但这并不意味着其中的某一个误差（目标点位置）会变小，甚至可能变大。例如下图所示的在ICL数据集中的某次定位结果，使用SLAM方法时，目标点在一次局部优化后位置出现了很大的变化，误差也立刻增大。

总之，使用SLAM方法定位时，目标点位置会受到相机位姿，其他特征点位置等各种误差的影响。而我们的方法希望能够使目标定位精度不受这些额外量的影响，且可应对目标遮挡、丢失等问题。

本文仅做学术分享，如有侵权，请联系删文。

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

14.ROS2从入门到精通：理论与实战

15.国内首个3D缺陷检测教程：理论、源码与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~