Vins-Fusion之 TrackImage-Lukas-Kanade光流法（四）

输入单目图像或者双目图像，分别调用同一个函数为：

cpp 复制代码

if(_img1.empty())
        featureFrame = featureTracker.trackImage(t, _img);//单目图像跟踪
    else
        featureFrame = featureTracker.trackImage(t, _img, _img1);//双目图像跟踪

函数声明如下：

cpp 复制代码

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat());

参数	类型	说明
t	double	图像时间戳（秒）
_img	const cv::Mat &	左图像（或单目图像）
_img1	const cv::Mat &	右图像（双目模式，可选，默认为空）

时序光流跟踪（Lucas-Kanade）这里就不复制别人的内容了，有很多大神写的很详细，参考：

经典光流算法Lucas-Kanade（有图助理解）-CSDN博客，先熟悉LK算法的原理后再学习Vins-Fusion中的LK算法。

cpp 复制代码

正向光流算法代码：
TicToc t_r;
    cur_time = _cur_time;
    cur_img = _img;//左视图
    row = cur_img.rows;
    col = cur_img.cols;
    cv::Mat rightImg = _img1;//右视图
    cur_pts.clear();
    //CPU模式下，如果上一帧有特征点，则进行特征跟踪，执行光流跟踪算法，计算当前帧与上一帧之间的特征点匹配关系，得到特征点在当前帧中的位置
    if (prev_pts.size() > 0)
    {
        vector<uchar> status;
        if(!USE_GPU_ACC_FLOW)//USE_GPU_ACC_FLOW 为 0 时走 CPU 路径，为 1 时走 GPU 路径
        {
            TicToc t_o;
            vector<float> err;
            //有 IMU 预测或运动模型预测时
            if(hasPrediction)//预测功能在初始化完成后、有深度估计时开启
            {
                cur_pts = predict_pts;//使用预测的特征点位置进行光流跟踪初始值
                // 使用1层金字塔，快速跟踪，输出cur_pts：当前帧跟踪的位置，status, err：每个点的跟踪成功标志和误差
                cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1, 
                cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
                
                int succ_num = 0;
                for (size_t i = 0; i < status.size(); i++)
                {
                    if (status[i])
                        succ_num++;
                }
                //如果跟踪成功的特征点数量小于10，则使用3层金字塔，慢速跟踪
                if (succ_num < 10)
                cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
            }
            else
                cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);

函数原型：

cpp 复制代码

void cv::calcOpticalFlowPyrLK(
    InputArray prevImg,        // 上一帧图像 (prev_img)
    InputArray nextImg,         // 当前帧图像 (cur_img)
    InputArray prevPts,        // 上一帧特征点位置 (prev_pts)
    InputOutputArray nextPts,  // 输入输出：当前帧特征点位置 (cur_pts)
    OutputArray status,        // 输出：跟踪状态
    OutputArray err,           // 输出：跟踪误差
    Size winSize,              // 搜索窗口大小 (21, 21)
    int maxLevel,              // 金字塔层数 (3)
    ...
);

这里思考一个问题：没有IMU预测的时候直接使用：cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);既然该函数输出：cur_pts、status、err，可以直接根据上一帧，输出当前帧的特征点，存入cur_pts中，为什么还要使用IMU预测的特征点作为行光流跟踪初始值？？

两种情况的性能对比

特性	无预测（149-150行）	有预测（132-148行）
初始搜索位置	prev_pts（上一帧位置）	predict_pts（预测位置，更准确）
搜索范围	较大（需要大范围搜索）	较小（预测位置接近真实位置）
金字塔层数	3 层（从粗到细）	1 层（快速）
计算速度	较慢	较快
跟踪成功率	正常	更高（初始位置更准确）
失败后处理	无	如果 succ_num < 10，再用 3 层金字塔重试

情况 1：有预测

cpp 复制代码

cur_pts = predict_pts;//使用预测的特征点位置进行光流跟踪初始值
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1, cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);

a.使用 OPTFLOW_USE_INITIAL_FLOW
b.cur_pts 作为输入：初始搜索位置 = predict_pts（预测位置）
c.使用 1 层金字塔（更快）

情况 2：无预测

cpp 复制代码

else
    cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);

a.不使用 OPTFLOW_USE_INITIAL_FLOW
b.cur_pts 初始为空，函数从 prev_pts 位置开始搜索
c.使用 3 层金字塔（更准确但更慢）

为什么需要预测位置作为初始值？

无预测情况：

prev_pts[i] = (x_prev, y_prev) ← 上一帧位置

↓

在 cur_img 中，以 (x_prev, y_prev) 为中心搜索

↓

搜索范围较大，需要多层金字塔

有预测情况：

predict_pts[i] = (x_predict, y_predict) ← 预测位置（更接近真实位置）

↓

cur_pts[i] = predict_pts[i] ← 设置为初始搜索位置

↓

在 cur_img 中，以 (x_predict, y_predict) 为中心搜索

↓

搜索范围较小，只需1层金字塔即可

反向光流检查:

cpp 复制代码

 /*验证匹配一致性：正向（上一帧→当前）和反向（当前→上一帧）都成功且位置误差小，才认为跟踪可靠。
            剔除误匹配/漂移：若正反不一致或误差大，标记为失败，减少错误特征进入后端。
            提高鲁棒性：在快速运动、遮挡、光照变化时过滤不稳定特征，降低后端优化的错误观测。*/
            if(FLOW_BACK)
            {
                vector<uchar> reverse_status;
                vector<cv::Point2f> reverse_pts = prev_pts;
                // 从当前帧跟踪回上一帧
                cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1, 
                cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
                // 检查反向跟踪误差
                for(size_t i = 0; i < status.size(); i++)
                {
                    // 如果正向和反向跟踪都成功，且误差<0.5像素
                    if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
                    {
                        status[i] = 1;// 跟踪成功
                    }
                    else
                        status[i] = 0;// 跟踪失败
                }
            }

最终结果：正向特征点匹配、反向特征点匹配及特征点像素误差小于0.5并且在图像范围内才认为特征点状态为1，成功的特征点。