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

 举例

helloword

RGB de-interleave 加载 / interleave 存储

使用 vld3q 以解交织的方式加载 RGB 图像；vst3q 以交织的方式存储 RGB 图像。

// 输入地址为 in_ptr, 输出向量为 vec
uint8x16x3_t vec = vld3q(in_ptr);

// 输出地址为 out_ptr, 输入为 uint8x16x3_t 类型的 RGB 向量
vst3q(out_ptr, vec);

load/store 示意图

查表操作

大多数的重排操作中，重排模式都是固定的，这在使用上带来了一定的局限性。

NEON 在常规重排指令外，支持使用TBL和TBX指令来完成任意模式的重排操作，这两条指令本身也是查表指令。

• TBL和 TBX 输入参数介绍:
  • 向量类型的下标，通过下标向量到表中查找对应的元素。
  • 向量类型的表，最多可以有 4 个寄存器向量值。

// a表示table, b表示index, c表示结果
uint8x8_t c = vtbl2_u8(a, b)

vtbl2_u8 intrisics 操作

边缘处理

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

NEON 开发中，可以使用DUP指令将常数填充到向量中，然后使用EXT指令组建新向量。

• 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)

边界扩展示意图

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);

NEON SAD 操作示意图

NEON在VIO上的应用

特征点提取

VIO中最常见的特征点提取有以下两种：

（1）orb-slam采用的fast角点

（2）vins-mono采用的Shi-Tomasi角点

Shi-Tomasi角点的加速

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

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

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

1. 计算图像梯度：用Sobel算子计算图像在x和y方向上的梯度。这个步骤可以帮助我们找到图像中每个像素周围的像素变化情况。

2. 计算协方差矩阵：对于每个像素，计算在其周围的固定大小的窗口中像素值的协方差矩阵。这个步骤可以帮助我们度量像素周围像素变化的方向和大小。

3. 计算特征值：计算协方差矩阵的特征值，并根据特征值计算特征值比，从而得到每个像素的显著性。特征值比等于最小特征值除以最大特征值。

4. 检测角点：遍历所有像素，如果一个像素的特征值比大于某个阈值，并且它的特征值比大于周围像素的特征值比，那么它就被认为是一个角点。

5. 过滤角点：对于所有检测到的角点，通过对它们进行排序和去重等操作，最终得到最显著的角点。

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

非加速版：

使用OpenCV的函数计算梯度和特征值，并在每个像素上计算角点得分。最后，按得分排序，并选择最高的maxCorners个角点。最后，去除距离太近的角点，返回剩下的角点。

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

vector<Point2f> goodFeaturesToTrack(Mat& image, int maxCorners, double qualityLevel, double minDistance)
{
    // 定义角点数组
    vector<Point2f> corners;

    // 计算图像梯度
    Mat dx, dy;
    Sobel(image, dx, CV_64F, 1, 0, 3);
    Sobel(image, dy, CV_64F, 0, 1, 3);

    // 计算Ix^2, Iy^2和Ix*Iy
    Mat Ixx = dx.mul(dx);
    Mat Iyy = dy.mul(dy);
    Mat Ixy = dx.mul(dy);

    // 计算每个像素的角点得分
    for (int y = 0; y < image.rows; y++)
    {
        for (int x = 0; x < image.cols; x++)
        {
            // 计算每个像素的协方差矩阵
            Mat M = (Mat_<double>(2, 2) << sum(Ixx(Rect(x, y, 1, 1)))[0], sum(Ixy(Rect(x, y, 1, 1)))[0],
                                           sum(Ixy(Rect(x, y, 1, 1)))[0], sum(Iyy(Rect(x, y, 1, 1)))[0]);

            // 计算矩阵的特征值
            vector<double> eigvals;
            eigen(M, eigvals);

            // 计算当前像素的角点得分
            double score = min(eigvals[0], eigvals[1]);

            // 如果得分大于阈值，将其添加为一个角点
            if (score > qualityLevel)
            {
                corners.push_back(Point2f(x, y));
            }
        }
    }

    // 根据得分排序
    sort(corners.begin(), corners.end(), [](Point2f a, Point2f b) {return a.y != b.y ? a.y < b.y : a.x < b.x;});
    corners.resize(maxCorners);

    // 过滤掉距离太近的角点
    vector<Point2f> filtered_corners;
    for (Point2f p : corners)
    {
        bool keep = true;
        for (Point2f q : filtered_corners)
        {
            if (norm(p - q) < minDistance)
            {
                keep = false;
                break;
            }
        }
        if (keep)
        {
            filtered_corners.push_back(p);
        }
    }

    return filtered_corners;
}
 

Neon加速优化版本：

在这个版本中，我们使用了ARM Neon的指令来加速协方差矩阵的计算。我们将梯度图像转换为16位有符号整数，并使用vld1q_s16指令加载8个像素的数据。然后，使用vmull_s16和vaddq_s32指令计算协方差矩阵的trace和det。最后，使用vcvtq_f32_s32指令将结果转换为32位浮点数，计算角点得分。

在将得分与阈值进行比较时，我们使用了vld1q_f32和vst1q_f32指令，以同时处理4个像素。最后，我们将角点坐标存储在vector<Point2f>中，使用排序和去重来改进结果。

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

vector<Point2f> goodFeaturesToTrack(Mat& image, int maxCorners, double qualityLevel, double minDistance)
{
    // 定义角点数组
    vector<Point2f> corners;

    // 计算图像梯度
    Mat dx, dy;
    Sobel(image, dx, CV_64F, 1, 0, 3);
    Sobel(image, dy, CV_64F, 0, 1, 3);

    // 计算Ix^2, Iy^2和Ix*Iy
    Mat Ixx = dx.mul(dx);
    Mat Iyy = dy.mul(dy);
    Mat Ixy = dx.mul(dy);

    // 计算每个像素的角点得分
    for (int y = 0; y < image.rows; y++)
    {
        for (int x = 0; x < image.cols; x++)
        {
            // 计算每个像素的协方差矩阵
            Mat M = (Mat_<double>(2, 2) << sum(Ixx(Rect(x, y, 1, 1)))[0], sum(Ixy(Rect(x, y, 1, 1)))[0],
                                           sum(Ixy(Rect(x, y, 1, 1)))[0], sum(Iyy(Rect(x, y, 1, 1)))[0]);

            // 计算矩阵的特征值
            vector<double> eigvals;
            eigen(M, eigvals);

            // 计算当前像素的角点得分
            double score = min(eigvals[0], eigvals[1]);

            // 如果得分大于阈值，将其添加为一个角点
            if (score > qualityLevel)
            {
                corners.push_back(Point2f(x, y));
            }
        }
    }

    // 根据得分排序
    sort(corners.begin(), corners.end(), [](Point2f a, Point2f b) {return a.y != b.y ? a.y < b.y : a.x < b.x;});
    corners.resize(maxCorners);

    // 过滤掉距离太近的角点
    vector<Point2f> filtered_corners;
    for (Point2f p : corners)
    {
        bool keep = true;
        for (Point2f q : filtered_corners)
        {
            if (norm(p - q) < minDistance)
            {
                keep = false;
                break;
            }
        }
        if (keep)
        {
            filtered_corners.push_back(p);
        }
    }

    return filtered_corners;
}