【PART 2】OAK-D+TurtleBot3机器人项目全解析
这个项目的第一篇文章请查看：【PART 1】，本文来源：medium。
本文主要内容有：
- TurtleBot3上安装OAK-D
- 通过ROS传输深度图
- 传感器信息同步
- 使用里程计建图
介绍
这个项目也是OAK用户参加OpenCV Spatial AI大赛的作品，同时也是开源的（地址）！在这篇文章中，我们会先展示如何将OAK-D安装到机器人平台上，然后我们解释了如何将OAK-D的深度图作为ROS消息发布，最后我们使用里程计信息作为真实姿势创建了一个小型室内地图，并将其用作我们的基准。
在TurtleBot 3上安装OAK-D
我们借助GoPro相机支架将OAK-D模块放置在TB3的顶部，该支架还可以防止模块左右摇摆，这对获得一致的相机外参很重要。此外，为了简化外在参数的校准过程，我们安装OAK-D，使其位置与TB3的base_footprint框架的X-Y坐标一致。
ROS上的深度流
用于从OAK-D传输深度图的DepthAI封装class
鉴于我们的ROS版本（ROS Melodic）不支持Python 3，我们决定使用用C++写的DepthAI API（depthai-core）。使用这个API从OAK-D查询的数据以二进制流的形式出现，需要重新解释成你期望的类型。在我们的例子中，我们将二进制流解释为单一的通道cv::Mat。为了实现这一点，我们编写了一个名为DepthAI的封装class，它继承了depthai-core中的Device class。
这DepthAI封装class需要3个参数：
- 设备名称
- 配置文件路径
- USB模式
使用适当的设备名称和USB模式，它检测并创建一个OAK-D设备实例。一旦OAK-D设备实例准备就绪，就可以使用配置文件来建立管道，并确定必须启用什么类型的流。在我们的封装class中，我们使用std::unordered_map，通过抓取OAK-D设备发送的二进制流来相应地放置每个启用的视频流。我们发现，由于异步流速率，当启用多个流时，会出现帧丢失的问题。我们计划在将来解决这个问题。
ROS深度图publisher
现在我们的设备可以将深度流作为cv::Mat对象，下一步是发布ROS话题的深度图，供下游节点使用。我们称之为发布者节点DepthMapPublisherNode。正如你在图2中看到的，此节点创建一个DepthAI实例，并以cv::Mat使用DepthAI::get_streams()。一旦我们获得了深度图，我们就使用cv_bridge把它封装成一个带有头信息和适当的编码的sensor_msgs::Image。根据官方OAK-D的官方文档，一个深度图数组由uint_16组成。因此，我们用sensor_msgs::image_encoding::TYPE_16UC1对图像进行编码。
注意:在我们的项目中，我们使用depthai-core库作为一个子模块，通过
add_subdirectory()将其添加到CMake包中。然而，这种方法并不是开箱即用的，因为depthai-core和Catkin（ROS的编译器）都是基于Hunter的。我们不得不明确地将HunterGate部分从depthai-core转移到我们的仓库中。你可以看看它们是如何被复制到我们的项目中的，这里.
节流里程计和时间同步的点云
在任何SLAM问题中，为所有考虑的时间戳找到最佳姿势是关键任务。正如我们在上一篇文章中所解释的，我们使用里程计和深度图来获得姿势约束。然而，与深度图相比，测距信息的发布频率要高得多，为了能够找到某个时间戳的最佳姿势，我们需要从该时间戳的多个来源获得姿势估计。这就需要做两件大事——首先，OAK-D和Jetson Nano上的时钟必须同步；其次，我们需要一种方法来在时间上匹配测距信息和从深度图中创建的点云。我们通过首先在空间上对传入的测距信息进行节制来解决这些问题，也就是说，我们放弃那些对应于线性和径向运动小于一些小阈值的测距信息。然后，我们将传入的深度图与节制的测绘信息同步，在每个时间戳给我们匹配的场景。
节流里程计
我们不断地从TB3上的内置轮子编码器发布测距信息。我们可以通过施加某些空间阈值来过滤掉重复的信息。因此，我们实现了一个ROS节点，该节点订阅原始测距信息，并在仅满足某些条件时发布节制的测距信息。这些条件参数如下：
- 从先前姿势到当前姿势的位置变化（移动的距离）
- 从先前姿势到当前姿势的旋转变化（偏航角）
我们可以比较当前姿势和先前姿势来计算位置和旋转的差异。当其中一个超过预定义的阈值时，我们最终可以发布节流里程计。在我们的项目中，我们将阈值设置如下：
- 距离阈值= 0.2米
- 偏航角阈值= 0.262rad(15度)
时间同步
一旦我们开始发布节流里程计，我们将不得不在给定位置获得相应的深度图。我们可以通过使用ROS消息过滤器来实现这一点。ROS提供了基于策略的同步器，因此我们可以根据ROS消息上的时间戳同步多个主题。我们将使用ApproximateTime同步器，因为它的策略使用自适应算法。图3下面的示例展示了ApproximateTime过滤器暂时同步消息。启用同步器后，我们可以将时间同步的深度图逆投影到点云中。
使用里程计约束创建地图
在这一节中，我们提供了一个无需执行任何姿势优化就可以实现的地图示例！这张地图的质量将作为未来使用姿势优化的地图的基准。由于里程计是对TB3姿态的一种估计，我们可以创建一个地图，将其视为TB3的真实位置。考虑到里程计容易产生漂移和误差积累，我们并不期望这张图看起来非常清晰。
现在我们有了一个点云和相同时间戳的里程计姿态，我们可以开始构建地图了。为此，我们创建了一个名为CreateMapNode的ROS节点，它不断地连接一系列点云并发布点云地图。由于反向投影的点云是在OAK-D的相机参考系中表示的，我们首先需要将它们传输到odom参考框架，这样我们就可以通过连接来创建地图。这是通过使用tf2_ros包完成的。
我们带着我们的TB3在房子周围进行了一次短途测试，并在一个rosbag中记录了原始的测距和深度图。然后，我们用RViz将点云地图和节流里程计一起可视化。图4是测试结束后完成的地图。请注意，这个地图的构建只使用了里程计姿势，没有任何姿势优化。我们将在以后的博文中讨论姿势优化。
此外，我们对原始点云应用不同的点云过滤算法。由PCL库提供的一些可能的过滤器如下：
对于以下视频中记录的地图创建，我们使用了统计异常值去除和体素网格从深度图中过滤反向投影的点云。我们仍在进行不同的实验，以决定使用什么样的参数进行过滤会产生最佳结果。这些文件将非常重要，因为我们将在连续的点云上运行ICP（迭代最接近点）算法来获得姿态约束。