SLAM在ARM上的加速（3）- Neon在SLAM中的应用

news2025/1/12 20:51:04

SLAM在ARM上的加速：

SLAM在ARM上的加速（1）- ARM加速基础

SLAM在ARM上的加速（2）- Neon

SLAM在ARM上的加速（3）- Neon在SLAM中的应用

视觉SLAM通常可以分为前端和后端两个部分：
(1)、前端主要负责在实时场景中提取特征点、跟踪相机运动和估计相机姿态。它的任务是通过分析输入的图像序列或点云数据，提取有意义的特征并将它们与地图进行对齐，从而得到相机的运动轨迹和局部地图。前端算法通常包括特征提取与描述子生成、特征匹配和运动估计等步骤。
(2)、后端负责优化前端产生的轨迹和地图，消除估计误差并提高精度。它的任务是使用优化技术（例如图优化和非线性优化）对前端产生的相机轨迹和地图进行优化，从而最小化误差并提高整个系统的一致性。后端通常使用梯度下降等方法对大规模非线性最小化问题进行优化，以获得更准确的相机轨迹和地图。

前端任务需要在实时环境下执行，因为SLAM系统通常需要即时地定位相机并生成地图，因此对前端的加速很有必要，可以从以下几个方面使用NEON完成加速：
(1)、特征提取：从输入的相机图像中提取有意义的特征点，这些特征点通常是图像中明显的角点或纹理丰富的区域。
(2)、特征跟踪：在连续的图像帧之间追踪特征点，这通常涉及到使用特征描述子将初始特征点与后续帧中的对应点匹配起来。
(3)、相机姿态估计：根据特征点的运动信息来估计相机的姿态变化，这包括相机的位姿（位置和方向）和运动速度。
(4)、深度估计：通过特征点的视差或其他视觉几何方法，估算相机观察到的场景中物体的深度信息。
(4)、其他：一些图像的处理操作。

1 特征提取

SLAM中最常见的特征点提取有以下两种：
（1）orb-slam采用的fast角点
（2）vins-mono采用的Shi-Tomasi角点

1.1 fast角点加速

1.2 Shi-Tomasi角点加速

cv::goodFeaturesToTrack（也称为shi-Tomasi角点检测）是一种角点检测算法，用于在图像中检测重要的地方或角点。这个算法最初由J. Shi和C. Tomasi在1994年的论文中提出。

goodFeaturesToTrack算法旨在找到最显著的角点，它假定角点周围的像素在各个方向上都会产生较大的像素变化。它通过计算每个像素周围窗口的协方差矩阵的特征值来度量每个像素的显著性。一个像素被认为是角点当且仅当它的最小特征值大于某个阈值，并且它的最小特征值比其周围像素的最小特征值大。

具体来说，goodFeaturesToTrack算法可以分为以下几个步骤：

计算图像梯度：用Sobel算子计算图像在x和y方向上的梯度。这个步骤可以帮助我们找到图像中每个像素周围的像素变化情况。
计算协方差矩阵：对于每个像素，计算在其周围的固定大小的窗口中像素值的协方差矩阵。这个步骤可以帮助我们度量像素周围像素变化的方向和大小。
计算特征值：计算协方差矩阵的特征值，并根据特征值计算特征值比，从而得到每个像素的显著性。特征值比等于最小特征值除以最大特征值。
检测角点：遍历所有像素，如果一个像素的特征值比大于某个阈值，并且它的特征值比大于周围像素的特征值比，那么它就被认为是一个角点。
过滤角点：对于所有检测到的角点，通过对它们进行排序和去重等操作，最终得到最显著的角点。

总的来说，goodFeaturesToTrack算法是一种基于协方差矩阵的特征检测算法，它通过计算像素周围像素变化的方向和大小，来找到最显著的角点。这个算法在计算量方面较小，并且对各种图像类型都有很好的鲁棒性。

2 特征跟踪

3 相机姿态估计

4 深度估计

5 其他

5.1 RGB加载de-interleave与存储interleave

ps：交织（interleave）和解交织（de-interleave）是与图像数据处理相关的两个概念。
交织（RGB存储）是指将RGB通道的数据按照特定的顺序组合在一起，形成一个连续的像素数组。每个像素数组包含红（R）、绿（G）、蓝（B）三个通道的数据。这种存储方式可以用于保存图像数据或传输图像数据。交织存储的目的是将各个通道的数据正确组合起来，以还原出原始图像的颜色信息。
解交织（RGB加载）是指将存储在不同通道中的像素数据重新合并为一个连续的图像帧。通过按照特定的顺序从各个通道提取像素数据，并将它们按顺序置于一个像素数组中，可以还原图像的完整信息。解交织常用于从RGB图像数据中提取各个通道的数据，以便对每个通道进行独立处理。

使用 vld3q 以解交织的方式加载 RGB 图像，输入地址为 in_ptr, 输出向量为 vec

uint8x16x3_t vec = vld3q(in_ptr);

vst3q 以交织的方式存储 RGB 图像，输出地址为out_ptr, 输入为 uint8x16x3_t 类型的 RGB 向量
vst3q(out_ptr, vec);

5.2 查表操作

快速找到特征点的值TBL/TBX。

a表示table，向量类型的表，最多可以有 4 个寄存器向量值。
b表示index，向量类型的下标，通过下标向量到表中查找对应的元素。
c表示结果
uint8x8_t c = vtbl2_u8(a, b)

5.3 边缘处理

处理图像边缘时，经常会有使用常数填充边界的情况。

NEON 开发中，可以使用DUP指令将常数填充到向量中，然后使用EXT指令组建新向量。例如 7x7 的 boxfilter，处理边界时需要填充 3 个像素的值。EXT指令还常常用于滤波向量的重组操作。

// 构造边界填充向量
uint8_t c_0 =0;
uint8x8_t v_c0 = v_dup_n_u8(c_0);

// 构建v_1
uint8x8_t v_1 = vext_u8(v_c0, v_0, 5)

// 使用 vext 构建边界向量，v0 表示从纵坐标为 0 起始的向量
uint8x8_t v_border = vext_u8(v_1, v_c0, 3)

5.4 SAD

SAD(sum of absolute difference) 运算可以使用 NEON 指令来加速。

首先使用vabd做差的绝对值计算，然后使用vdot将上面的结果做累加。

// 初始化 v_sum 和 v_c1  
uint32x4_t v_sum = vmovq_n_u32(0);
uint8x16_t v_c1  = vmovq_n_u8(1);

// v_src0, v_src1为两幅图的输入
// 将做差的绝对值计算
uint8x16_t v_abd_res = vabdq_u8(v_src0, v_src1);

// 做 vdot操作
v_sum = vdotq_u32(v_sum , v_abd_res, v_c1);
...
// 将最后的结果累加
uint32_t res = vaddvq_u32(v_sum);