前端feature_tracker

最新推荐文章于 2025-01-22 09:00:00 发布

原创最新推荐文章于 2025-01-22 09:00:00 发布 · 449 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#深度学习

本文详细介绍了一种基于光流追踪的视觉追踪流程，包括图像预处理、特征点检测与跟踪、误匹配点剔除等关键步骤，并解释了如何利用F矩阵进行误匹配点的筛选。

1、图像预处理

首先对图像帧做自适应直方图均衡化，调用createCLAHE(double clipLimit, Size tileGridSize)函数。
通过forw_img.empty() 判断有无存储图像，若无，则说明该帧为第一帧，将其同时赋值给prev_img、cur_img、forw_img

2、第一帧图像

第一帧图像进来后示意图如下
在这里插入图片描述
首先n_pts中存储的是特征点坐标，forw_img是当前帧图像，此时调用addPoints()，将n_pts中存储的坐标赋值给forw_pts。同时更新ids（能够被跟踪到的特征点的id） 与track_cnt（当前帧forw_img中每个特征点被追踪的时间次数），再进行迭代更新

//当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据
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;

3、第二帧图像

第二帧图像forw_img进来后，由其与上一帧图像cur_img进行光流跟踪，调用calcOpticalFlowPyrLK()函数，此时cur_pts所存储的为之前forw_pts的角点，status表示 cur_pts和forw_pts中对应点对是否跟踪成功。
在这里插入图片描述
除此之外还要判断跟踪成功的点是否都在图像，有部分点可能不在图像内，调用inBorder() 函数计算点forw_pts坐标是否在图像内。若不在，则status置为0.

4、更新跟踪成功点

对3中成功跟踪的点（除去跟踪失败的点与不在图像内的点）进行保留，即status = 1的点。

void reduceVector(vector<cv::Point2f> &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);
}

在这里插入图片描述
再对成功跟踪的点进行重排列。此时将第二次跟踪到的点track_cnt加1

5、剔除误匹配点

调用rejectWithF() 函数，实际是调用findFundamentalMat() 函数，通过F矩阵来剔除误匹配点。

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;
            //根据不同的相机模型将二维坐标转换到三维坐标
            //通过liftProjective函数先将cur_pts像素坐标转换为归一化图像坐标，再判断是否有畸变
            //有的话进行畸变矫正操作，最后转换为三维坐标
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            //转换为归一化像素坐标  u = fX/Z +cx , v = fY/Z + cy
            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());
    }
}