Ether Blog

复现FAST-LIVO2

2025-07-04

项目名称 : FAST-LIVO2复现

任务规划

论文阅读

FAST-LIVO

FAST-LIVO2

新贡献

  1. 问题 : 激光雷达和视觉测量之间维度不匹配 (测量维度 / 数据类型 / 分布特性) , 而 FAST-LIVO中使用异步更新。

    解决方案 : ESIKF框架,惯顺序更新(sequential update)。

  2. 问题 : FAST-LIVO中假设图像块中所有像素共享相同深度,该假设降低了对齐中仿射变换的准确性。

    解决方案 : 使用并进一步优化来自激光雷达点的平面先验。

  3. 问题: FAST-LIVO 基于与当前视图的接近度选择参考块,导致参考块低质量,降低了准确性。

    解决方案 : 提出了一种参考块 Reference Patch 更新策略 ,通过选择具有大视差和足够纹理细节的高质量内点参考块提高图像对齐的准确性。

  4. 问题 : 环境光照变化会降低图像对齐中的收敛性,FAST-LIVO中未解决该问题。

    解决方案 : 在线曝光时间估计。

  5. 问题 : 激光雷达近距离盲区造成激光雷达点测量缺失,FAST-LIVO中未考虑该问题。

    解决方案 : 提出按需进行体素射线投射的方法，以增强系统在因激光雷达近距离盲区造成的激光雷达点测量缺失情况下的鲁棒性。

    图2-FAST-LIVO2系统概述图

图2-FAST-LIVO2系统概述图

Section 4 顺序更新的ESIKF

  1. 扫描重组 Scan Recombination

    1. 将$t_k-1$时刻到$t_k$时刻的所有激光点都合并到$t_k$时刻。

      将高频、连续采样的LiDAR原始点云数据，按照相机采样时刻进行分割，从而得到与相机采样时刻对应的独立LiDAR扫描帧。

    2. 通过硬同步使雷达和相机采样一致

      激光雷达和相机在$t_k-1$ 时刻同时触发采集，但相机在瞬间完成采集，激光雷达则在$t_k$时刻才完成一帧数据。虽然触发时间相同，系统实际处理的是$t_k$时刻图像与延迟 $t$ (约100ms) 的激光雷达帧。

  2. 传播 Propagation

图3-前向传播与反向传播示意图

图3-前向传播与反向传播示意图

​ 1. 前向传播：KF在预测阶段将噪声$W_i$ 设为0，预测$t_k-1$ 到$t_k$ 这段时间每个IMU输入时的状态。由KF预测可得 此时状态量为$\hat{x}$，状态量对应的协方差$\hat{P}$作为更新阶段的先验。

​ 2. 反向传播 : 补偿激光雷达运动畸变，确保所有的点都是在tk时刻观测到的。

​ 运动畸变 : 由于激光雷达在移动过程中分时采点，若忽略每个点的具体采集时间,统一视为同一时刻采样，会导致 点云在空间上的扭曲或错位。

图4-反向传播示意图

图4-反向传播示意图

​ 3. 顺序更新 : 利用激光雷达测量细化的状态将被更新地图的几何结构，该状态将在视觉更新被进一步收敛，其中 视觉更新将在每个金字塔层面上进行，每层都要收敛。最终得到的状态被用于传播新到来的IMU测量，并更新地 图的视觉结构（纹理）。

Section 5 局部建图

  1. 地图结构 Map Structure

    图5-Voxel Map示意图

    ​ 图5-Voxel Map示意图

​ 自适应体素结构,按照哈希表和每个哈希条目的八叉树组织。

​ 哈希表管理根体素,每一个根体素为0.5x0.5x0.5m的固定维度,每个根体素包含一个八叉树结构。叶体素代表一个局部平面,存储一个平面特征(即平面中心、法向量和不确定性),以及一组位于该平面上的激光雷达原始点。叶体素的不同大小使其能够表示不同尺度的局部平面,从而适应具有不同结构的环境。部分点与三级图像块(8×8图块大小)相连(即视觉地图点),收敛的视觉地图点仅与参考块关联，未收敛的点与参考块和其他可见块都关联。

​ 为了防止地图大小无限增长,设置初始地图范围后, 在该范围进行进行探测和建图，当雷达探测范围触碰到范围边界，局部地图范围会在该边界方向上移动d距离。存储的A处点云数据将被重置为C处的数据,保持内存大小固定。每次ESIKF更新后检查一次地图是否移动。

图6-局部地图滑动示意图

图6-局部地图滑动示意图

在(a)中,灰色矩形是初 始地图区域,长度为 L。红色圆圈是以 p0 为中心的初始 检测区域。在(b)中,检测区域移动到新位置 p1,在此位 置触碰到地图边界。地图区域移动到新位置(蓝色矩形) ,移动距离为 d。在(c)中,内存空间B保持不变。存储绿色区域的内存空间A被重置为(b)中的蓝色区域C。

  1. 几何构造及更新 : 基于ESIKF对激光雷达点测量构建和更新实现

    在ESIKF中的激光雷达更新后,将激光雷达扫描的所有点注册到全局坐标系中。对于每个注册的激光雷达点,确定其在哈希图中的根体素位置。

    • 如果不存在,用新点初始化该体素并将其索引到哈希图中。

    • 如果确定的体素已经存在于地图中, 将该点附加到现有体素中。

    在所有扫描中的点分配完成后,进行几何构建和更新。

    1. 对于新创建的体素

      通过奇异值分解来确定其所有包含的点是否位于一个平面上。

      • 如果是,计算中心点 $ q = p ̄$ 、平面法线 $n$ 和平面的协方差矩阵$ Σ_{n,q} $。$ Σ_{n,q} $ 用于表征平面不确定性 (平面不确定性源于姿态估计的不确定性和点测量噪声)。
      • 如果不是,体素将不断细分为八个更小的八分体,直到子体素中的点被确定为形成一个平面或达到最大层数。达到最大层数后,叶体素中的点将被丢弃。
      • 地图仅包含被识别为平面的体素(根体素或子体素)。

    2. 对于已存在的体素

      评估新的点和已有的点能否构建为一个平面

      • 如果否：和前面一样，对体素进行划分。
      • 如果是：更新平面参数和协方差。
      • 一旦平面参数是收敛的，该平面将被视为成熟面，平面参数将被视为固定，平面上的新点将被丢弃。

    3. 平面上的激光雷达点生成视觉地图点

      • 对于成熟的平面,最近的50个雷达点是生成视觉地图点的候选者。

      • 对于未成熟的平面,所有雷达点都是候选者。

      • 视觉地图点生成过程将识别其中一些候选点作为视觉地图点,并将它们与图像块附加以进行图像对齐。

  2. 视觉地图点生成和更新

    选择地图中的候选激光雷达点用于生成和更新。(详见第Section 7-1)

    候选点条件:

    1. 在当前帧视角可见
    2. 在当前图像中表现出显著灰度梯度

    在视觉更新后,将候选点投影到当前图像上,并在每个体素中保留最小深度的候选点作为局部平面。将当前图像划分为每个30 × 30像素的均匀网格单元。

    • 若一个栅格元没有包含任何投影到这的视觉地图点,则使用带有最高灰度梯度的候选点新建一个新的视觉地图点。将该点与当前图像块、估计的当前状态（位姿/曝光时间）以及平面法向量相关联。与视觉地图点相关联的块有三个相同大小的层（每层11x11像素）,构成块金字塔。

    • 若一个栅格元包含了投影的视觉地图点，并满足以下条件之一就添加新的块到当前视觉地图点上:

      1. 超过20帧没有添加新的块到视觉地图点。
      2. 视觉地图点在当前帧中的像素位置比其在上一个块的位置相差至少40像素（晃得太厉害，点在屏幕里移动了一大段距离，重新拍一张）。

    地图点将拥有视角分布均匀的有效块Patch。

  3. Reference Patch 参考块更新 : 一个视觉地图点会拥有不止一个块。需要选择一个作为参考块用于视觉更新中的图像对齐，基于光度相似性和视角给每个块 f 评分。在所有附加到视觉地图点的Patch中,得分最高的Patch被更新为参考块。

上述评分机制倾向于选择:

	1. 外观与大多数其他补丁相似(以NCC为标准)的参考补丁, 以避免动态目标上的补丁;
	2. 视角方向与平面正交,从而在高分辨率下保持纹理细节。

FAST-LIVO 中的参考块更新策略直接选择与当前帧视角方向差异最小的补丁,导致所选参考补丁与当前帧非常接近,从而对当前姿态更新施加了弱约束。

  1. 法向量细化

    每个视觉地图点都被假设位于一个小的局部平面上。（以前的研究假设像素块上的所有像素拥有同样深度）

    使用源于雷达点的平面参数取得更好的精度(详见第Section 5-2)。

    平面法向量对于视觉更新过程中的图像仿射变换对齐至关重要,平面法向量细化能够增强仿射变换的精度。

    具体：通过最小化参考块与视觉地图点关联的其他块之间的光度误差，来优化参考块中的平面法向量。

Section 6 激光雷达测量模型

用于ESIKF LiDAR更新的LiDAR测量模型$y_l = h_l(x, v_l)$。

  1. 点到平面的雷达测量模型

    • 投影无畸变点到世界系;

    • 确定该世界点所在哈希图的根/子体素，并构建观测公式

      如果没有体素被发现或该体素不包含平面则丢弃该点。 否则，构建该体素中平面与这个点的观测公式。 假设一个激光真值点，准确的到世界系变换阵，这个点与平面中心点应该是在一个平面上的。

    • 带入激光点测量和噪声，平面法向量估计和中心点估计及协方差,

      得到测量噪声的形式:$v_l=(δ^L\mathbf{p}_j,δ\mathbf{n}_j,δ\mathbf{q}_j)$

  2. 带有光束发散效应的Lidar测量噪声

    测量噪声主要由两个元素构成，飞行时间导致的测距不确定性和编码导致的轴承方向不确定性，除此外，还有激光的发散角度。随着方位方向与法向量之间的角度 $φ$的增加，激光雷达点的测距不确定性显著增加，而方位方向的不确定性不受影响。当从地面和墙面选的点更多时，位姿估计的精度更高。 $φ$越小越好。

Section 7 视觉测量模型

用于ESIKF视觉更新的视觉测量模型$y_c = h_c(x, v_c)$。

1.视觉地图点选择

为了视觉更新中的稀疏对齐，首先选择合适的视觉地图点。

  1. 稀疏直接的视觉测量模型

上面提取的视觉地图点 {$G_{pi}$} 用于构建视觉测量模型。 其基本原理是,利用真值状态将地图点投影至当前图像，参考块与当前块之间的光度误差应当为0.

要根据测量方程估算反向曝光时间 $τ_k$，将初始反向曝光时间固定为 $τ_0$ = 1 ，以消除方程在所有反向曝光时间都为零时的退化现象。后续帧的估计反向曝光时间是相对于第一帧的曝光时间。测量方程被用于三个层次的视觉更新步骤,估计的状态随后用于生成可视化地图点和更新参考块。

本机环境 (和Fast-Livo2无关)

只是电脑又坏了。

  1. NVIDIA驱动 :

    • 关闭bios security boot

    • 在命令行模式下删除之前安装的nvidia驱动

      Ctrl + Alt + F3
      sudo systemctl stop gdm
      sudo apt-get purge '^nvidia-.*'
      sudo apt-get autoremove
      sudo rm -rf /etc/X11/xorg.conf
      sudo rm -rf /lib/modules/$(uname -r)/kernel/drivers/video/nvidia*
      sudo rm -rf /usr/src/nvidia*
      sudo rm -rf /usr/share/doc/NVIDIA_GLX-1.0*
      sudo rm -rf /usr/share/man/man1/nvidia*
      sudo rm -rf /usr/bin/nvidia*
      sudo rm -rf /etc/modprobe.d/blacklist-nvidia.conf

    • 重新安装,选择recommend的版本

      sudo ubuntu-drivers devices # 查看可以使用的驱动版本

    • 重启

      reboot

    • 检查

      nvidia-smi

NUC环境搭建

1. 修网卡

fast@fast:~/WIFI/backport-iwlwifi$ lspci | grep -i wireless
fast@fast:~/WIFI/backport-iwlwifi$ lsmod | grep iwl

系统没有识别出无线网卡设备,疑似硬件问题.最后使用网线上网.

2. 依赖项

3. 工作空间配置

Avia只支持livox_ros_driver

livox_ros_driver可以不着急先编译,等最后一起build

4. Build

cd ~/fast_livo2_ws/
catkin_make
source ~/fast_livo2_ws/devel/setup.bash

​

说明

+ livox_ros_driver.cpp
	 // 将文件映射为一个共享内存区域，大小为sizeof(time_stamp)，通过指针pointt可以直接读写它。
	  const char *user_name = getlogin();
      std::string path_for_time_stamp = "/home/" + std::string(user_name) + "/timeshare";
      const char *shared_file_name = path_for_time_stamp.c_str();
      int fd = open(shared_file_name, O_CREAT | O_RDWR | O_TRUNC, 0666);
      if (fd == -1) {
        ERR_EXIT("open");
      } else {
        printf("open code: %d\n", fd);
      }
      lseek(fd, sizeof(time_stamp) * 1, SEEK_SET);
      write(fd, "", 1);
      pointt = (time_stamp *)mmap(NULL, sizeof(time_stamp) * 1,
                                  PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
+lddc.cpp
    // 实现了用一个普通文件作为共享内存的载体，进程间可以通过映射该文件进行数据共享。
    // 结构体time_stamp用于在多个进程（或设备驱动与应用程序）间共享硬件时间戳。
    //******************************************************************** add code
    if (isOpended == false)
    {
      const char *user_name = getlogin();
      std::string path_for_time_stamp = "/home/" + std::string(user_name) + "/timeshare";

      const char *shared_file_name = path_for_time_stamp.c_str();
      int fd = open(shared_file_name, O_CREAT | O_RDWR | O_TRUNC, 0666);
      if (fd == -1)
      {
        ROS_ERROR("open failed\n");
        isOpended = false;
      }
      else
      {
        ROS_ERROR("open code: %d\n", fd);
        isOpended = true;
      }
      lseek(fd, sizeof(time_stamp) * 1, SEEK_SET);
      write(fd, "", 1);
      pointt = (time_stamp *)mmap(NULL, sizeof(time_stamp) * 1,
                                  PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
    }
    //********************************************************************

STM32硬同步

1. 材料

2. 原理图

图9-硬件同步方案

图9-硬件同步方案

Livox_avia不支持直接读取GPRMC格式的指令,mid360支持直接读取.如果使用mid360连线原理图将会改变.本篇只考虑Livox_avia.

图10-硬件同步原理图

图10-硬件同步原理图

图11-时间对齐原理图

图11-时间对齐原理图

3. 硬件连线图

**硬件连线参考图 : **

图12-硬件连线参考图

图12-硬件连线参考图

相机线说明 :

相机的电源及 I/O 接口为 6-pin P7 接口。

图13-相机同步线线序图

图13-相机同步线线序图

Livox Converter 2.0 同步线说明 :

图14-雷达同步线线序图

图14-雷达同步线线序图

4. 串口绑定

为了赋予特定串口权限,且防止串口在ubuntu下名称跳变，本片介绍ubuntu系统下如何绑定串口。

1. 建立udev规则

sudo gedit /etc/udev/rules.d/my_rule.rules

2. 获取串口设备详情

列出当前串口设备。

ls /dev/ttyUSB*

以ttyUSB0为例。

udevadm info --attribute-walk --name=/dev/ttyUSB0

获得详细信息，我习惯以第四行==looking …==以下的信息作为信息绑定的依靠。

looking at device '/devices/pci0000:00/0000:00:14.0/usb3/3-3/3-3:1.0/ttyUSB0/tty/ttyUSB0':
    KERNEL=="ttyUSB0"
    SUBSYSTEM=="tty"
    DRIVER==""

  looking at parent device '/devices/pci0000:00/0000:00:14.0/usb3/3-3/3-3:1.0/ttyUSB0':
    KERNELS=="ttyUSB0"
    SUBSYSTEMS=="usb-serial"
    DRIVERS=="cp210x"
    ATTRS{port_number}=="0"

  looking at parent device '/devices/pci0000:00/0000:00:14.0/usb3/3-3/3-3:1.0':
    KERNELS=="3-3:1.0"
    SUBSYSTEMS=="usb"
    DRIVERS=="cp210x"
    ATTRS{interface}=="CP2102 USB to UART Bridge Controller"
    ATTRS{authorized}=="1"
    ATTRS{bAlternateSetting}==" 0"
    ATTRS{bInterfaceProtocol}=="00"
    ATTRS{bNumEndpoints}=="02"
    ATTRS{supports_autosuspend}=="1"
    ATTRS{bInterfaceSubClass}=="00"
    ATTRS{bInterfaceClass}=="ff"
    ATTRS{bInterfaceNumber}=="00"

  looking at parent device '/devices/pci0000:00/0000:00:14.0/usb3/3-3':
    KERNELS=="3-3"
    SUBSYSTEMS=="usb"
    DRIVERS=="usb"
    ATTRS{idProduct}=="ea60"
    ATTRS{bConfigurationValue}=="1"
    ATTRS{tx_lanes}=="1"
    ATTRS{bNumInterfaces}==" 1"
    ATTRS{maxchild}=="0"
    ATTRS{avoid_reset_quirk}=="0"
    ATTRS{devpath}=="3"
    ATTRS{bDeviceSubClass}=="00"
    ATTRS{bDeviceClass}=="00"
    ATTRS{ltm_capable}=="no"
    ATTRS{quirks}=="0x0"
    ATTRS{serial}=="0001"
    ATTRS{bDeviceProtocol}=="00"
    ATTRS{speed}=="12"
    ATTRS{busnum}=="3"
    ATTRS{bNumConfigurations}=="1"
    ATTRS{manufacturer}=="Silicon Labs"
    ATTRS{bMaxPacketSize0}=="64"
    ATTRS{version}==" 1.10"
    ATTRS{bcdDevice}=="0100"
    ATTRS{devnum}=="6"
    ATTRS{rx_lanes}=="1"
    ATTRS{authorized}=="1"
    ATTRS{removable}=="removable"
    ATTRS{product}=="CP2102 USB to UART Bridge Controller"
    ATTRS{configuration}==""
    ATTRS{bMaxPower}=="100mA"
    ATTRS{urbnum}=="12"
    ATTRS{bmAttributes}=="80"
    ATTRS{idVendor}=="10c4"

  looking at parent device '/devices/pci0000:00/0000:00:14.0/usb3':
    KERNELS=="usb3"
    SUBSYSTEMS=="usb"
    DRIVERS=="usb"
    ATTRS{devpath}=="0"
    ATTRS{authorized_default}=="1"
    ATTRS{ltm_capable}=="no"
    ATTRS{speed}=="480"
    ATTRS{bNumInterfaces}==" 1"
    ATTRS{configuration}==""
    ATTRS{bMaxPower}=="0mA"
    ATTRS{bmAttributes}=="e0"
    ATTRS{avoid_reset_quirk}=="0"
    ATTRS{serial}=="0000:00:14.0"
    ATTRS{manufacturer}=="Linux 5.15.0-139-generic xhci-hcd"
    ATTRS{busnum}=="3"
    ATTRS{interface_authorized_default}=="1"
    ATTRS{bConfigurationValue}=="1"
    ATTRS{bDeviceClass}=="09"
    ATTRS{bMaxPacketSize0}=="64"
    ATTRS{quirks}=="0x0"
    ATTRS{bcdDevice}=="0515"
    ATTRS{idProduct}=="0002"
    ATTRS{tx_lanes}=="1"
    ATTRS{rx_lanes}=="1"
    ATTRS{product}=="xHCI Host Controller"
    ATTRS{urbnum}=="98"
    ATTRS{removable}=="unknown"
    ATTRS{bNumConfigurations}=="1"
    ATTRS{devnum}=="1"
    ATTRS{bDeviceProtocol}=="01"
    ATTRS{version}==" 2.00"
    ATTRS{idVendor}=="1d6b"
    ATTRS{maxchild}=="12"
    ATTRS{bDeviceSubClass}=="00"
    ATTRS{authorized}=="1"

  looking at parent device '/devices/pci0000:00/0000:00:14.0':
    KERNELS=="0000:00:14.0"
    SUBSYSTEMS=="pci"
    DRIVERS=="xhci_hcd"
    ATTRS{local_cpus}=="ffff"
    ATTRS{dma_mask_bits}=="64"
    ATTRS{power_state}=="D0"
    ATTRS{device}=="0x51ed"
    ATTRS{subsystem_device}=="0x3037"
    ATTRS{ari_enabled}=="0"
    ATTRS{class}=="0x0c0330"
    ATTRS{msi_bus}=="1"
    ATTRS{vendor}=="0x8086"
    ATTRS{revision}=="0x01"
    ATTRS{local_cpulist}=="0-15"
    ATTRS{label}=="Onboard - Other"
    ATTRS{numa_node}=="-1"
    ATTRS{driver_override}=="(null)"
    ATTRS{consistent_dma_mask_bits}=="64"
    ATTRS{subsystem_vendor}=="0x8086"
    ATTRS{dbc}=="disabled"
    ATTRS{enable}=="1"
    ATTRS{broken_parity_status}=="0"
    ATTRS{irq}=="145"
    ATTRS{d3cold_allowed}=="1"
    ATTRS{index}=="7"

  looking at parent device '/devices/pci0000:00':
    KERNELS=="pci0000:00"
    SUBSYSTEMS==""
    DRIVERS==""
    ATTRS{waiting_for_supplier}=="0"

3. 在udev规则下写入串口信息

如果没有相同ID设备的话可直接写入。

其中KERNEL=="ttyUSB*"表明绑定的设备是串口，ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60"是Step2中设备的信息，MODE:="0777"给予设备最高权限，SYMLINK+="ttyUSB_TTL2USB"为绑定后设备的名称。

KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="ttyUSB_TTL2USB"

若有相同ID设备，则可再添加ATTRS{devpath}=="5.2"信息，该量一般与设备与电脑链接的端口号有关。

4. 重启udev

sudo service udev reload
sudo service udev restart

再次查看串口设备，即可看到绑定成功后的名称。若还没成功，可尝试重启电脑或者热插拔设备。

图15-绑定串口显示

图15-绑定串口显示

5. 雷达设置

雷达选型 : Livox AVIA

转换器 : Livox Converter 2.0

Livox雷达时间同步说明

参考:https://github.com/Livox-SDK/Livox-SDK/wiki/livox-device-time-synchronization-manual-cn#%E9%99%84%E5%BD%95

Livox设备支持3种时间同步方式：

FAST-LIO只涉及GPS和PPS两种,只介绍这两种。

GPS

GPS时钟源的PPS端口每秒发送一次硬件脉冲（PPS信号），随后数据端口发送一次对应这个脉冲上升沿的时间信息（GPRMC格式）。Livox设备接收到PPS信号上升沿，并由GPRMC数据解析出正确的时间信息后，会设置点云时间为GPS时间，并保持此时间基准持续累加，来实现和GPS设备的时间同步。

图17-GPS信号示意图

图17-GPS信号示意图

Livox Converter 2.0将GPS模块的时间信号（GPRMC）通过TTL转usb模块接入PC，PPS信号接入LiDAR转接盒同步口（Sync Port）。

图18-Livox Converter 2.0连线图

图18-Livox Converter 2.0连线图

PPS

Livox LiDAR每次接收到PPS信号的上升沿后，会将当前时刻的点云时间置为0，然后重新开始计时直到下一个PPS脉冲到来,利用这个特性，来实现PPS脉冲对LiDAR时间的同步。

伪代码:

// PPS Time Synchronization
static uint64_t lidar_time_last;
static uint64_t lidar_time_real;

// 1. Read the PPS rising edge time, Unit is nanosecond.
uint64_t pps_time_ns = get_pps_rising_nsecond();
// 2. Read LiDAR point time, Unit is nanosecond.
uint64_t lidar_time = get_lidar_pack_time();
// 3. Update real time.
if (lidar_time < lidar_time_last)
{
    //LiDAR time jump indicates the generation of PPS rising edge.
    lidar_time_real = pps_time_ns + lidar_time%(1000000000);
}
else
{
    lidar_time_real += lidar_time - lidar_time_last;
}
//Update history
lidar_time_last = lidar_time;

设置

1. 安装Livox Viewer

Livox Viewer 0.10.0(64bit) : https://www.livoxtech.com/downloads

注：Livox Viewer2并不支持Avia

以太网设置:

图19-Livox雷达以太网设置

图19-Livox雷达以太网设置

点击左上角播放按钮,出现点云.

图20-Livox Viewer点云示意图

图20-Livox Viewer点云示意图

插上硬同步设备后,查看雷达的连接转态和数据的读取状态，来确认雷达是否已进入PPS同步状态 (在这里不能修改TIme Sync / PPS State,此设置为连接硬同步后自动触发)

图21-Livox Viewer设置状态图

图21-Livox Viewer设置状态图

2. 修改livox_lidar_config.json

图22-livox_lidar_config.json参数设置

图22-livox_lidar_config.json参数设置

3. 测试

roslaunch livox_ros_driver livox_lidar_rviz.launch

可以在自动弹出来的rviz中查看是否有点云正常输出和显示

6. 相机设置

相机 : 海康相机MV-CS020-10UC

驱动 : MVS_STD_V3.0.1

1. 安装驱动

海康威视工业相机SDK的ros驱动依赖MVS的库文件，先安装MVS客户端.

官方下载地址：https://www.hikrobotics.com/cn/machinevision/service/download/?module=0

2. 下载和编译相机的雷达ros驱动

(见NUC环境搭建 - 3.工作空间配置)

3. 调整配置文件

路径 : mvs_ros_driver/config

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
SerialNumber: "DA2099368" # Not needed for single camera. Specify serial number for multiple cameras. 
TopicName: "left_camera/image"

TriggerEnable: 1 # 0 stands for Off, 1 stands for On

ExposureAutoMode: 0 # 0 stands for Off, 1 stands for Once, 2 stands for Continues
ExposureTime: 5000 # us

# ExposureAutoMode: 2
# AutoExposureTimeLower: 100
# AutoExposureTimeUpper: 20000

image_scale: 0.5 # 1 0.5
GainAuto: 2 # Gain Auto, 0 stands for Off, 1 stands for Once, 2 stands for Continues
Gain: 15 # min: 0   max: 17.0166
Gamma: 0.7  # min: 0   max: 17.0166
GammaSelector: 1 # 0 stands for user, 1 stands for sRGB
# GammaEnable: 1

PixelFormat: 0 # 0: RGB8, 1: BayerRG8, 2: BayerRG12Packed, 3: BayerGB12Packed, 4: BayerGB8

最重要的参数是PixelFormat,需要根据相机型号调整。遍历一遍看哪个数值可用当然可以,代价是报错的时候不能理解其中原因,会造成很多麻烦。

如何找到适合本相机的PixelFormat :

图23-相机支持像素格式

图23-相机支持像素格式

​ 看似这个相机支持RGB8 / BayerRG8 / BayerRG12Packed, PixelFormat选择0/1/2都可以, 实则不然。

​ 当选择1或2的时候,运行roslaunch mvs_ros_driver mvs_camera_trigger.launch时, 出现报错 :

图24-mvs_camera_trigger报错图

图24-mvs_camera_trigger报错图

​ 针对这个报错, 打开MVS的官方图形化界面看一下 :

​ 图形化界面参数太多了,再查阅海康机器人USB3.0工业面阵相机用户手册,发现Gamma参数应该在Analog Control 属性中.

图25-海康相机用户手册详细参数

图25-海康相机用户手册详细参数

​ 在Bayer RG 8 / BayerRG12Packed模式下 , 没有Gamma这个参数 ．只有在RGB8模式下才有Gamma参数．

图26-海康相机图形化界面参数

图26-海康相机图形化界面参数

图27-海康相机图形化界面参数

图27-海康相机图形化界面参数

​ 探究一下原理：

4. 测试

7. 烧录

  1. STLINK驱动安装

    https://docs.qq.com/doc/DT2hyS2ZjY21WQkZt

  2. 硬件连接

    图29-烧录硬件连接原理图

    图29-烧录硬件连接原理图

    图30-实物图(stlink正面)

    图30-实物图(stlink正面)

图31-实物图(stlink侧面)

图31-实物图(stlink侧面)

图32-实物图(stm32背面)

图32-实物图(stm32背面)

  1. Keil烧录

    图33-烧录步骤-1

    图33-烧录步骤-1

图34-烧录步骤-2

图34-烧录步骤-2

图35-烧录步骤-3

图35-烧录步骤-3

图36-烧录步骤-4

图36-烧录步骤-4

图37-烧录步骤-5

图37-烧录步骤-5

图38-烧录步骤-6

图38-烧录步骤-6

图39-烧录步骤-7

图39-烧录步骤-7

测试

跑包

HKU_Lecture_Center_02.bag

roslaunch fast_livo mapping_avia.launch
rosbag play HKU_Lecture_Center_02.bag

建图精度

图40-rviz-1

图40-rviz-1

图41-rviz-2

图41-rviz-2

墙面与地面分界线清晰

图42-rviz-3

图42-rviz-3

图片处理(拉高亮度)后,可以看到墙面纹路,转角非常清晰

图43-rviz-4

图43-rviz-4

图44-rviz-5

图44-rviz-5

系统资源占用

运行launch和rviz : 资源主要由rviz占用,且初始rviz config中点云queue数值极大,rviz实时看全局着色点云很卡

  1. 可以把queue值 / decay值适当改小;
  2. 可以重建完成后保存pcd文件,通过pcl_viewer（最好+显卡 +opengl）或者cloud compare等软件离线查看;
  3. 使用带gpu的电脑提升实时rviz性能.
字段 含义
PRI Priority（优先级）
NI Nice 值（友好度）
VIRT Virtual Memory（虚拟内存大小）
RES Resident Memory（常驻内存） 当前进程实际占用的物理内存大小
SHR Shared Memory（共享内存）
S State（进程状态）R：运行（Running）S：休眠（Sleeping）
CPU% 进程当前使用的 CPU 百分比（所有核总和）
MEM% 进程使用的物理内存占系统总内存的百分比

图45-htop查看资源占用

图45-htop查看资源占用

图46-fast-livo2运行占用资源

图46-fast-livo2运行占用资源

图47-fast-livo2的rviz占用资源

图47-fast-livo2的rviz占用资源

只运行launch:

图48-只运行launch系统占用资源

图48-只运行launch系统占用资源

里程计数据

纵轴单位(m) ,三条曲线分别是x轴,y轴,z轴里程计数据变化

图48-里程计数据总览

图48-里程计数据总览

图49-里程计数据细节(图48蓝色框部分)

图49-里程计数据细节(图48蓝色框部分)

图50-里程计数据统计(图49中数据部分)

图50-里程计数据统计(图49中数据部分)

处理时间

参考该帧处理时间数量级

图51-处理时间参考

图51-处理时间参考

###可行性初测试

修改参数

图52-fast-livo2文件结构

图52-fast-livo2文件结构

cam_model: Pinhole
cam_width: 1624
cam_height: 1240
scale: 0.5
cam_fx: 1340.62925
cam_fy: 1340.84375
cam_cx: 821.09
cam_cy: 647.1655
cam_d0: -0.0482
cam_d1: 0.0739
cam_d2: 0.0
cam_d3: 0.0

除了要修改雷达的内参参数外,还要注意cam_width / cam_height / scale 三个参数 :

  • 在 mvs_ros_driver / config / left_camera_trigger.yaml中,scale默认为0.5,camara_pinhole.yaml中的值需要与其保持一致
  • mvs_ros_driver中默认不修改相机图像大小,cam_width / cam_height 为相机原尺寸
  • 图像传输时缩放了 0.5 倍，尺寸变为 812×620。

启动

启动雷达：

roslaunch livox_ros_driver livox_lidar_msg.launch

注意：这里我们启动的是Avia雷达的自定义消息格式，而非PointCloud2格式

启动相机:

roslaunch mvs_ros_pkg mvs_camera_trigger.launch

Fast Livo2主节点启动:

roslaunch fast_livo mapping_avia.launch

建图效果，如下方所示：

图54-实时rviz

图54-实时rviz

包数据分析

对齐效果 :

图55-包对齐总览

图55-包对齐总览

图56-包对齐细节

图56-包对齐细节

  • 雷达 / imu / 图像均为10hz
  • 图像和雷达时间差约为30ms,查看官方包也是这样的偏差,目前暂时不清楚这个时间差意味着什么

完全测试

硬件设备

图57-硬件设备设计图

图57-硬件设备设计图

图58-硬件设备实物图

图58-硬件设备实物图

图59-硬件设备实物图

图59-硬件设备实物图

设备标注:

图60-硬件设备标注

图60-硬件设备标注

设备接线:

图61-设备接线标注

图61-设备接线标注

设备供电:

待补图~

图62-设备供电展示

建图精度

表现 : 基本可以看清大色块纹理,也许需要更准确的标定数据?(存疑)

图63-复杂墙面建图总览

图63-复杂墙面建图总览

图64-复杂墙面建图细节-1

图64-复杂墙面建图细节-1

图65-复杂墙面建图细节-2

图65-复杂墙面建图细节-2

图66-大号文字墙面建图

图66-大号文字墙面建图

回环精度

表现 : 长距离回环 z轴有1.8m左右的漂移, x轴,y轴回环较为稳定

图67-回环路径展示

图67-回环路径展示

图68-回环里程计数据-z轴

图68-回环里程计数据-z轴

图68-回环里程计数据-x轴(蓝)/y轴(绿)

图68-回环里程计数据-x轴(蓝)/y轴(绿)

不同场景

  1. 12分钟+室内+室外

    表现 : 里程计数据基本平滑 , 室外相机有过曝情况 , 代码里没有看到和论文中对应的自动调整曝光的部分,也没有发布自动曝光话题 (存疑)

    图69-室外场景实时rviz

    图69-室外场景实时rviz

    图70-室外场景里程计数据总览

    图70-室外场景里程计数据总览

    图71-室外场景里程计数据细节

    图71-室外场景里程计数据细节

  2. 室内

    表现 : 玻璃会影响建图精度,从开发空间进入密闭空间(推门走进楼梯间)易造成错误建图

    图72-室内场景rviz-1

    图72-室内场景rviz-1

    图73-室内场景rviz-2

    图73-室内场景rviz-2

    图74-室内场景rviz-3

    图74-室内场景rviz-3

  3. 楼梯间

    表现 : 跑飞,窗户处建图错误

    推测 : 楼梯间狭窄且全为白墙,特征点少,avia雷达1-2m处点云质量差,存在盲区

图75-楼梯间rviz-1

图75-楼梯间rviz-1

图76-楼梯间rviz-2

图76-楼梯间rviz-2

← Back to Home