点击上方“3D视觉车间，选择星标
第一时间送达干货
来源小猪实验室
这一次，我们将学习如何使用深度相机来实现机器人的视觉SLAM施工图和导航。
一、 概述
1.深度相机
2.常见的深度相机
2.1 双目视觉(Stereo)
2.2 结构光(Structured-light)
2.3 光飞行时间法(TOF)
2.4 综合对比
二、 相机通路开通
1.硬件连接
2.安装驱动
3.测试摄像头
4.点云显示
5.转激光雷达数据测试
三、 视觉 vSLAM 建图及导航
1.vSLAM建图
2.导航
四、视觉 激光雷达建图及导航
1.视觉 激光雷达建图
2.视觉vSLAM 激光雷达导航
一、 概述
1.深度相机
随着机器视觉、自动驾驶等颠覆性技术的逐步发展，采用3D相机越来越多地用于物体识别、行为识别和场景建模。可以说。可以说，深度相机是终端和机器人的眼睛。那么什么是深度相机前的普通相机（2D）想比较，有什么区别？深度相机又称3D顾名思义，相机是通过相机检测拍摄空间的景深距离，这也是与普通相机最大的区别。
普通彩色相机拍摄的图片可以看到相机视角中的所有物体并记录下来，但记录的数据不包括与相机的距离。只能通过图像的语义分析来判断哪些物体离我们很远，哪些物体更近，但没有确切的数据。深度相机刚刚解决了这个问题。通过深度相机获得的数据可以准确地知道图像中每个点与摄像头的距离，加上2点D图像中的(x,y)图像中每个点的三维空间坐标可以通过坐标获得。真实场景可以通过三维坐标还原，实现场景建模等应用。
深度相机也有一些普通相机的缺点，容易受到视差的影响：包括黑色物体（特征点较少）、透明物体（光穿透）、光滑物体（光反射过强）、图像无纹理（特征点较少）、过度曝光（特征点较少）等。
2.常见的深度相机
目前市场上常见的深度相机方案有三种:
2.1 双目视觉(Stereo)
双眼立体视觉(Binocular Stereo Vision)它是一种重要的机器视觉形式。它是基于视差原理，利用成像设备从不同位置获取被测物体的两个图像，通过计算图像对应点之间的位置偏差来获取物体的三维几何信息。
双目相机的主要优点是：
1)硬件要求低，成本低。 CMOS 相机即可
2)适用于室内外。只要光线合适，就不要太暗
但双目的缺点也很明显：
1)对环境光非常敏感。光的变化导致图像偏差大，导致匹配失败或精度低
2)不适用于缺乏纹理的单调场景。双眼视觉根据视觉特征匹配图像，没有特征会导致匹配失败
3)计算复杂性高，需要高计算资源。该方法为纯视觉方法，对算法要求高，实时性差
4)基线限制了测量范围。测量范围与基线(两个摄像头间距)成正比，导致无法小型化
代表公司：
Leap Motion， ZED， 大疆
2.2 结构光(Structured-light)
具有一定结构特征的光线通过近红外激光器投射到被拍摄对象上，然后由专用红外摄像头收集。这种具有一定结构的光会收集不同的图像相位信息，然后通过计算单元将结构的变化转换为深度信息，以获得三维结构。简单地说，通过光学手段获取被拍摄对象的三维结构，然后更深入地应用所获得的信息。不可见的红外激光通常用作具有特定波长的光源，它发出的光通过 在物体上投射一定的编码，通过一定的算法计算返回的编码图案的畸变来获取物体的位置和深度信息。下图是结构光相机的典型示意图：
结构光(散斑)的优点主要有:
1)方案成熟，相机基线小，小型化方便。
2)资源消耗低，单帧 IR 深度图可以计算，功耗低。
3)主动光源，晚上也可以使用。
4)精度高，分辨率高，分辨率高 1280×1024,帧率可达 60FPS。
散斑结构光的缺点与结构光类似：
1)易受环境光干扰，户外体验差。
2)随着检测距离的增加，精度会变差。
代表公司：
苹果，奥比中光(Prime Sense)，微软 Kinect-1，英特尔 RealSense, Mantis Vision 等。
2.3 光飞行时间法(TOF)
顾名思义，它是通过测量光飞行时间来获得距离的。具体来说，它是通过连续向目标发射激光脉冲，然后使用传感器 通过探测光脉冲的飞行往返时间，从反射光接收确切的目标距离。由于光速激光，通过直接测量 光飞行时间实际上是不可行的，通常通过检测和调制光波的相位偏移来实现。TOF 根据调制方法的不同，一般可分为脉冲调制两种：（Pulsed Modulation）连续波调制 （Continuous Wave Modulation）。脉冲调制要求很高度时钟进行测量，且需要发出高频高强度激光，目 前大多采用检测相位偏移办法来实现 TOF 功能。简单来说就是，发出一道经过处理的光，碰到物体以后会反射回来，捕捉来回的时间，因为已知光速和调制光的波长，所以能快速准确计算出到物体的距离。
其原理示意图：
因为 TOF 并非基于特征匹配，这样在测试距离变远时，精度也不会下降很快，目前无人驾驶以及一些高端的消费类 Lidar 基本都是采用该方法来实现。
TOF 的优点主要有：
1）检测距离远。在激光能量够的情况下可达几十米
2）受环境光干扰比较小
但是 TOF 也有一些显而易见的问题：
1）对设备要求高，特别是时间测量模块。
2）资源消耗大。该方案在检测相位偏移时需要多次采样积分，运算量大。
3）边缘精度低。
4）限于资源消耗和滤波，帧率和分辨率都没办法做到较高。
代表公司：
微软 Kinect-2，PMD，SoftKinect， 联想 Phab。
-
传感器技术不是很成熟，因此，分辨率较低，成本高，但由于其原理与另外两种完全不同，实时性高，不需要额外增加计算资源，几乎无算法开发工作量，是未来。
2.4 综合对比
从上面三种主流的 3D 相机成像方案来看，各有优劣，但是从实际应用场景来看，在非无人驾驶域，结构光，特别是散斑结构光的用途是最广泛。因为从精度，分辨率，还有应用场景的范围来看双目和 TOF 都没有办法做到最大的平衡。而且对于结构光容易受环境光干扰，特别是太阳光影响问题，鉴于此类相机都有红外激光发射模块，非常容易改造为主动双目来弥补该问题。
结构光与TOF的对比：
对比来看，结构光技术功耗更小，技术更成熟，更适合静态场景。而TOF方案在远距离下噪声较低，同时拥有更高的FPS，因此更适合动态场景。
目前，结构光技术主要应用于解锁以及安全支付等方面，其应用距离受限。而TOF技术主要用于智能机后置摄影，并在AR、VR等领域（包括3D拍照、体感游戏等）有一定的作用。
3D结构光和TOF两者其实各有优劣势。结构光最大的优势是发展的较为成熟，成本比较低，劣势是只适合中短距离使用。ToF优势是抗干扰性较好，视角较宽，缺陷是功耗高，造价贵，精度及深度图分辨率较低。两项技术各有侧重点和适配使用场景。
综合考虑成本，本文将采用此类相机，在这里单独说明下。
Kinect（xbox游戏机配件）：
乐视体感相机（乐视tv配件）：
相机一般有三只眼睛，从左到右分别为：
投射红外线pattern的IR Projector（左）
摄像头Color Camera（中）
读取反射红外线的IR Camera（右）
Depth传感器读取左右两边投射的红外线pattern，通过pattern的变形来取得Depth的信息。
二、 摄像头通路打通
1.硬件连接
考虑成本，我们在上一次组装的ROS+底盘套装的基础上，新增一个深度相机即可：
乐视体感三合一摄像头 * 1
usb子母延长线 * 1
2.安装驱动
# 安装相关依赖
sudo apt install ros-$ROS_DISTRO-rgbd-launch ros-$ROS_DISTRO-libuvc-camera ros-$ROS_DISTRO-libuvc-ros ros-$ROS_DISTRO-camera-info-manager # ros-$ROS_DISTRO-libuvc
# 安装vslam相关依赖
sudo apt install ros-$ROS_DISTRO-rtabmap ros-$ROS_DISTRO-rtabmap-ros ros-$ROS_DISTRO-depthimage-to-laserscan
# 克隆功能包源码
cd ~/catkin_ws/src
git clone git@github.com:orbbec/ros_astra_camera.git
git clone git@github.com:yanjingang/robot_vslam.git
# 创建udev规则
cd ros_astra_camera
chmod +x ./scripts/create_udev_rules
./scripts/create_udev_rules
# 配置并加载环境变量(机器人端执行)
cd ~/catkin_ws/src
./robot_vslam/script/setrgbd.sh astrapro
source ~/.bashrc # export CAMERA_TYPE=astrapro
# 编译
cd ~/catkin_ws
catkin_make --pkg astra_camera robot_vslam
source ~/catkin_ws/devel/setup.bash3.测试摄像头
在完成第一步的功能包编译之后，将摄像头连接到机器人的主机
# 0.检查摄像头是否正常识别
ll /dev/astra*
lrwxrwxrwx 1 root root 15 Jan 6 12:50 /dev/astra_pro -> bus/usb/001/014
lrwxrwxrwx 1 root root 15 Jan 6 12:50 /dev/astrauvc -> bus/usb/001/015
# 1.机器人端启动摄像头(启动后会弹出几个警告，不用管它，这是由于驱动中的一些参数摄像头不支持)
#roslaunch astra_camera astra.launch # Use Astra
roslaunch robot_vslam astrapro.launch
# 如果报Warning: USB events thread - failed to set priority. This might cause loss of data...，是因为没有权限，执行以下命令
#sudo -s
#source /home/work/.bashrc
#roslaunch robot_vslam astrapro.launch
# 2.PC 端查看图像(常用topic：深度：/camera/depth/image；彩色：/camera/rgb/image_raw/compressed；红外：/camera/ir/image；注:ir图像只能通过rviz显示，在rqt_image_view 中显示会是全黑，这和图像格式有关，并非设备问题)
rviz 或 rqt_image_view4.点云显示
这一部分为了让大家了解一下 rviz 中的各类插件的使用，将带着大家一起搭建一个 rviz 显示环境。（如果不想进行手动配置，也可以直接使用预先配置好的文件：/robot_vslam/rviz/astrapro.rviz）
首先我们添加一个点云显示插件，点击”Add”，在弹出的界面中双击” PointCloud2”：
展开左侧PointCloud2选型卡，选择Topic：
将 Fixed Frame 选择为 camera_link：
此时界面中就会显示出点云图：
在 Color Transformer 中选择不同的颜色模式可以给点云按照不同的规则着色：
最后通过左上角的”File”选项卡中 Save Config As 可以将当前 rviz 配置保存到指定位置，下次打开 rviz 候可以通过 Open Config 直接打开配置文件避免再次手动配置界面。
5.转激光雷达数据测试
# 启动深度相机（机器人端）
roslaunch robot_vslam camera.launch
# 启动深度图到雷达数据的转换
roslaunch robot_vslam depth_to_laser.launch
# pc端可视化
rviz配置一下界面就可以正常显示雷达数据了
这里可以注意到，相对于机器人的 TF 坐标位置，雷达数据并不是像我们常见的激光雷达数据是 360 度的，这是由于深度相机的视场角(即 FOV，我们这款相机水平 FOV 为 58.4 度，垂直FOV 为 45.5 度)是有限的，所以转换出的雷达数据角度范围和相机是水平 FOV是一致的，简单理解就是，只能看到相机前方一定角度范围内的东西。
三、 视觉 vSLAM 建图及导航
1.vSLAM建图
机器人端启动建图：
# 启动底盘base control
roslaunch ros_arduino_python arduino.launch
# 启动camera
roslaunch robot_vslam camera.launch
# 启动rtab vslam
roslaunch robot_vslam rtabmap_rgbd.launch在 rtabmap_rgbd.launch 启动完成后，camera 节点和 rtabmap 的节点中会分别输出以下信息(因为这里输出的信息较多且有参考价值，所以我们没有将所有的启动文件写成一个 launch 文件在同一个终端中输出)
PC端查看建图过程：
# PC端打开rviz终端(在rviz终端中就可以看到所建立的平面地图和立体地图)
roslaunch robot_vslam rtabmap_rviz.launch
# 也可以使用rtabmap自带的图形化界面rtabmapviz在PC端来观测建图过程（不建议，带宽占用100M以上）
# roslaunch robot_vslam rtabmapviz.launch键盘控制机器人移动：
# 键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py我们建议是将机器人的直线运动速度不超过 0.2m/s,旋转速度不超过 0.4rad/s 以保证建图质量。建图过程和激光雷达建图基本一致，遥控机器人在环境中遍历一遍完成建图。
地图是默认保存在机器人端~/.ros/rtabmap.db 文件中，由于这个文件包含了建图过程中的图片和特征点的信息，所以文件会比较大。
注意:启动 roslaunch robot_vslam rtabmap_rgbd.launch 都会覆盖掉 rtabmap.db 文件，如果需要保留上次建图的结果，注意备份文件。
建图完成后 rviz 中的地图如下所示
这里我们也可以修改点云显示的颜色来实现其他效果，例如这样
2.导航
机器人端打开四个终端，分别运行:
# 启动底盘base control
roslaunch ros_arduino_python arduino.launch
# 启动camera
roslaunch robot_vslam camera.launch
# 启动rtab并自动定位
roslaunch robot_vslam rtabmap_rgbd.launch localization:=true
# 启动movebase（纯前向视觉无法后退）
roslaunch robot_vslam move_base.launch planner:=dwa move_forward_only:=true这里 move_base 的 planner 参数可以选择 dwa 和 teb，这部分在激光雷达导航部分已经介绍过，这里就不再赘述，move_forward_only 参数是为了只允许机器人向前运动(视觉传感器只有前向感知) 。启动后检查各个节点有无报错，正常应该是可以直接定位成功的。
# pc端rviz查看和控制
roslaunch robot_vslam rtabmap_rviz.launch# pc端rtabmap查看（需手动下载全局地图）
roslaunch robot_vslam rtabmapviz.launch在 rtabmapviz 中点击红框中的按钮同步地图数据库的信息，可能会花费一点时间，取决于你的局域网条件。如果左下位置中没有出现图像且图像两侧处为黑色，则机器人尚未完成初始位置的确定。如果如下图所示，则机器人已经完成定位，可以开始导航无需执行下面的步骤(通常机器人从纹理比较丰富且上次已经到达过的位置启动，都可以直接完成定位，这里文档中是为了说明初始定位这一过程故意从一处几乎没有纹理的区域启动使机器人不能完成初始定位)
如果没有自动定位成功，则可以在 PC 端或机器人端启动一个键盘控制节点：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py控制慢速移动机器人，直到左下角位置出现图像且两侧为绿色表示定位成功
接下来就可以在地图中指定目标点让机器人导航过去了。
注意：rgbd 相机的视场角较小，且深度信息存在较大的盲区，在导航中有可能会发生碰撞，可以通过调节增大 robot_vslam/param/MarsRobot/costmap_common_params.yaml中的inflation_radius 参数来改善，但是很难达到和激光雷达导航或rgbd相机+激光雷达的效果。
四、视觉+激光雷达建图及导航
1.视觉+激光雷达建图
这里操作过程和第三章节完全一致，只是部分启动指令有差异，所以这里只列出启动指令，不再描述操作过程。机器人端打开三个终端，分别运行:
# 启动激光雷达+base_control
roslaunch robot_navigation robot_lidar.launch
# 启动深度摄像头
roslaunch robot_vslam camera.launch
# 启动rgbd+lidar联合建图
roslaunch robot_vslam rtabmap_rgbd_lidar.launch
# pc端查看
roslaunch robot_vslam rtabmap_rviz.launch
# pc端控制移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py我们建议是将机器人的直线运动速度不超过 0.2m/s,旋转速度不超过 0.4rad/s 以保证建图质量建图过程和激光雷达建图基本一致，遥控机器人在环境中遍历一遍完成建图。这里可以看到，由于加入了360 度的激光雷达数据，建图在 2D 的地图方面表现要更好一些。
2.视觉vSLAM +激光雷达导航
机器人端打开四个终端，分别运行:
# 启动激光雷达+base_control
roslaunch robot_navigation robot_lidar.launch
# 启动camera
roslaunch robot_vslam camera.launch
# 启动rtab并自动定位
roslaunch robot_vslam rtabmap_rgbd_lidar.launch localization:=true
# 启动movebase（允许后退）
roslaunch robot_vslam move_base.launch planner:=dwa move_forward_only:=false
# pc端查看
roslaunch robot_vslam rtabmap_rviz.launch
roslaunch robot_vslam rtabmapviz.launch这里由于加入了 360 度的激光雷达，机器人具备了全向的感知能力，所以可以将 move_forward_only 参数设置为 false 允许机器人向后运动。今天带大家学习了如何使用深度相机实现视觉slam建图和导航，同时我们也尝试了视觉+激光雷达的协同使用，机器人在障碍物感知和避障方面有了明显改善，同时通过摄像头我们也能更方便的实现远程监控和控制。下一次你想看到什么呢？欢迎留言，我将带大家一起实现～！
本文仅做学术分享，如有侵权，请联系删文。
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从入门到精通：理论与实战
扫码添加小助手微信，可
也可申请加入我们的细分方向交流群，目前主要有、、、、、等微信群。
一定要备注：，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。也请联系。
▲长按加微信群或投稿
▲长按关注公众号
：针对3D视觉领域的五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题