一、主函数

主函数,程序入口:

image-20220530212530040

1、ros 初始化和设置句柄,设置 logger 级别

ros::init(argc, argv, "feature_tracker"); // ros 节点初始化
 ros::NodeHandle n("~"); // 声明一个句柄,~代表这个节点的命名空间:feature_tracker/feature
 ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); //设置 logger 级别

2、读取配置参数 (函数:../parameters.cpp)

readParameters(n); // 传入参数就是定义的句柄 n

读取配置参数的函数调用过程

3、读取每个相机实例对应的相机内参,NUM_OF_CAM=1 为单目

for (int i = 0; i < NUM_OF_CAM; i++)
    trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

查看函数readIntrinsicParameter:

// 读取相机内参
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    // 读取相机内参赋给 m_camera (这里涉及 单例设计模式)
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

4、判断是否加入鱼眼 mask 来去除边缘噪声

if(FISHEYE)
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
        if(!trackerData[i].fisheye_mask.data)
        {
            ROS_INFO("load mask fail");
            ROS_BREAK();
        }
        else
            ROS_INFO("load mask success");
    }
}

5、订阅话题 IMAGE_TOPIC(如 / cam0/image_raw),执行回调函数 img_callback

详细请戳>>>

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

这一步就是前端预处理最重要、最核心的地方,回调函数的含义就是,每订阅到一条数据就执行一次;

6、发布 feature,实例 feature_points,即跟踪的特征点,给后端优化用

pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);

发布 feature_img,实例 ptr,即跟踪的特征点图,给 RVIZ 用和调试用

7、ros::spin()

ros::spin();

ros::spin()用于调用所有可触发的回调函数, 将进入循环,不会返回,类似于在循环里反复调用spinOnce(), 如果使用ros::spinOnce(),则只会去触发一次; ros::spin()就像一个“监督者”,实时查看有没有新的话题订阅进来,从而进入循环,反复执行回调函数;

二、回调函数img_callback

img_callback

1、判断是否是第一帧

如果是第一帧,赋给其时间戳

if(first_image_flag) // 如果是第一帧图像
{
    first_image_flag = false;
    first_image_time = img_msg->header.stamp.toSec();
    last_image_time = img_msg->header.stamp.toSec();
    return;
}

2、判断时间间隔是否正确,异常则 restart

判断①图像当前时间戳和上一帧的时间戳是否出超过1s(超过说明时间间隔太大了,出现了异常) 以及 ②当前时间戳小于上一帧的时间戳,即时间戳错乱(异常)

if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
    // 异常之后的一些常规的reset
    ROS_WARN("image discontinue! reset the feature tracker!");
    first_image_flag = true; 
    last_image_time = 0;
    pub_count = 1;
    std_msgs::Bool restart_flag;
    restart_flag.data = true;
    pub_restart.publish(restart_flag); // 发布复位指令,告诉其他模块(如后端)要重启了
    return;
}
last_image_time = img_msg->header.stamp.toSec();

3、发布频率控制,并不是每读入一帧图像,就要发布特征点;

$$ 平均频率=\frac{发布次数}{间隔时间}\leq设定频率 $$

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
    PUB_THIS_FRAME = true; // 发布此帧

    // 时间间隔内的发布频率十分接近设定频率时,我们就认为这段时间甚佳,重启一下,避免时间间隔过大,对频率变化不敏感
    if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
    {
        // 更新时间间隔起始时刻,并将数据发布次数置0
        first_image_time = img_msg->header.stamp.toSec();
        pub_count = 0;
    }
}
else
    PUB_THIS_FRAME = false;

4、将图像编码 8UC1 转换为 mono8

由于ROS订阅到的图像的消息是ROS的消息格式,为我们后续需要调用OpenCV格式,因此需要把 img_msg 转成 CV::Mat

cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding == "8UC1")
{
    sensor_msgs::Image img;
    img.header = img_msg->header;
    img.height = img_msg->height;
    img.width = img_msg->width;
    img.is_bigendian = img_msg->is_bigendian;
    img.step = img_msg->step;
    img.data = img_msg->data;
    img.encoding = "mono8";
    // cv_bridge 的作用就是把ROS 的图片消息格式  转换成OpenCV图像的格式
    ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
    ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);

cv::Mat show_img = ptr->image;

5、进行光流追踪

这是回调函数的核心部分,也是前端预处理的核心部分,重点就是光流追踪的函数,readImage

详细请戳>>>

FeatureTracker trackerData[NUM_OF_CAM];
TicToc t_r;
for (int i = 0; i < NUM_OF_CAM; i++)
{
    ROS_DEBUG("processing camera %d", i);
    if (i != 1 || !STEREO_TRACK)
        // 单目时:FeatureTracker::readImage() 函数读取图像数据进行处理
        // 如果是双目相机图象是并排存储,这里是单目,所以 i=0
        /**
             * @description: 光流跟踪算法的核心:readImage
             * @param {ptr->image.rowRange()}  = CV::Mat 图像信息
             * @param {img_msg->header.stamp.toSec()}  = 时间戳
             * @return {void} 
             */
        trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
    else
    {
        if (EQUALIZE)
        {
            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
            clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
        }
        else
            trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
    }
#if SHOW_UNDISTORTION
    trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}

6、更新全局 ID

for (unsigned int i = 0;; i++)
{
    bool completed = false;
    for (int j = 0; j < NUM_OF_CAM; j++)
        if (j != 1 || !STEREO_TRACK)
            completed |= trackerData[j].updateID(i);
    if (!completed)
        break;
}

7、如果 PUB_THIS_FRAME=1 则进行发布

if (PUB_THIS_FRAME)
 {
     pub_count++; // 计数器更新
     sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
     sensor_msgs::ChannelFloat32 id_of_point;
     sensor_msgs::ChannelFloat32 u_of_point;
     sensor_msgs::ChannelFloat32 v_of_point;
     sensor_msgs::ChannelFloat32 velocity_x_of_point;
     sensor_msgs::ChannelFloat32 velocity_y_of_point;

     feature_points->header = img_msg->header;
     feature_points->header.frame_id = "world";

     vector<set<int>> hash_ids(NUM_OF_CAM); 
     for (int i = 0; i < NUM_OF_CAM; i++)
     {
         // trackerData[i]中的参数都是readImg函数处理后得到的;
         auto &un_pts = trackerData[i].cur_un_pts;  // 去畸变后的归一化相机坐标系下的坐标
         auto &cur_pts = trackerData[i].cur_pts;    // 像素坐标
         auto &ids = trackerData[i].ids;// id
         auto &pts_velocity = trackerData[i].pts_velocity;// 归一化下的速度
         for (unsigned int j = 0; j < ids.size(); j++)
         {   
             // 只发布追踪次数大于1的点,因为等于1的点无法构成重投影约束,也没办法三角化
             if (trackerData[i].track_cnt[j] > 1)
             {
                 int p_id = ids[j];
                 hash_ids[i].insert(p_id);
                 geometry_msgs::Point32 p; // ROS消息格式
                 p.x = un_pts[j].x; 
                 p.y = un_pts[j].y;
                 p.z = 1;
                 // 将数据存入定义的ROS消息格式
                 feature_points->points.push_back(p);
                 id_of_point.values.push_back(p_id * NUM_OF_CAM + i); //id
                 // 存入像素坐标
                 u_of_point.values.push_back(cur_pts[j].x);  
                 v_of_point.values.push_back(cur_pts[j].y);
                 // 存入像素速度
                 velocity_x_of_point.values.push_back(pts_velocity[j].x);
                 velocity_y_of_point.values.push_back(pts_velocity[j].y);
             }
         }
     }
     feature_points->channels.push_back(id_of_point);
     feature_points->channels.push_back(u_of_point);
     feature_points->channels.push_back(v_of_point);
     feature_points->channels.push_back(velocity_x_of_point);
     feature_points->channels.push_back(velocity_y_of_point);
     ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
     // skip the first image; since no optical speed on frist image
     if (!init_pub)
     {
         init_pub = 1;
     }
     else
         pub_img.publish(feature_points); // 前端得到的消息通过这个Publisher发布出去

8、可视化相关的操作

if (SHOW_TRACK)
{
    ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
    //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
    cv::Mat stereo_img = ptr->image;

    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
        cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);

        for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
        {
            double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
            cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
        }
    }
    pub_match.publish(ptr->toImageMsg());
}

三、光流追踪readImage

/**
 * @description: 对图像使用光流法进行特征点跟踪
 * @param[in] {Mat} &_img  输入图像
 * @param[in] {double} _cur_time 图像的时间戳
 * @return {void}
 */
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)

readImage光流追踪流程

1.图像的均衡化处理

// 如果EQUALIZE=1,表示太亮或则太暗,需要进行均衡化处理,提升图片对比度,有助于提取角点
if (EQUALIZE) // 需要均衡化
{
    // createCLAHE() 判断并对图像进行自适应直方图均衡化;
    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;

// 这里frow表示当前,curr表示上一帧
if (forw_img.empty())
{
    //如果当前帧的图像数据forw_img为空,说明当前是第一次读入图像数据
    //将读入的图像赋给当前帧forw_img
    //同时,还将读入的图像赋给prev_img、cur_img,这是为了避免后面使用到这些数据时,它们是空的
    prev_img = cur_img = forw_img = img;
}
else
{
    //否则,说明之前就已经有图像读入
    //所以只需要更新当前帧forw_img的数据
    forw_img = img;
}

直方图均衡化:一般可以用来提升图片的亮度

  • cv2.equalizeHist (进行直方图均衡化)

示例代码:

import cv2
import numpy as np
import matplotlib.pyplot as plt

# 第一步:读入图片
img = cv2.imread('./lena.jpg', 0)

# 第二步: 使用 cv2.equalizeHist 实现像素点的均衡化
ret = cv2.equalizeHist(img)

# 第三步:使用 plt.hist 绘制像素直方图
plt.subplot(121)
plt.hist(img.ravel(), 256)
plt.subplot(122)
plt.hist(ret.ravel(), 256)
plt.show()

# 第四步:使用 cv2.imshow() 绘值均衡化的图像
cv2.imshow('ret', np.hstack((img, ret)))
cv2.waitKey(0)

equalizeHist-灰度直方图

直方图均衡化效果图对比

  • cv2.createCLAHA(clipLimit, titleGridSize) (用于生成自适应均衡化图像)

clipLimit 颜色对比度的阈值

titleGridSize 进行像素均衡化的网格大小,即在多少网格下进行直方图的均衡化操作

示例代码:

import cv2
import numpy as np
import matplotlib.pyplot as plt

# 直方图均衡化
img = cv2.imread('./lena.jpg', 0)
ret = cv2.equalizeHist(img)

# 使用自适应直方图均衡化
# 第一步:实例化自适应直方图均衡化函数
clahe = cv2.createCLAHE(clipLimit=2.0,tileGridSize=(8, 8))
# 第二步:进行自适应直方图均衡化
clahe = clahe.apply(img)

# 第三步:进行图像的展示
cv2.imshow('imgs', np.hstack((img, ret, clahe)))
cv2.waitKey(0) 
cv2.destroyAllWindows()

原图、直方图均衡化、自适应均衡化效果对比

可以发现:对于全局的直方图均衡化会存在一些问题,由于整体亮度的提升,也会使得局部图像的细节变得模糊(上图中间的图像),因为我们需要进行分块的局部均衡化操作,而自适应均衡化没有使得人物脸部的细节消失(上图右一的图像)

2.进行光流追踪

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

if (cur_pts.size() > 0) // 上一帧有特征点,就可以进行光流追踪了
{
    TicToc t_o;
    vector<uchar> status;
    vector<float> err;
    // calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪,得到forw_pts
    //status标记了从前一帧cur_img到forw_img特征点的跟踪状态,无法被追踪到的点(outlier)status标记为0(删除)
    cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

    //将位于图像边界外的点,status标记为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,记录特征点 id 的 ids,和记录特征点被跟踪次数的 track_cnt 中剔除;
    reduceVector(prev_pts, status);
    reduceVector(cur_pts, status);
    reduceVector(forw_pts, status);
    reduceVector(ids, status);        // 特征点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++;

稀疏光流法的参数:

void cv::calcOpticalFlowPyrLK(
    InputArray prevImg,          //上一帧单通道灰度图
    InputArray nextImg,          //当前帧单通道灰度图
    InputArray pre vPts,         //像素点上一帧像素坐标pts
    InputOutputArray nextPts,    //像素点当前帧像素坐标pts
    OutputArray status,          // 输出状态向量,(无符号字符);如果找到相应特征的流,则向量的每个元素设置为1,否则设置为0。
    OutputArray err,             //输出错误的矢量;如果未找到流,则未定义错误(使用status参数查找此类情况)。
    Size winSize = Size(21, 21), //每个金字塔等级的搜索窗口的winSize大小。
    int maxLevel = 3,// 基于0的最大金字塔等级数;如果设置为0,则不使用金字塔(单级),如果设置为1,则使用两个级别,依此类推;
    TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01), //参数,指定迭代搜索算法的终止条件
    int flags = 0,
    double minEigThreshold = 1e-4 //算法计算光流方程的2x2正常矩阵的最小特征值,除以窗口中的像素数;
)

关于光流法的详细介绍,参考 [ch8_使用LK光流法]()

3.基本矩阵剔除 outliers

这一步主要调用函数rejectWithF(),通过利用对及约束,求解基本矩阵F,来剔除 outliers 点,该函数的实现主要分为两步,首先需要利用函数 liftProjective 将像素坐标转换为无畸变的归一化坐标,然后将处理过的无畸变的归一化坐标传给cv中的函数 findFundamentalMat,最后利用该函数的 F_THRESHOLD(门限)和status 参数来进行 outliers 点的剔除。

基本矩阵剔除 outliers

关于去畸变的方式,VINS并没有直接使用OpenCV中的去畸变函数,而是采用了迭代逼近的方式去畸变,这种方法比起OpenCV中的方法更加迅速,因为VINS采用的是正向计算,并没有像OpenCV那样反向求解高度非线性的畸变函数,可谓是十分之精妙!在其他文章中有单独的介绍,此处不做赘述。此处放链接___________________

cv2::findFundamentalMat(points1, points2, method, param1, param2, status)

points1 – 包含第一张图像的 N 个点的数组

points2 – 与 points1 具有相同大小和格式的第二个图像点的数组

method – 计算基本矩阵的方法。

CV_FM_7POINT for a 7-point algorithm. N = 7
CV_FM_8POINT for an 8-point algorithm. N>=8
CV_FM_RANSAC for the RANSAC algorithm. N>=8
CV_FM_LMEDS for the LMedS algorithm. N>=8

param1 – 用于 RANSAC 的参数。 它是从一个点到一条以像素为单位的对极线的最大距离,超过该距离的点被认为是异常值,不用于计算最终的基本矩阵。 它可以设置为 1-3 之类的值,具体取决于点定位的准确性、图像分辨率和图像噪声。
param2 –仅用于 RANSAC 或 LMedS 方法的参数。 它指定了估计矩阵正确的理想置信水平(概率)。
status– 输出包含 N 个元素的数组,其中的每个元素对于异常值都设置为 0,对于其他点设置为 1。 该数组仅在 RANSAC 和 LMedS 方法中计算。 对于其他方法,它设置为全 1。

4.去除密集点setMask()

用途:该函数主要是对图像上的特征点进行一次整理。避免过于拥挤

思路:先对特征点进行按照被追踪次数的降序排序,随后按顺序将这些点标记在mask上,并对周围半径为30以内的特征点删除,避免拥挤。

什么是Mask?

首先,对于我这种小白,必须先知道什么是mask?

mask又称为图像掩膜,用选定的图像、图形或物体,对处理的图像(全部或局部)进行遮挡,来控制图像处理的区域或处理过程。它的作用主要如下:

  1. 提取感兴趣区:用预先制作的感兴趣区掩膜与待处理图像相乘,得到感兴趣区图像,感兴趣区内图像值保持不变,而区外图像值都为0;
  2. 屏蔽作用:用掩膜对图像上某些区域作屏蔽,使其不参加处理或不参加处理参数的计算,或仅对屏蔽区作处理或统计;
  3. 结构特征提取:用相似性变量或图像匹配方法检测和提取图像中与掩膜相似的结构特征;
  4. 特殊形状图像的制作

以图和掩膜的与运算为例

原图中的每个像素和掩膜中的每个对应像素进行与运算。比如1 & 1 = 1;1 & 0 = 0;图像中,各种位运算,比如与、或、非运算与普通的位运算类似。
比如一个3 3的图像与3 3的掩膜进行运算,得到的结果图像就是:

image-20220604142557126

cv::circle

介绍相关代码之前,先熟悉一个OpenCV中的函数:

void cv::circle    (    InputOutputArray     img,//画圆的图像
                                Point     center,//圆的中心
                                int     radius,//圆的半径.
                                const Scalar &     color,//圆的颜色
                                int     thickness = 1,//果是正数,表示组成圆的线条的粗细程度。如果是负值(如FILLED),表示圆是否被填充
                                int     lineType = LINE_8,//线条的类型。默认是8
                                int     shift = 0 //圆心坐标点和半径值的小数点位数
)

setMask()

通过上面对mask的相关介绍,我们就很容易理解,作者是如何做到将图像中拥挤的点进行均匀化的。

// 对跟踪点进行排序并去除密集点:
// 对跟踪到的特征点,按照被追踪到的次数排序并依次选点;
// 使用 mask 进行类似非极大抑制的方法,半径为 30,去掉分部密集的点,使特征点分布均匀
void FeatureTracker::setMask()
{
    if (FISHEYE)
        mask = fisheye_mask.clone();
    else
        // 生成一个ROW*COL大小的8位单通道,初始像素全为255(白色)的mask
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));

    // prefer to keep features that are tracked for long time
    // 构造(cnt,pts,id)序列
    vector<pair<int, pair<cv::Point2f, int> > > cnt_pts_id;

    for (unsigned int i = 0; i < forw_pts.size(); i++)
        // 第一个pair< 跟踪次数, 第二个pair<当前帧的像素点的坐标,特点的Id>  >
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));

    //对光流跟踪到的特征点forw_pts,按照被跟踪到的次数cnt从大到小排序(因为点被追踪的次数越多,说明点越稳定)
    // 利用STL的自定义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);//MIN_DIST=30
        }
    }
}

5.提取新的特征点

如果说,我们的特征点的最大限度为MAX_CNT,我们不需要保持特征点的分布均匀,也不能使得特征点过于稀疏,当特征点过少时,我们需要调用OpenCV中的函数,来提取新的特征点。

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;
    // goodFeaturesToTrack() 寻找新的特征点 (SHI-Tomasi 角点),
    cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
    n_pts.clear();
void cv::goodFeaturesToTrack    (
    InputArray         image, //输入图像
    OutputArray     corners,//存放检测到的角点的vector
    int             maxCorners,//回的角点的数量的最大值
    double             qualityLevel,//角点质量水平的最低阈值(范围为0到1,质量最高角点的水平为1),小于该阈值的角点被拒绝
    double             minDistance,//返回角点之间欧式距离的最小值
    InputArray     mask = noArray(),//和输入图像具有相同大小,类型必须为CV_8UC1,用来描述图像中感兴趣的区域,只在感兴趣区域中检测角点
    int    blockSize = 3,//计算协方差矩阵时的窗口大小
    bool useHarrisDetector = false,//指示是否使用Harris角点检测,如不指定,则计算shi-tomasi角点
    double     k = 0.04 //Harris角点检测需要的k值
)

6.添加新的追踪点

// 添将新检测到的特征点n_pts,ID初始化-1,跟踪次数1
void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1);
        track_cnt.push_back(1);
    }
}

7.畸变校正与速度计算

这里主要还是作者自己创新的去畸变方法 liftProjective(),文章 ==========已经叙述

// 对特征点的图像坐标去畸变矫正,并计算每个角点的速度
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())));
    }
    // 计算点的速度
    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;
}
🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/66.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 28 日
赏杯咖啡