LVI-SAM 视觉 feature_tracker.cpp 代码解析

#include "feature_tracker.h"

int FeatureTracker::n_id = 0;

bool inBorder(const cv::Point2f &pt)
{
    const int BORDER_SIZE = 1;
    int img_x = cvRound(pt.x);
    int img_y = cvRound(pt.y);
    return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}

void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
    // 这个根据status的值进行记录，如果为0则不记录，为1则进行记录
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}

void reduceVector(vector<int> &v, vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}


FeatureTracker::FeatureTracker()
{
}

首先include库，然后对头文件里面的函数进行详细的定义。inborder()函数，如果点在里面，则返回true，如果在边界，则返回false。reduceVector()这个根据status的数值进行剔除。然后featureTracker()函数进行特征的跟踪，这里定义了个空的函数体。

// 对跟踪点进行排序并去除密集点。对跟踪到的特征点，按照被追踪到的次数排序并依次选点
// 使用mask进行类似非极大抑制，半径为30，去掉密集点，使特征点分布均匀
void FeatureTracker::setMask()
{
    // 对鱼眼镜头多做一个处理
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    
    // prefer to keep features that are tracked for long time
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id; // 构造（cnt，pts，id）序列

    for (unsigned int i = 0; i < forw_pts.size(); i++)
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));

    // 对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式）
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
         {
            return a.first > b.first;
         });

    // 清空cnt，pts，id并重新存入
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)
        {
            // 当前特征点位置对应的mask值为255，则保留当前特征点，将对应的特征点位置pts,id,被追踪次数cnt分别存入
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);
            // 在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

设置掩膜，这个对跟踪到的特征点，去掉密集点，使特征点分布均匀。就是防止某个地方的特征点太多了，密度太大。所以使用setMask来使特征点的密度更加均匀。

// 添加新的追踪点
void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1); // 新提取的特征点id初始化为-1
        track_cnt.push_back(1); // 新提取的特征点被跟踪的次数初始化为1
    }
}

添加点的函数，这个没啥好说的，for循环是个技巧，for里面的内容相当于一个放入再初始化的操作。

// 1、createCLAHE()判断并对图像进行自适应直方图均衡化
// 2、calcOpticalFlowPyrLK()从cur_pts 到 forw_pts做LK金字塔光流法
// 3、根据status，把跟踪失败的和位于图像边界外的点剔除，剔除时不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts、cur_pts，记录特征点id的ids，和记录特征点被跟踪次数的track_cnt中剔除
// 4、setMask()对跟踪点进行排序并依次选点，设置mask：去掉密集点，使特征点分布均匀
// 5、rejectWithF()通过基本矩阵F剔除outliers
// 6、goodFeaturesToTrack()寻找新的特征点(shi-tomasi角点)，添加(MAX_CNT -forw_pts.size())个点以确保每帧都有足够的特征点
// 7、addPoints()向forw_pts添加新的追踪点，id初始化-1，track_cnt初始化为1
// 8、undistortedPoints()对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    // 图像处理
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    // 如果为1，则表示太亮或者太暗，需要进行直方图均衡化处理
    // 判断是否进行直方图均衡化处理
    if (EQUALIZE)
    {
        // 自适应直方图均衡
        // createCLAHE(double clipLimit, Size tileGridSize)
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    // 数据准备
    // 如果当前帧的图像数据forw_img为空，说明是第一次读入图像数据帧
    if (forw_img.empty())
    {
        // 将img图像赋值给前面三个变量
        // 如果当前帧的图像数据forw_img为空，说明是第一次读入图像数据
        // 将读入的图像赋给当前帧forw_img,cur_img_prev_img，这是为了避免后面使用到这些数据时，它们是空的。
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        // 不是第一次，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
        forw_img = img;
    }

    // 此时forw_pts可能还保存的是上一帧图像中的特征点，所以把它清除掉
    forw_pts.clear();

    // LK金字塔光流跟踪
    // 调用cv::calaOpticalFlowPyrLK()对前一帧特征点cur_pts进行LK金字塔光流跟踪
    // 得到forw_pts。status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被
    // 跟踪到的点标记为0.并剔除失败的特征点，最后再更新追踪次数。
    if (cur_pts.size() > 0)
    {
        TicToc t_o; // 这个tictoc就是关于时间的，有种说法是tictoc就像是钟表按下、弹上来的声音。
        vector<uchar> status;
        vector<float> err;
        
        // 调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts
        // status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        // 将位于图像边界外的点标记为0
        // 根据if语句可以看出来，inborder函数，如果点在里面返回true，点在边界返回false
        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;
        
        // 根据status，把跟踪失败的点剔除
        // 不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts,prev_pts和cur_pts中剔除
        // prev_pts和cur_pts中的特征点是一一对应的
        // 记录特征点id的ids，和记录特征点被跟踪次数的track_cnt也要剔除
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status); // 猜测ids，应该是id的集合
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    // 光流追踪成功，特征点被成功跟踪的次数就加1，数值代表被追踪的次数，数值越大，说明被追踪的就越久
    for (auto &n : track_cnt)
        n++;

    // 发布特征点
    // 如果需要发布特征点，则需要将特征点提取出来
    // PUB_THIS_FRAME = 1，需要发布特征点
    if (PUB_THIS_FRAME)
    {
        // 通过基本矩阵剔除outliers 异常值
        rejectWithF();
        
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        
        setMask(); // 保证相邻的特征点之间要相隔30个像素，设置mask
        ROS_DEBUG("set mask costs %fms", t_m.toc()); // 那么这个tictoc就是关于时间的变量
        // ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;
        
        // 计算是否需要提取新的特征点
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1) // 8UC1是一个图像的格式
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        // 将新检测到的特征点n_pts添加到forw_pts中，id初始化-1，track_cnt初始化为1
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }
    // 当下一帧图像到来时，当前数据成为了上一帧发布的数据
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    // 把当前帧的数据forw 赋值给上一帧 cur
    cur_img = forw_img;
    cur_pts = forw_pts;
    // 根据不同的相机模型去畸变校正 和 转换到归一化坐标系上，计算速度
    undistortedPoints();
    prev_time = cur_time;
}

这个readImage()函数是非常重要的函数，在这里分为8个步骤进行。

readImage()函数步骤：

1、createCLAHE()判断并对图像进行自适应直方图均衡化

2、calcOpticalFlowPyrLK()从cur_pts到forw_pts做LK金字塔光流法

3、根据status，把跟踪失败的和位于图像边界外的点剔除，剔除时不仅要从当前帧数据剔除，而且还要从cur_un_pts、prev_pts、cur_pts，记录特征点id的ids，和记录特征点被跟踪次数的track_cnt中剔除

4、setMask()对跟踪点进行排序并依次选点，设置mask，去掉密集点，使特征点分布均匀

5、rejectWithF()通过基本矩阵F剔除outliers

6、goodFeatureToTrack()寻找新的特征点，添加(MAX_CNT -forw_pts.size())个点以确保每帧都有足够的特征点

7、addPoints()向forw_pts添加新的追踪点，id初始化-1，track_cnt初始化为1

8、undistortedPoints()对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度

这个函数先对EQUALIZE进行判断，使用直方图均衡化，这个步骤是防止图像过亮或者过暗。然后光流跟踪，这个也没啥好说的，就是具体内容还得去看cv库的源函数。这部分附近有个ids，这个应该是id的集合，这个我认为应该是的，懒得看代码了。就是个集合，看这个ids在其他地方的使用即可知道，有一步是提取ids的size的，所以应该是个集合。下面的话就是通过基本矩阵剔除outliers，然后设置掩膜。然后这个函数结束，下面的话就是这个函数里面使用子函数的具体定义。

// 自己定义的函数
// 通过基本矩阵剔除outliers
// 该函数只要是通过基本矩阵(F)去除外点outliers。首先将图像坐标畸变矫正后转换为像素坐标，通过cv::findFundamentalMat()计算F矩阵，利用得到的status通过reduceVector()去除outliers
void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            Eigen::Vector3d tmp_p;
            // 根据不同的相机模型将二维坐标转换到三维坐标
            // 对于PINHOLE(针孔相机)可将像素坐标转换到归一化平面并去畸变
            // 对于CATA(卡特鱼眼相机)将像素坐标投影到单位圆内
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            // 转换为归一化像素坐标
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        // 调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        // 下面这个就是根据status的值进行剔除
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

bool FeatureTracker::updateID(unsigned int i)
{
    if (i < ids.size())
    {
        if (ids[i] == -1)
            ids[i] = n_id++;
        return true;
    }
    else
        return false;
}

// 读取内参
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

void FeatureTracker::showUndistortion(const string &name)
{
    cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
    vector<Eigen::Vector2d> distortedp, undistortedp;
    for (int i = 0; i < COL; i++) // 这个相当于x
        for (int j = 0; j < ROW; j++) // 这个相当于y
        {
            Eigen::Vector2d a(i, j);
            Eigen::Vector3d b;
            m_camera->liftProjective(a, b); // 将二维转换为三维
            distortedp.push_back(a);
            undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
            //printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
        }
    for (int i = 0; i < int(undistortedp.size()); i++)
    {
        cv::Mat pp(3, 1, CV_32FC1);
        pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
        pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
        pp.at<float>(2, 0) = 1.0;
        //cout << trackerData[0].K << endl;
        //printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
        //printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
        if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
        {
            undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
        }
        else
        {
            ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
        }
    }
    cv::imshow(name, undistortedImg);
    cv::waitKey(0);
}

// 对角点图像坐标去畸变校正，并计算每个角点的速度
void FeatureTracker::undistortedPoints()
{
    // 先初始化
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
        // 根据不同的相机模型将二维坐标转换到三维坐标 没看懂
        m_camera->liftProjective(a, b);
        // 再延伸到深度归一化平面上
        cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }
    // caculate points velocity
    // 计算每个特征点的速度到pts_velocity
    if (!prev_un_pts_map.empty())
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)
            {
                std::map<int, cv::Point2f>::iterator it;
                it = prev_un_pts_map.find(ids[i]);
                if (it != prev_un_pts_map.end())
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;
                    double v_y = (cur_un_pts[i].y - it->second.y) / dt;
                    pts_velocity.push_back(cv::Point2f(v_x, v_y));
                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));
            }
            else
            {
                pts_velocity.push_back(cv::Point2f(0, 0));
            }
        }
    }
    else
    {
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    prev_un_pts_map = cur_un_pts_map;
}

第一个函数rejectWithF()通过本质矩阵，来剔除outliers。然后里面有个liftProjective()把二维坐标转换成三维坐标。然后是updateID()来更新ID值，这个没啥好说的，直接更新全局变量n_id来进行id的赋值。下面是读取内参，从一个yaml文件里面读取内参，感觉这个好麻烦。然后下面是显示去畸变的效果，和去畸变。然后本次代码阅读结束，这里面的话那个RANSIC还是不太会，没看懂，到时候再认真学一下。这篇结！