12-[LVI-SAM]visual_feature_光流追踪算法

最新推荐文章于 2023-06-09 16:46:08 发布

loyer_kong

最新推荐文章于 2023-06-09 16:46:08 发布

阅读量2.1k

点赞数

分类专栏： LVI-SAM分析文章标签：算法图像处理计算机视觉

本文链接：https://blog.csdn.net/loyer_kong/article/details/121733143

版权

LVI-SAM分析专栏收录该内容

15 篇文章 23 订阅

订阅专栏

2021SC@SDUSC

visual_feature_光流追踪算法

上一个blog，我们简要分析了img_callback这个回调函数，但是没有分析里面使用的函数。特别是我们之前已经提到过的光流追踪算法。

而本次blog主要就是分析上一个blog中提到的readImage函数。

文章目录

visual_feature_光流追踪算法

0. 图像处理与准备

a. 图像处理

如果图像太亮或者太暗，需要进行直方图均衡化处理。其中，EQUALIZE表示图像太亮或者太暗。

直方图均衡化(Histogram Equalization)是一种增强图像对比度(Image Contrast)的方法，其主要思想是将一副图像的直方图分布变成近似均匀分布，从而增强图像的对比度。直方图均衡化虽然只是数字图像处理(Digital Image Processing)里面的基本方法，但是其作用很强大，是一种很经典的算法。

在LVI-SAM中，我们处理的算法是限制对比度自适应直方图均衡化(CLAHE)。效果如下：

12-1

处理之后：

12-2

处理之后，可以看到图像明暗对比更明显。亮的地方更亮，暗的地方更暗，拉开了差距。因此，也增大了图像的信息量，方便后续处理。

// 需要用到的一些变量的初始化
cv::Mat img;
TicToc t_r;
cur_time = _cur_time;

// 如果为1，则表示太亮或者太暗，需要进行直方图均衡化处理
if (EQUALIZE)
{
    // 使用自适应直方图均衡处理
    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;

b. 数据准备

在正式开始算法之前，需要将一些变量清楚，并且判断是否是第一帧。

// 如果当前帧的图像数据forw_img为空，说明是第一次读入图像数据帧
if (forw_img.empty())
{
    // 将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img
    prev_img = cur_img = forw_img = img;
}
else
{
    // 不是第一次，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
    forw_img = img;
}

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

1. LK金字塔光流跟踪

这一部分的代码，调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts。status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0。并剔除失败的特征点。最后再更新追踪次数。

LK金字塔光流跟踪介绍

if (cur_pts.size() > 0)
{
    TicToc t_o;
    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
    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);
    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++;

2. 发布特征点

如果需要发布特征点，则需要将特征点提取出来。

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

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

可以看到这里用了几个自己定义的函数。

a. rejectWithF()

通过基本矩阵剔除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;
            // 根据不同的相机模型将二维坐标转换到三维坐标
            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();
        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());
    }
}

b. setMask()

对跟踪点进行排序并去除密集点。对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用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);
        }
    }
}

c. addPoints()

添加新的追踪点。

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
    }
}

3. 结束

//当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;

//把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts
cur_img = forw_img;
cur_pts = forw_pts;

//根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度
undistortedPoints();
prev_time = cur_time;

a. undistortedPoints()

对角点图像坐标去畸变矫正，并计算每个角点的速度

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

附录：代码

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;+
    cur_time = _cur_time;

    if (EQUALIZE)
    {
        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;

    if (forw_img.empty())
    {
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        forw_img = img;
    }

    forw_pts.clear();

    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    for (auto &n : track_cnt)
        n++;

    if (PUB_THIS_FRAME)
    {
        rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();
        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)
                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;
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;
    undistortedPoints();
    prev_time = cur_time;
}

附录：引用

ManiiXu/VINS-Mono-Learning: VINS-Mono

直方图均衡化 - 知乎

loyer_kong

关注

0
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
12-[LVI-SAM]visual_feature_光流追踪算法

上一个blog，我们简要分析了img_callback这个回调函数，但是没有分析里面使用的函数。特别是我们之前已经提到过的光流追踪算法。而本次blog主要就是分析上一个blog中提到的`readImage`函数。
复制链接

扫一扫