在FPGA开发板上实现基于立体视觉的 SLAM。

绪论

SLAM（同步定位和地图绘制）在自动驾驶、AGV 和无人机等各种应用中引起了人们的广泛关注。尽管目前有很多优秀的 SLAM 项目可以参考，但是他们的复杂性（高性能）及依赖性（依赖于许多外部库），使得它们无法移植到简单的平台（例如嵌入式系统）。

该项目更加重视简洁的算法和更少的依赖性。很多不开源的库也将被删除。另一方面，利用FPGA加速来达到实时的处理速度。

功能

• 10 FPS实时运行

• 闭环检测

• 3D占用网格地图生成

• 通过 USB 3.0 连接进行实时监控

• 软件和硬件的所有设计文件均开源

GitHub

项目很复杂，感兴趣的不会太多，提前放出代码

所有设计文件都包含在以下 GitHub 中。

❝

https://github.com/sdoira/U96-SLAM

❞

• bin --- 预构建的二进制文件

• doc---相关文件

• src --- 源文件

• vivado --- Vivado 工程目录

系统概览

系统级框图如下所示。

传感器板

传感器板连接到FPGA开发板（ Ultra96-V2 ）以捕获立体图像。该板硬件是开源的，开源链接如下：

❝

https://github.com/sdoira/U96-SVM

❞

该板包含双 CMOS 图像传感器和两个 mikroBUS 。

在两个mikroBUS站点中的一个站点上安装了一个带有内置LED（按钮G点击）的单按钮开关的模块，用于在独立模式下控制系统。IMU模块也已安装，但未在本项目中使用。

图像格式为 640x480 ，30 FPS。然后将帧速率降低到 FPGA 内的所需速率。

迁移到其他传感器板

该传感器板的设计符合 Ultra96-V2 规范。如果其他传感器板也符合这些规范，则应该可以迁移到其他传感器板。图像传感器配置为 640x480 分辨率和 30 FPS。一个按钮开关和一个 LED 连接到 FPGA。

FPGA

图像传感器连接到 FPGA（或可编程逻辑，PL端）。用于立体视觉的图像处理，如立体校正和块匹配（stereo rectification 和 block matching）。FPGA也被用作一些功能的硬件加速。

远程申请

裸机应用程序在两个 R5 处理器之一上运行，用来控制 FPGA。此应用程序在本文中也称为“远程应用程序”。此应用程序与 Linux 应用程序协同工作。此应用程序还控制 USB 3.0 连接，因此如果板卡连接到 Windows PC，此系统就像是具有某些立体视觉功能的 USB 网络摄像头一样工作。

Linux应用

Petalinux 系统建立在四个 A53 处理器上。在该系统上运行处理 SLAM 相关操作的应用程序。该应用程序在本文中称为“Linux 应用程序”。

Petalinux 系统以 SMP（Symmetric Multiprocessing）模式运行。这意味着工作负载由 Linux 系统分配给每个处理器。

处理器间通信 (IPC)

这两个应用程序通过在 FPGA 中实现的内存映射寄存器相互通信。这些寄存器由“消息”寄存器和“参数”寄存器组成。处理器在“消息”寄存器中写入特定的消息 ID 以通知对方。另一个 CPU 轮询“消息”寄存器并做出适当的响应。如有必要，可以发送四个 32 位参数。

调试电脑

调试电脑用于监控板子的状态。除了通过 UART 进行的调试功能外，还可以通过 USB 3.0 连接实时查看视频处理中的立体图像。当连接到 Windows PC 时，此系统被视为 UVC（USB Video Class）设备，因此不需要特殊的设备驱动程序。

内存映射

FPGA开发板上有 2GB 的物理内存。该区域的前 3 / 4 被 Linux 系统使用。另一个保留给远程应用程序。

开发环境

主要开发在 Windows 上执行，但 Petalinux 开发需要 Linux 环境。所以使用VirtualBox在Windows 10上虚拟搭建一个Linux环境。

此项目需要安装两个 Vitis 。Windows 上的一个用于远程应用程序开发，另一个用于 Linux 应用程序。

开发阶段

嵌入式系统的开发比较麻烦，所以需要划分为三个阶段。

第 1 阶段是在 Windows上进行纯软件解决方案开发。这个阶段对于软件开发是最有效的。算法的性能也在这个阶段得到了验证。

在第 2 阶段，软件被移植到运行在开发板板上的 Petalinux 系统。在这个过渡阶段，注意软件源代码是相同的。板载 SD 卡用于存储数据。

在最后阶段，一些功能被FPGA电路和控制FPGA的裸机应用所取代。一些功能还应用了硬件加速，以进一步减少处理时间。

算法（传感器数据采集）

立体校正

立体校正过程将左右图像在同一平面上进行变换，并使它们水平对齐。立体校正在 FPGA 内部实时执行，然后在存储到 DDR 内存之前进行双线性插值。

为了使 FPGA 电路更简单，支持信息由软件预先生成。此信息包括要处理的数据的位置和长度，并按到达时间的顺序排序。

立体校正参数通过 OpenCV 函数获得，该函数使用 7x5 棋盘图案实现 Bouguet 算法。这些参数保存在 XML 文件中并存储在 SD 卡上。

当前的实现忽略了镜头畸变，因为使用的图像传感器几乎没有畸变，而试图消除它们的畸变会导致图像产生更多畸变。

X-Sobel 滤波器

X-Sobel 滤波器用作块匹配的预处理，结果存储在 DDR 内存中。

块匹配

块匹配搜索立体图像对之间的视觉对应关系。立体校正后，左图中的一个位置出现在右图中同一行的左侧。源图像中每个像素的这些差异形成了密集的深度图。

块匹配是通过移植OpenCV的StereoBM功能在FPGA中实现的。块匹配所需的计算量非常大，但可以通过 OpenCV 中实现的“滑动窗口”技术来减少。为了进一步减少处理时间，FPGA 并行计算 32 个视差。

GFTT探测器

GFTT（Good Features To Track）用于检测关键点。关键点是图像中通常包含角的独特部分。

该算法与OpenCV的goodFeaturesToTrack函数相同，但部分函数移植到FPGA中实现，以减少软件处理时间。

该功能由以下步骤组成。

• 应用XY-Sobel滤波器提取边缘

• 计算特征值量化角点的尖锐度

• 对特征值应用阈值并选择好的关键点

步骤1和步骤2需要对图像中的每个像素都进行计算，计算量较大，因此采用FPGA实现。

ORB 描述符生成器

ORB（Oriented FAST and Rotated BRIEF）特征描述符用于量化检测到的关键点的视觉唯一性。同一物体的关键点具有相似的描述符，因此即使比例和角度略有不同，我们也可以从不同的图像帧中搜索同一物体的关键点。

实际计算由 OpenCV 函数执行。每个 ORB 描述符都是一个 256 位的二进制字符串。

算法（SLAM）

SLAM 算法是根据RTAB-Map中实现的 F2F 算法（http://introlab.github.io/rtabmap/）构建的。

坐标

该项目涉及两个坐标系。它们是图像坐标和world (或者 robot)坐标。

这两个都是右手坐标系，所以一个简单的旋转矩阵R就可以在它们之间进行转换。源图像在图像坐标中捕获。然后将计算出的相机位姿转换为world 坐标。

视觉里程计Visual Odometry

视觉里程计计算连续图像帧期间相机姿势的转换。

该算法由以下阶段组成。

1.关键帧选择

实际视觉里程计是在关键帧和新图像帧之间计算的。使用关键帧的原因是为了减少累积每一帧的测距误差，尤其是当相机靠近固定位置时。当匹配的关键点数量低于阈值时，关键帧将被更新。

2. 关键点匹配

关键点在两个图像帧之间匹配。通过比较关键点的 ORB 描述符来计算相似度。以前的相机姿势（如果可用）用于缩小搜索范围。此阶段的输出是匹配关键点的 ID 及其 2D/3D 位置。

3.运动估计

解决 PnP 问题以计算相机的旋转和平移，从而最大限度地减少两者之间的误差

• 投影到当前图像平面上的参考帧中关键点的 3D 位置，以及当前帧中关键点的二维位置。

输出是相机的相对运动。它在图像坐标中计算，然后转换为world坐标。

在这个项目中，相机姿势是使用图表来描述的。估计的相机姿势和运动分别作为节点和链接添加到图中。

视觉关键词Visual Word Dictionary

视觉关键词包含视觉词，它们实际上是分配有唯一 ID 的 ORB 描述符。每次新的图像帧到达时，该帧中包含的 ORB 描述符都会与现有的视觉词相匹配。如果它与现有单词匹配，则增加该单词的引用计数器。如果不是，则描述符被分配一个新的 ID 并成为一个新的视觉词。

视觉词的数量随时间增加。与所有现有的视觉词匹配实际上是这个应用程序中最耗时的过程。为了让软件实时运行，这个计算在一个单独的线程中处理。因为闭环检测不一定在每一帧中运行，所以这一操作很有效，。

闭环检测

闭环检测是识别先前访问过的场景并向该节点添加另一个链接。

向图形添加闭环链接可以通过两种方式减少图形错误。

1. 添加闭环链接时会重建图。在这个过程中，连接了从起始节点到结束节点的最短路径。这将消除循环期间累积的里程计误差。

1. 闭环链接将为图形添加额外的约束。通过最小化由此类约束引起的误差，将提高估计姿势的准确性。

默认情况下，闭环检测每 5 帧运行一次。最新的 30 帧也将被忽略。这些抽取是为了消除相邻节点被接受为闭环，因为它们对图优化几乎没有贡献。

每当一个新的图像帧到达时，TF-IDF（词频-逆文档频率）分数就会通过查阅视觉词典来计算其他图像帧的分数。当更多的单词包含在共同点时，这个分数会更高，并且单词越稀有。

选择具有最高 TF-IDF 分数的图像帧作为闭环的候选者。然后，在两个图像帧之间执行类似于视觉里程计中的运动估计。当重投影误差低于阈值时，该链接被接受为闭环链接并添加到图中。

图形优化

当闭环链接向图形添加额外约束时，会出现差异，从而导致图形中出现错误。通过最小化此类错误，可以提高图表的准确性。

在这个项目中，只估计相机位姿。这称为“姿态调整”，与“束调整”相对，后者估计相机姿态和观察点的 3D 坐标。

这类问题可以通过最小化这种形式的成本函数 F(x) 来解决。

这里 eij 被定义为位姿 xi 和 xj 之间的误差向量，而 zij 是它们之间的约束。Ω为信息矩阵，由重投影误差的协方差的倒数得到。

F(x) 的一阶近似值通过围绕 x 初始值的泰勒级数展开如下给出。

Jij 是关于 xi 和 xj 的雅可比矩阵。

F(x) 由 x 最小化，x 是通过求解以下等式获得的，其中 λ 是倾销因子。

将 Δx 添加到初始值以更接近最优解。

雅可比计算遵循g2o的实现（https://github.com/RainerKuemmerle/g2o）。原始相机姿态包括旋转矩阵，它们不能直接放入方程中，因为它们是一种过度参数化的表示。在这个项目中，归一化四元数的轴用作最小表示。这也遵循 g2o 的实现，因此我们可以对雅可比矩阵使用相同的计算。因此相机姿势将是 6 个元素的向量。

假设我们有 N 个节点，那么 H 的大小是 6N x 6N，向量 b 是 6N。矩阵 H 可以非常大，但其元素大部分为零，因为 H 仅在对应节点之间存在约束的情况下才为非零。因此，将 H 视为稀疏矩阵是有利的。构建稀疏矩阵和求解方程由Eigen执行（https://eigen.tuxfamily.org/），SimplicialLDLT 作为稀疏线性求解器。

占用网格图

3D 占用网格图是从优化的位姿图和密集的深度图生成的。地图的实际生成由Octomap执行。结果以“二叉树（.bt）”格式存储在 SD 卡上。

表现

KITTI 数据集（https://www.cvlibs.net/datasets/kitti/）在验证算法时用作参考。

准确性

下图是用KITTI数据集序列00模拟时的轨迹鸟瞰图。

在此仿真中，主要在 5 个区域检测到闭环，平移和旋转误差分别为 0.91% 和 0.0038 度/米。

请注意，此结果仅显示算法的性能，因为此仿真中使用的图像是由不同的图像传感器捕获的。

3D 占用网格地图

下图是在上述模拟中生成并由octovis（https://github.com/OctoMap/octomap/tree/devel/octovis）显示的 3D 占用网格图。密集深度图的分辨率在两个方向上都降低了 1 / 4，然后在通过 Octomap 构建体素图之前通过估计的相机姿势进行投影。

处理时间

下图显示了处理图像传感器输入时应用程序和 FPGA 主线程的处理时间。

视觉关键词更新和闭环检测在应用程序的子线程中运行。处理时间随着视觉词的数量增加，如下所示。

时隙为 500 毫秒，因为它们每 5 帧运行一次。当处理时间超过这个时隙时，下一次执行将推迟到上一个线程完成，这样就不会干扰视觉里程计的实时运行。

内存消耗

下图显示了在 Windows 上处理 KITTI 数据集序列 00 时的内存消耗（仅显示前 1700 帧）。内存消耗随着时间的推移而增加，其中大部分是密集的深度图和视觉词。当应用程序运行在FPGA上时，这块内存占用了Linux控制的内存空间，限制了连续运行的时间。

FPGA利用率

下表显示了 FPGA 资源利用率。FPGA设备是 XCZU3EG-SBVA484-1-I。

如何复现

先决条件

• Xilinx Tools 2020.2 必须安装在两个平台（Ubuntu和Windows）上。

• Petalinux 2020.2 必须安装在 Ubuntu 上。

• 假定 Xilinx Tools 安装到 Ubuntu 上的 [XILINX_DIR]。

• 假定 git 中的必要文件已复制到两个平台。

• 下载 Eigen 3.4.0 并将其放置在“slam/include”目录下，目录结构如下：

slam
 └─include
       └─Eigen

硬件

传感器板的扩展接口是开漏电路，不能直接控制开关和LED。因此，在 Button G click board 上需要进行以下修改。

构建 FPGA 项目（在 Windows 上）

git文件中已经构建好项目。

“dvp”项目

启动 Vivado，并在“Tcl Console”中键入以下命令。

cd [WORK_DIR]/U96-SLAM/vivado
source create_dvp.tcl

⇒ 将创建名为“dvp”的项目。

点击“工具→创建并打包新IP...”，打开“创建并打包新IP”对话框。继续进行以下设置。

[Create Peripheral, Package IP or Package a Block Design]
  Packaging Options: Package your current project
[Package Your Current Project]
  IP location: [WORK_DIR]/U96-SLAM/src/ip_repo/dvp

出现提示时选择“是”，然后单击“完成”。

将出现“Package IP - dvp”。

选择“Review and Package”，点击“Package IP”。

⇒ “dvp”的 IP 源将导出到“ip_repo/dvp”目录。

关闭“dvp”项目。

“fpga_top”项目

启动 Vivado，并在“Tcl Console”中键入以下命令。

cd [WORK_DIR]/U96-SLAM/vivado
source create_fpga_top.tcl

⇒ 将创建名为“fpga_top”的项目。

双击“Sources”面板中的“Design Sources→design_1_wrapper→design_1_i”，打开“design_1.bd”框图。

如果“/dvp_0 block in this design should be upgraded.” 显示在窗口顶部，单击“Report IP Status”，然后单击“Upgrade Selected”。只有当修改了“dvp”模块时才会发生这种情况。

单击 Flow Navigator 中的“Generate Bitstream”。

⇒ “design_1.bit”将在“fpga_top.runs/impl_1/”目录中创建。

单击“File→Export→Export Hardware”打开“Export Hardware Platform”对话框。继续进行以下设置。

[Output]
  Include bitstream: Selected
[Files]
  XSA file name: design_1_wrapper
  Export to: [WORK_DIR]/U96-SLAM/vivado/fpga_top

⇒ “design_1_wrapper.xsa”将在指定目录中创建。

构建裸机应用程序（在 Windows 上）

• 只有在修改裸机应用程序时才需要此项目。“StereoBM.elf”已经包含在 git 存储库中。

在“[WORK_DIR]/U96-SLAM”下创建名为“vitis”的目录。启动 Vitis，将此目录设置为 Vitis Workspace，然后单击“Launch”。

[WORK_DIR]/U96-SLAM/vitis

单击“Create Application Project”以打开“新建应用程序项目”对话框。继续进行以下设置。注意选择R5处理器。

[Platform]
  Create a new platform from hardware (XSA)
  XSA File: [WORK_DIR]\U96-SLAM\vivado\fpga_top\design_1_wrapper.xsa
  Target processor to create FSBL: psu_cortexr5_0
[Application Project Details]
  Application project name: StereoBM
  Target processor: psu_cortexr5_0
[Domain]
  Remain as default
[Templates]
  SW development templates: Empty Application

单击“完成”。

⇒ 将创建“StereoBM_system”项目。

在“Application Project Settings”中，单击“Navigate to BSP Settings”。

点击“Board Support Package”中的“Modify BSP Settings...”。“板级支持包设置”对话框将打开。

点击“Overview→standalone”，进行如下修改。

stdin: psu_uart_1
stdout: psu_uart_1

这是必要的，因为 uart_1 在 开发板中用作标准输入/输出。

在“Explorer”中右键单击“StereoBM_system→StereoBM→src”，然后从菜单中单击“Import Sources...”以打开“Import Sources”对话框。继续进行以下设置。

From directory: [WORK_DIR]/U96-SLAM/src/StereoBM/src
Select All: click

单击“完成”。

在资源管理器窗格中选择“StereoBM”，然后通过单击 Hammer 图标旁边的箭头图标选择“Release”构建。

⇒ “StereoBM.elf”将在“Release”目录中生成。

• 构建“StereoBM_system”而不是“StereoBM”也会生成 ROM 引导文件。

构建 Petalinux 系统（在 Ubuntu 上）

配置系统

获取 Petalinux 环境。

source [XILINX_DIR]/petaLinux-2020.2/bin/settings.sh

通过键入以下命令创建“petalinux”项目。

cd [WORK_DIR]/U96-SLAM
petalinux-create --type project --template zynqMP --name petalinux
cd petalinux/

⇒ “petalinux”目录将在 [WORK_DIR]/U96-SLAM/” 下创建。

将“design_1_wrapper.xsa”复制到“U96-SLAM/vivado”目录。如果没有更改 FPGA 设计，则在 git 存储库的“U96-SLAM/bin”目录中提供预构建文件。

如下配置 Petalinux 系统。

• 以下“petalinux-xxxx”命令必须在“petalinux”目录中发出。

petalinux-config --get-hw-description ../vivado

“misc/config 系统配置”对话框将打开。进行以下设置，然后“退出”。

Subsystem AUTO Hardware Settings → Serial Settings →
  PMUFW Serial stdin/stdout : psu_uart_1
  FSBL Serial stdin/stdout: psu_uart_1
  ATF Serial stdin/stdout: psu_uart_1
  DTG Serial stdin/stdout: psu_uart_1
DTG Settings → MACHINE_NAME: avnet-ultra96-rev1
Image Packaging Configuration → Root filesystem type: EXT4 (SD/eMMC/SATA/USB)

• 以下命令第一次运行可能需要很长时间。

petalinux-config -c kernel
“Linux/arm64 5.4.0 内核配置”对话框将打开。进行以下设置，然后“退出”。

Enable loadable module support [*] (default)
Networking support → Bluetooth subsystem support < >
Device Drivers → Remoteproc drivers →
  Support for Remote Processor subsystem [*] (default)
  ZynqMP_r5 remoteproc support <M> (default)

最后，键入以下配置命令。

petalinux-config -c rootfs

“Configuration”对话框将打开。进行以下设置，然后“退出”。

Filesystem Packages →
  libs → libmetal → libmetal [*]
  misc → gdb [*] (for debug purpose)
       → sysfsutils → libsysfs [*]
Petalinux Package Groups →
  packagegroup-petalinux-openamp → packagegroup-petalinux-openamp [*]
  packagegroup-petalinux-opencv → packagegroup-petalinux-opencv [*]
Image Features → auto-login [*]

在“[WORK_DIR]/U96-SLAM/petalinux/project-spec/meta-user/recipes-bsp/device-tree/files/”中打开“system-user.dtsi”并复制并粘贴以下文本。

/include/ "system-conf.dtsi"
/ {
reserved-memory {
        #address-cells = <2>;
        #size-cells = <2>;
        ranges;
        rproc_0_dma: rproc@0x6ed00000 {
            no-map;
            compatible = "shared-dma-pool";
            reg = <0x0 0x6ed00000 0x0 0x00100000>;
        };
        rproc_0_reserved: rproc@0x5ed00000 {
            no-map;
            reg = <0x0 0x5ed00000 0x0 0x10000000>;
        };
    };
    zynqmp-rpu {
        compatible = "xlnx,zynqmp-r5-remoteproc-1.0";
        #address-cells = <2>;
        #size-cells = <2>;
        ranges;
        core_conf = "split";
        r5_0: r5@0 {
            #address-cells = <2>;
            #size-cells = <2>;
            ranges;
            memory-region = <&rproc_0_reserved>, <&rproc_0_dma>;
            pnode-id = <0x7>;
            mboxes = <&ipi_mailbox_rpu0 0>, <&ipi_mailbox_rpu0 1>;
            mbox-names = "tx", "rx";
            tcm_0_a: tcm_0@0 {
                reg = <0x0 0xFFE00000 0x0 0x10000>;
                pnode-id = <0xf>;
            };
            tcm_0_b: tcm_0@1 {
                reg = <0x0 0xFFE20000 0x0 0x10000>;
                pnode-id = <0x10>;
            };
        };
    };
    zynqmp_ipi1 {
        compatible = "xlnx,zynqmp-ipi-mailbox";
        interrupt-parent = <&gic>;
        interrupts = <0 29 4>;
        xlnx,ipi-id = <7>;
        #address-cells = <1>;
        #size-cells = <1>;
        ranges;
        /* APU<->RPU0 IPI mailbox controller */
        ipi_mailbox_rpu0: mailbox@ff90000 {
            reg = <0xff990600 0x20>,
                  <0xff990620 0x20>,
                  <0xff9900c0 0x20>,
                  <0xff9900e0 0x20>;
            reg-names = "local_request_region",
                        "local_response_region",
                        "remote_request_region",
                        "remote_response_region";
            #mbox-cells = <1>;
            xlnx,ipi-id = <1>;
        };
    };
    chosen {
        bootargs = "console=ttyPS0,115200 root=/dev/mmcblk0p2 rw earlyprintk rootfstype=ext4 rootwait uio_pdrv_genirq.of_id=generic-uio devtmpfs.mount=1 earlycon";
    };
};

&dvp_0 {
  compatible = "generic-uio";
};

该文件声明使用远程处理器并保留其内存空间。该文件还将FPGA内部的“dvp”模块设置为“generic-uio”设备，以便我们可以使用内置的“generic-uio”设备驱动程序访问它。

自动运行应用程序

以下过程将使我们的应用程序在系统启动时自动运行。

petalinux-create -t apps --template install -n myinit --enable

⇒ “myinit”目录将在“project-spec/meta-user/recipes-apps”下创建。

将以下文本复制并粘贴到“myinit.bb”和“files/myinit”。

【myinit.bb】

#
# This file is the myapp-init recipe.
#
SUMMARY = "Simple myinit application"
SECTION = "PETALINUX/apps"
LICENSE = "MIT"
LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/MIT;md5=0835ade698e0bcf8506ecda2f7b4f302"

SRC_URI = "file://myinit \
 "

S = "${WORKDIR}"

FILESEXTRAPATHS_prepend := "${THISDIR}/files:"
inherit update-rc.d
INITSCRIPT_NAME = "myinit"
INITSCRIPT_PARAMS = "start 99 S ."

do_install() {
 install -d ${D}${sysconfdir}/init.d
 install -m 0755 ${S}/myinit ${D}${sysconfdir}/init.d/myinit
}

FILES_${PN} += "${sysconfdir}/*"

【文件/myinit】

#!/bin/sh
cd ~/home/root
./run

这些文件使“myinit”成为一个启动应用程序，它将在“home/root”目录中执行“run”脚本。

构建 Petalinux

现在我们可以通过以下命令构建 Petalinux 系统。

• “petalinux-build”命令可能需要很长时间。

petalinux-build

第一次会出现错误消息，说明设备树中存在错误。然后，打开以下目录中的“pl.dtsi”，删除mipi_csi_rx_subsyst_0和mipi_csi_rx_subsyst_1条目。

“[WORK_DIR]/U96-SLAM/petalinux/components/plnx_workspace/device-tree/device-tree/pl.dtsi”

生成的文件应如下所示。

【pl.dtsi】

/ {
 amba_pl: amba_pl@0 {
  #address-cells = <2>;
  #size-cells = <2>;
  compatible = "simple-bus";
  ranges ;
  dvp_0: dvp@a0000000 {
   clock-names = "s00_axi_aclk", "m00_axi_aclk";
   clocks = <&zynqmp_clk 71>, <&zynqmp_clk 71>;
   compatible = "xlnx,dvp-1.0";
   interrupt-names = "intr";
   interrupt-parent = <&gic>;
   interrupts = <0 89 4>;
   reg = <0x0 0xa0000000 0x0 0x10000>;
   xlnx,m00-axi-addr-width = <0x20>;
   xlnx,m00-axi-aruser-width = <0x0>;
   xlnx,m00-axi-awuser-width = <0x0>;
   xlnx,m00-axi-burst-len = <0x10>;
   xlnx,m00-axi-buser-width = <0x0>;
   xlnx,m00-axi-data-width = <0x20>;
   xlnx,m00-axi-id-width = <0x1>;
   xlnx,m00-axi-ruser-width = <0x0>;
   xlnx,m00-axi-target-slave-base-addr = <0x40000000>;
   xlnx,m00-axi-wuser-width = <0x0>;
  };
  misc_clk_0: misc_clk_0 {
   #clock-cells = <0>;
   clock-frequency = <200000000>;
   compatible = "fixed-clock";
  };
  misc_clk_1: misc_clk_1 {
   #clock-cells = <0>;
   clock-frequency = <1500000000>;
   compatible = "fixed-clock";
  };
 };
};

CSI 接口似乎会自动添加到设备树中，但我们在这里不需要它们，因为它们由裸机应用程序控制。

• 此文件是自动生成的，不应手动编辑，但我找不到其他方式解决上面的问题。每次编辑“system-user.dtsi”时，此问题仍然存在。

然后再次构建 Petalinux 系统。

petalinux-build

这次项目应该构建成功了。

创建SDK

键入以下命令为平台项目创建 SDK。

petalinux-build --sdk

这将在“/images/linux/”目录中生成“sdk.sh”。

然后键入以下命令以在当前位置解压“sdk.sh”。

cd images/linux
petalinux-package --sysroot

构建平台项目（在 Ubuntu 上）

在“petalinux/images/linux/”目录下创建“linux.bif”文件。然后复制并粘贴以下文本。

【linux.bif】

/* linux */
the_ROM_image:
{
    [fsbl_config] a53_x64
    [bootloader] <zynqmp_fsbl.elf>
    [pmufw_image] <pmufw.elf>
    [destination_device=pl] <bitstream>
    [destination_cpu=a53-0, exception_level=el-3, trustzone] <bl31.elf>
    [destination_cpu=a53-0, exception_level=el-2] <u-boot.elf>
}

启动 Vitis，并选择“[WORK_DIR]/U96-SLAM/vitis”作为其工作区。

点击“File→New→Platform Project...”打开“New platform project”对话框。继续进行以下设置。

[Create new platform project]
  Platform project name: platform
[Platform]
  Choose "Create a new platform from hardware (XSA)".
  XSA File: [WORK_DIR]/U96-SLAM/vivado/fpga_top/design_1_wrapper.xsa
  Operating system: linux
  Processor: psu_cortexa53
  Architecture: 64-bit
  Generate boot components: checked
  Target processor to create FSBL: psu_cortexa53_0

单击“完成”。

在左窗格中选择“platform→psu_cortexa53→linux on psu_cortexa53”。

在“域：linux_domain”对话框中填写以下信息。

Bif File                 : [WORK_DIR]/U96-SLAM/petalinux/images/linux/linux.bif
Boot Components Directory: [WORK_DIR]/U96-SLAM/petalinux/images/linux/
Linux Image Directory    : [WORK_DIR]/U96-SLAM/petalinux/images/linux/
Linux Rootfs             : [WORK_DIR]/U96-SLAM/petalinux/images/linux/rootfs.tar.gz
Sysroot Directory        : [WORK_DIR]/U96-SLAM/petalinux/images/linux/sdk/sysroots/aarch64-xilinx-linux

选择“平台→psu_cortexa53_0→zynqmp_fsbl→板级支持包”

单击“修改 BSP 设置...”。

选择“Overview → standalone”，修改如下。

stdin : psu_uart_1
stdout : psu_uart_1

单击“确定”。

通过单击锤子图标构建项目。

⇒ 将在 Vitis 工作区中创建一个名为“platform”的项目。

现在我们准备构建一个运行在该平台上的 Linux 应用程序。

构建 SLAM 应用程序 (Ubuntu)

启动 Vitis，并选择“[WORK_DIR]/U96-SLAM/vitis”作为其工作区。

点击“File→New→Application Project...”打开“New Application Projec”对话框。

继续进行以下设置。

[Platform]
  Select a platform from repository: platform [custom]
[Application Project Details]
  Application project name: slam
[Domain]
  Remain as default.
[Templates]
  SW development templates: Empty Application (C++)

• 如果在 git 控制的目录中创建 Vitis 工作区，则可能无法识别平台项目。如果发生这种情况，请尝试在 git 控制的目录之外的某个位置创建 Vitis 工作区。

单击“完成”。

⇒ 将创建名为“slam”的项目。

将 git 存储库中“vitis/slam”中的“src”和“include”目录复制到 [WORK_DIR]/U96-SLAM/vitis/slam/”目录。

单击锤子图标旁边的箭头图标并选择“Release”。

在“Explorer”中右击“slam”，选择“C/C++ Build Settings”。设置如下。

[ARM v8 Linux g++ compiler]
 ├─Directories
 │  └─Include Paths
 │     [WORK_DIR]/U96-SLAM/petalinux/images/linux/sdk/sysroots/aarch64-xilinx-linux/usr/include
 │     [WORK_DIR]/U96-SLAM/vitis/slam/include
 └─Miscellaneous
    -c -fmessage-length=0 -MT"$@" -ftemplate-backtrace-limit=0
[ARM v8 Linux g++ linker]
 └─Libraries
   └─Libraries
      opencv_core
      opencv_photo
      opencv_video
      opencv_videoio
      opencv_optflow
      opencv_tracking
      opencv_features2d
      opencv_imgcodecs
      opencv_highgui
      opencv_imgproc
      opencv_calib3d
      pthread

右键单击“Explorer”中的“slam”，然后单击“Clean Project”。

再次右键单击“slam”并单击“Build Project”。

⇒ 将生成“Release/slam.elf”。

准备 SD 卡（在Ubuntu 上）

SD 卡使用 GParted 格式化，如下图所示。

■ 引导文件

如果更改了 FPGA 设计，请将“design_1_wrapper.bit”从 Windows 复制到 Ubuntu。以下命令假定“.bit”文件位于“/vivado”目录中。

键入以下命令以创建“BOOT.BIN”。

cd [WORK_DIR]/U96-SLAM/petalinux
petalinux-package --boot --force --fsbl images/linux/zynqmp_fsbl.elf --fpga ../vivado/design_1_wrapper.bit --u-boot

⇒ “BOOT.BIN”将在“/petalinux/images/linux/”目录中生成。

将以下3个文件复制到SD卡的BOOT目录下。

[WORK_DIR]/U96-SLAM/petalinux/images/linux/boot.scr
                                           BOOT.BIN
                                           image.ub

■ 系统文件

通过以下命令将“rootfs.tar.gz”解压到SD卡的“root”目录下。

sudo tar xzvf [WORK_DIR]/U96-SLAM/petalinux/imeges/linux/rootfs.tar.gz -C [SD_CARD_DIR]/root

在“[SD_CARD_DIR]/root/lib/”目录下创建“firmware”目录。

将“StereoBM.elf”和“slam.elf”复制到上述目录。

运行应用程序

根据自动运行设置，会自动执行SD卡上“root/home/root/”目录下的“run”脚本。

创建一个名为“run”的文件并赋予其执行权限。

chmod 774 run

然后，复制粘贴以下内容，将文件移动到SD卡的“root/home/root/”目录下。

【home/root/run】

rm *.csv
rm *.bmp
rm *.png
rm *.jpg
rm *.txt
rm -rf work
echo StereoBM.elf > /sys/class/remoteproc/remoteproc0/firmware
echo start > /sys/class/remoteproc/remoteproc0/state
#/lib/firmware/slam.elf -app "STEREO_CAPTURE" -lc "calib_left.yml" -rc "calib_right.yml"
#/lib/firmware/slam.elf -app "FRAME_GRABBER"
#/lib/firmware/slam.elf -app "SLAM_BATCH" -dir "kitti/sequences/00" -l "image_0" -r "image_1" -t "times.txt" -gt "../../poses/00.txt" -lc "calib.txt" -n 100
/lib/firmware/slam.elf -app "SLAM_REALTIME" -lc "calib_left.yml" -rc "calib_right.yml"
shutdown -h now

“echo”命令与将在远程处理器上启动“StereoBM”应用程序的 OpenAMP 相关。“StereoBM.elf”必须位于“lib/firmware”中。

接下来的几行启动带有一些参数的 SLAM 应用程序。此文件包含每种应用程序类型的示例。取消注释其中之一并适当修改它。

最后一行将关闭操作系统。如果操作系统未正确关闭，则可能不会生成输出文件。

根据应用类型，可能还需要此目录中的校准文件和测试数据。

实用程序

git 上包含一些实用程序。

它们是为 Windows 上的 Visual C++ Express 2015 编写的。除“slam”项目外，源文件位于各自的目录中。“slam”项目的源文件与我们已经构建的 Petalinux 上的“slam”项目相同。创建 Visual C++ 项目并将源文件添加到项目中。

这里列出了成功构建所需的其他设置。它们适用于“Release/x64”构建。

所有这些程序都是基于OpenCV 3.x 。在本文中，假设 OpenCV 3.2.0 安装在以下目录结构中。

opencv-3.2.0
  └─build
    ├─include
    │ └─opencv2
    └─x64
      └─vc14
        ├─bin
        │ ├─opencv_world320.dll
        │ └─opencv_world320d.dll
        └─lib
          ├─opencv_world320.lib
          └─opencv_world320d.lib

■ 捕获视频

该程序从 USB 视频类设备捕获图像。

当按下“Enter”键时，接收到的图像将在写入文件之前水平分成两半。如果与在“Frame Grabber”模式下运行的 U96-SLAM 一起使用，该程序将适当地左右分割图像。按“ESC”退出程序。

“main.cpp”中的“DEVICE_ID”决定了打开哪个设备。这些索引由系统以增量顺序自动分配。可能需要根据已连接到的 PC 的 UVC 设备的数量更改该值。

[Configuration Properties]
  C/C++ → General → Additional Include Directory: 
    [OPENCV_DIR]\opencv-3.2.0\build\include
  Linker → General → Additional Library Directories: 
    [OPENCV_DIR]\opencv-3.2.0\build\x64\vc14\lib
           Input → Additional Dependencies: opencv_world320.lib
[Argument parameters]
  None

■ stereo_calib

该程序读取棋盘图案的立体图像对，使用 OpenCV 函数计算立体校准参数，然后将它们存储到文件中。

[Configuration Properties]
  C/C++ → General → Additional Include Directory: 
    [OPENCV_DIR]\opencv-3.2.0\build\include
  Linker → General → Additional Library Directories: 
   [OPENCV_DIR]\opencv-3.2.0\build\x64\vc14\lib
           Input → Additional Dependencies: opencv_world320.lib
[Argument parameters]
  -w, -h : The number of the 'inner' intersections of the chessboard pattern.
  -s : The size of the grid in meters. The unit of this parameter is important as it determines all the subsequent units including the pose graph output of SLAM application.
[Example]
  -w=7 -h=5 -s=0.03 [FILE_PATH]/dataset.xml

■ SLAM

这是 SLAM 应用程序的 Windows 版本。源文件与 Petalinux 上的 SLAM 应用程序相同。将“src”目录下的所有文件添加到项目中。在 Windows 上只有没有 FPGA 加速的批处理模式可用。

[Configuration Properties]
  C/C++ → General → Additional Include Directory: 
    [OPENCV_DIR]\opencv-3.2.0\build\include
    [WORK_DIR]\U96-SLAM\vc\slam\include
          Advanced → Disable Specific Warnings: 4996;4819
  Linker → General → Additional Library Directories: 
    [OPENCV_DIR]\opencv-3.2.0\build\x64\vc14\lib
           Input → Additional Dependencies: opencv_world320.lib
[Argument parameters]
-app    : Application type, only "SLAM_BATCH" is available.
-dir    : Base directory path. All the below paths are relative to this directory.
-l/-r   : Image file paths.
-lc/-rc : Calibration file paths.
-t      : Path to timestamp file.
-gt     : Path to ground truth file.
-n      : Number of files to be preocessed, negative value means all files.
[Example]
-app "SLAM_BATCH" -dir "KITTI/odometry/dataset/sequences/00" -l "image_0" -r "image_1" -t "times.txt" -gt "../../poses/00.txt" -lc "calib.txt" -n -1

参考

❝

https://www.hackster.io/sdoira/u96-svm-stereo-vision-front-end-for-ultra96-v2-87fcda

❞

❝

https://zhuanlan.zhihu.com/p/501102444

❞

❝

https://www.hackster.io/sdoira/

❞

❝

https://github.com/sdoira/U96-SVM

❞

未来的计划

• 替换为面向计算机视觉的图像传感器

• 自动校准

• 保存/加载多地图会话的功能

• 与其他传感器集成，例如 IMU 和 GNSS

• 迁移到更小的设备中