【PART 1】OAK-D+TurtleBot3机器人项目全解析：SLAM、ROS、深度图、点云。 -

【PART 1】OAK-D+TurtleBot3机器人项目全解析：SLAM、ROS、深度图、点云。

by OAK中国
in OAK应用指南
on 2022年3月17日

来源：Deepak Talwar，编辑：OAK中国。

这个项目的第二篇文章请查看：【PART 2】

前言

著名的开源计算机视觉库OpenCV在2020年宣布了由英特尔赞助的首届3D人工智能竞赛，随着OAK-D智能深度相机（OpenCV AI Kit with Depth）的推出，OpenCV号召参赛者使用OAK-D来解决现实世界的问题。OAK-D相机内置双目深度立体摄像头和一个RGB摄像头。它还配备了一个强大的视觉处理单元（来自英特尔的Myriad X VPU），以实现板载深度神经网络推理。

Deepak Talwar的团队被OAK-D相机的性能及潜力所震撼，所以他们决定在7月份提交参赛的项目书，并最终通过了大赛第一阶段的筛选（从235个提案中选出32个），参与到决赛。本文主要是分享他们在整个开发过程中的进展、想法和挑战。

项目介绍

Deepak Talwar团队的项目名称是：使用3D人工智能来检测室内地标，并实现基于图形的姿态地标SLAM。利用OAK-D相机解决室内位姿图SLAM问题，但增加了姿态地标约束。OAK-D不仅能够检测物体，还能够提供它们的相对3D位置，这使得解决这个问题成为唯一可能。一般来说，位姿图SLAM问题是一个优化问题，它使用移动代理的多个姿态估计源，并通过调整它们来使得这些估计之间的误差最小化。这种优化使我们能够在其环境中定位代理，同时使我们能够拼接机器人的传感器测量值，以创建环境的地图表示。

大多数室内SLAM使用以下两种姿态估计源:

轮式里程计
旋转激光扫描仪(又名2D激光雷达)

项目实施过程中，我们的目标是使用轮式里程计作为第一来源，但用OAK-D取代2D激光雷达扫描仪。图1显示了项目的实施流程图。以下是OAK-D如何独特地帮助解决这个问题：

1.位姿-位姿约束的来源：
通常，通过使用诸如迭代最近点(ICP)的对准算法，来自2D激光雷达的激光扫描可以用于估计位姿图中的姿态约束。思考ICP的一种非正式方式——如果从两个不同的位置给你两次激光扫描，你需要如何移动和旋转才能使两次激光扫描对准。这个运动+旋转给了我们一个位姿-位姿约束的估计。Deepak Talwar表示，“我们希望在使用OAK-D时使用类似的方法”。为此，他们需要从OAK-D深度图创建点云，然后将它们与ICP对齐。这些将提供更丰富、更精确的位姿约束，因为可以利用3D信息。

2.位姿-地标约束的来源：
然而，使用OAK-D的主要好处是它可以直接在边缘进行3D对象检测！特别之处在于，这允许你使用基于深度学习的对象检测器来检测对象，并在位姿图优化问题中使用它们作为界标。你可能想知道，什么被认为是地标？在OAK-D多次测量中观察到的环境中的任何静止物体都可以被用作地标，因为它在世界坐标框架中的位置是不变的，而代理相对于地标的相对位置保持发展。对同一对象的这些观察允许你创建位姿-地标约束，其中地标的位置被认为是固定的。这些地标可以用作整个室内地图的“定位”点。

有了这些资源，你将能够创建一个既包含位姿-位姿约束又包含位姿-地标约束的图形。然后，优化这些位姿。一旦位姿确定，你只需简单地拼接OAK-D的观测数据（这些数据可以是RGB图像、深度图或点云）来创建室内地图。

设置机器人平台

Jetson Nano + TurtleBot3 Waffle + OAK-D

首先需要的是一个室内移动机器人，我们可以在上面安装OAK-D。该机器人必须符合以下标准：

它应该提供一个轮式里程表的来源。虽然我们可以自己添加这个，但因为这不是这个项目的主要任务，最好是使用一个已经有编码器电机的平台。
支持机器人操作系统。我们的目标是使用ROS作为这个项目的主干，所以一个支持ROS的机器人是必须的。
它应该能够为OAK-D和单板计算机（SBC）供电。

我们选择修改一个TurtleBot3 Waffle来满足这些要求。我们选择TurtleBot3 Waffle的原因很简单——这是我们手头上有的！另外，TurtleBot3 Waffle几乎检查所有的盒子。以下是它所提供的内容和我们需要改变的地方的总结:

TB3具有里程计：TB3配备了DYNAMIXEL电机，这些电机具有内置编码器，可提供里程计。
TB3带有OpenCR嵌入式平台可以为我们选择的SBC（NVIDIA Jetson Nano）以及OAK-D提供足够的电力。
默认情况下，TB3带有一个树莓派3 B+。然而，我们想用一台Jetson Nano作为我们的SBC，因为有更高的性能。
我们移除了TB3附带的2D激光扫描仪。
我们将在TB3上安装OAK-D。

图2：我们的机器人平台设置。我们用NVIDIA Jetson Nano替换了Robotis TurtleBot3 Waffle Pi上的Raspberry Pi。

图2显示了修改后的机器人平台，已经安装了Jetson Nano，并配置为与TB3一起运行。我们还没有安装和校准OAK-D。

在TurtleBot3上使用Jetson Nano作为单板计算机

如前所述，TurtleBot3默认带有一个Raspberry Pi 3 B+。幸运的是，为TB3设置Jetson Nano作为SBC并不十分困难。要让Jetson Nano和OpenCR板通过ROS进行对话，需要执行以下步骤。

在OpenCR板上安装/更新固件

OpenCR嵌入式平台板负责为DYNAMIXEL电机供电并与之通信。这意味着它从电机读取编码器读数并生成里程计信息，以及接收速度命令并将其发送给这些电机，这两者对该项目都至关重要。我们遵循了这里提供的说明更新OpenCR板的固件。

在Jetson Nano上安装TurtleBot ROS堆栈

这个Jetson Nano已经在其他机器人项目中用过，我们已经在它上面安装了ROS Melodic。然而，为了让它与OpenCR板一起工作，我们需要将这个板设置为TurtleBot3的大脑。为此，我们在Jetson Nano的新ROS工作区中设置了TB3 ROS堆栈。通过以下代码可以轻松实现：

sudo apt install ros-melodic-turtlebot3

现在，我们可以通过micro-USB将Jetson Nano连接到OpenCR板。

使用调出TB3堆栈roslaunch turtlebot3_bringup turtlebot3_robot.launch。现在，我们已经准备好接收里程表，并从Jetson Nano向TB3发送速度命令。

接收里程表和发送速度命令

我们可以启动遥操作节点来控制马达，并查看里程计的更新。这可以通过使用roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
为了查看流式里程计值，我们使用了rostopic echo /odom. 示例里程计消息如下所示：

---
header: 
  seq: 987
  stamp: 
    secs: 1597031283
    nsecs: 652199083
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: -0.147986590862
      y: -0.0244562737644
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.179800972342
      w: 0.983703017235
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: -0.11722844094
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.217905953526
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

至此，我们的SBC和TB3的设置完成。下一步是将OAK-D添加到这个设置中。

从OAK-D获取深度图

OAK-D相机由三个摄像头组成——两个用于提供深度感知的黑白全局快门相机和一个RGB相机。OAK-D提供了两种不同的方法来估计深度。第一种方法使用半全局匹配（SGBM）算法使用左右摄像头，同时在一个摄像头(左、右或RGB)上运行神经推理来执行对象检测。深度图随后用于在相机的参照系中定位检测到的物体。第二种方法在左和右摄像头上并行运行神经网络推断，以估计两幅图像之间的差异。然后，检测到的特征使用相机的固有特性进行三角测量，以计算它们的3D位置。在这个项目中，我们将利用第一种方法，因为它同时为我们提供了两种方法——密集深度图（用于创建点云）和3D对象检测（在SLAM问题中用作地标）。

图4：OAK-D相机的深度图样本。如图所示，它可以检测物体（这里是一个人），还可以报告它们在相机坐标框架中的3D位置

在我们开始从OAK-D传输深度图之前，我们首先需要获得内在的相机校准矩阵，这对于将深度图转换为点云是必不可少的。如果你用的是有外壳的OAK-D，那你就不用校准相机了。我们通过以下方法获得OAK-D的内在参数，校准教程来自这里。

Python 3和C++提供了用于从OAK-D传输数据的DepthAI API。由于我们的ROS版本（Melodic）不支持Python 3，我们决定使用C++ API。我们遵循提供的示例是经过反复试验，在DepthAI开发人员的帮助下，我们能够构建一个能够从OAK-D传输深度图的程序。这些深度图将通过ROS发布，供下游节点使用。在接下来的几周里，我们将致力于提高这些深度图的质量。

将深度图转换为点云

使用OAK-D的好处之一是它能够生成原始深度图，深度图的每个像素包含估计的深度信息。通常，当我们知道相机的内在校准矩阵（我们称之为K），我们可以将场景中的3D点投影到2D图像平面上。这个过程被称为透视投影。

3×3内在校准矩阵由4个不同的参数组成：

fᵤ 和 fᵥ 是水平和垂直焦距。它们是从相机中心到图像平面的距离测量值，以像素为单位。
oᵤ 和oᵥ是代表像平面光学中心的主要点。

给定一个具有坐标的3D点(X，Y，Z)在相机坐标框架中，当投影到图像平面上时，我们可以用以下等式检索像素位置：

上面的等式告诉我们，我们也可以逆向工程带坐标的2D像素(u，v)如果我们知道固有的校准矩阵和深度，或者Z的像素坐标。求解X和Y，我们得到下面的等式：

为了构建由以下部分组成的点云N点数（其中N等于2D深度图中的像素数），你可以简单地访问深度图中的每个像素并计算（X，Y，Z）该点的逆投影。然而，使用双for循环访问每个像素的成本会非常高。因此，我们可以使用线性代数以一种计算更优化的方式来解决同一问题。

根据上述等式，我们只需找到固有校准矩阵的逆矩阵，然后执行矩阵乘法。基于上面的等式，我们可以导出K⁻。

这个过程使我们能够将来自OAK-D的深度图转换成点云，使用这个过程生成的样本点云如图5所示。这个点云没有经过二次采样或清理，也没有从原始深度图中生成，这就是质量不高的原因。这将在未来改进。

Tags: #ROS, #slam

【PART 1】OAK-D+TurtleBot3机器人项目全解析：SLAM、ROS、深度图、点云。