OAK-D机器人项目全解析:SLAM、ROS、深度图、点云。

OAK-D机器人项目全解析:SLAM、ROS、深度图、点云。

来源:Deepak Talwar,编辑:OAK中国。

前言

著名的开源计算机视觉库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问题是一个优化问题,它使用移动代理的多个姿态估计源,并通过调整它们来使得这些估计之间的误差最小化。这种优化使我们能够在其环境中定位代理,同时使我们能够拼接机器人的传感器测量值,以创建环境的地图表示。

图1:项目的流程图

大多数室内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。该机器人必须符合以下标准:

  1. 它应该提供一个轮式里程表的来源。虽然我们可以自己添加这个,但因为这不是这个项目的主要任务,最好是使用一个已经有编码器电机的平台。
  2. 支持机器人操作系统。我们的目标是使用ROS作为这个项目的主干,所以一个支持ROS的机器人是必须的。
  3. 它应该能够为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板上安装/更新固件

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

在Jetson Nano上安装TurtleBot ROS堆栈

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

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

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

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

  1. 我们可以启动遥操作节点来控制马达,并查看里程计的更新。这可以通过使用roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
  2. 为了查看流式里程计值,我们使用了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问题中用作地标)。

图3: OAK-D相机,两个黑白相机用于深度,一个RGB相机位于中间。
图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个不同的参数组成:

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

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

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

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

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

图5:OAK-D深度图生成的样本点云

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