Vins-Mono 论文 && Coding 一 2. feature_tracker

最新推荐文章于 2022-07-31 10:05:59 发布

scofield_ly

最新推荐文章于 2022-07-31 10:05:59 发布

阅读量289

点赞数

分类专栏： # vio 文章标签： slam

本文链接：https://blog.youkuaiyun.com/weixin_40072161/article/details/108310441

版权

vio 专栏收录该内容

13 篇文章

订阅专栏

本文围绕VINS-Mono代码展开，先介绍了相关文件和成员变量，接着阐述视觉特征提取与关联过程，包括图像预处理、KLT光流跟踪、过滤无效点、提取新角点等步骤，还说明了设置mask的作用，最后提及视觉特征跟踪结果发布的主题。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

一、代码注释

1. 相关文件

VINS-Mono/feature_tracker/src/feature_tracker.h

VINS-Mono/feature_tracker/src/feature_tracker.cpp

2. 成员变量

class FeatureTracker

prev_frame(current frame 的上一帧)	double prev_time	prev_frame 时间戳
	cv::Mat prev_img	prev_frame 图像
	vector<cv::Point2f> prev_pts	prev_frame 角点(未去畸变)
	vector<cv::Point2f> prev_un_pts	prev_frame 角点(去畸变)
cur_frame (当前处理完的帧)	double cur_time	cur_frame 时间戳
	cv::Mat cur_img	cur_frame 图像
	vector<cv::Point2f> cur_pts	cur_frame 角点(未去畸变)
	vector<cv::Point2f> cur_un_pts	cur_frame 角点(去畸变)
forw_frame(新加入待处理的帧)	cv::Mat forw_img	forw_frame 图像
forw_frame(新加入待处理的帧)	vector<cv::Point2f> forw_pts	forw_frame 角点
id	vector<int> ids	当前帧各个角点的 id
跟踪次数	vector<int> track_cnt	当前帧各个角点的跟踪次数
id 计数	int n_id	id 计数器, 用于给每个角点分配 id
计算光流速度相关	vector<cv::Point2f> pts_velocity map<int, cv::Point2f> cur_un_pts_map; map<int, cv::Point2f> prev_un_pts_map;	cur_pts 光流速度(prev_frame 没有对应角点，设置为0) cur_pts : id <--> 去畸变像素 prev_pts : id <--> 去畸变像素
角点提取 mask	cv::Mat mask	角点提取 mask img
角点提取结果	vector<cv::Point2f> n_pts;	角点提取结果

二、视觉特征提取 && 关联

1. 图像预处理

直方图均衡化 (optional)

cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
clahe->apply(_img, img);

2. 特征跟踪

(1). KLT 光流跟踪

输入: cur_frame(上一帧图像)， forw_frame(当前帧图像)， cur_pts(上一帧角点)

输出: forw_pts(当前帧跟踪的角点)，status (跟踪状态)

// status ：输出状态向量（无符号字符）;如果找到相应特征的光流，则向量的每个元素设置为1，否则设置为0
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

(2). 过滤无效点

 for (int i = 0; i < int(forw_pts.size()); i++)
     if (status[i] && !inBorder(forw_pts[i]))
        status[i] = 0;

(3). 依据 status, 删除无效的关联

reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);

(4). 更新角点的跟踪次数

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

(5). 依据 Fundamental Matrix 剔除 outlier

void FeatureTracker::rejectWithF() {
  if (forw_pts.size() >= 8) {
    // 恢复去畸变后的像素坐标
    // cur_pts -> un_cur_pts
    // forw_pts -> un_forw_pts
    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++) {
     ......
    }
    // fundamental matrix RANSAC
    vector<uchar> status;
    cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD,
                           0.99, status);
    // 剔除 outlier
    reduceVector(prev_pts, status);
    reduceVector(cur_pts, status);
    reduceVector(forw_pts, status);
    reduceVector(cur_un_pts, status);
    reduceVector(ids, status);
    reduceVector(track_cnt, status);
  }
}

3. 提取新的角点

(1). 设置 mask img

void FeatureTracker::setMask()

i. 像素值全部初始化为 255

// mask: 单通道，初始化像素值全部为 255
if (FISHEYE)
  mask = fisheye_mask.clone();
else
  mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));

ii. 跟踪成功的角点邻域处 mask 设置为 0

按照跟踪次数排序，跟踪次数多的优先设置 mask

// prefer to keep features that are tracked for long time
// 将角点按照 跟踪次数排序，跟踪次数多的在前面
......

// 按照跟踪次数依次设置 mask, 对应像素值设置为 0
for (auto &it : cnt_pts_id) {
  if (mask.at<uchar>(it.second.first) == 255) {
    forw_pts.push_back(it.second.first);
    ids.push_back(it.second.second);
    track_cnt.push_back(it.first);
    cv::circle(mask, it.second.first, MIN_DIST, 0, -1);  // 已有角点半径邻域内 mask 设置为  0
  }
}

(2). 提取新的角点

i. 依据前面设置的 mask，继续在当前帧提取其余角点

// MAX_CNT - forw_pts.size(): 角点提取最大值
// mask: mask img, 标记为 0 处，不进行角点提取
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01,
                         MIN_DIST, mask);

这里设置 mask 的作用

a: 避免和 klt 跟踪的角点冲突

b: 设置最小半径 mask，去掉分部密集的点，使特征点分布均匀

mask 可视化结果如下

ii. 添加新提取的角点

void FeatureTracker::addPoints() {
  for (auto &p : n_pts) {
    forw_pts.push_back(p);
    ids.push_back(-1);
    track_cnt.push_back(1);
  }
}

4. 对角点去畸变，并计算光流速度

void FeatureTracker::undistortedPoints() {
  // 对 cur_pts 去畸变
  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);
  }

  // 计算每个角点的光流速度, 计算失败设置为0
  // caculate points 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()) {  // 通过 id 关联
          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;
}

三、视觉特征跟踪结果发布

topic: "feature"

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;

// 存储像素坐标
for (int i = 0; i < NUM_OF_CAM; i++) {
    auto &un_pts = trackerData[i].cur_un_pts;  // 去畸变后像素 vector
    auto &cur_pts = trackerData[i].cur_pts;  // 去畸变前像素 vector
    auto &ids = trackerData[i].ids;  // track-id  vector
    auto &pts_velocity = trackerData[i].pts_velocity; // 光流速度 vector
    for (unsigned int j = 0; j < ids.size(); j++) {
      if (trackerData[i].track_cnt[j] > 1) {
            int p_id = ids[j];
            geometry_msgs::Point32 p;
            p.x = un_pts[j].x;
            p.y = un_pts[j].y;
            p.z = 1;
            feature_points->points.push_back(p);  // points: xyz: u,v,1
            id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
            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);  // channe0: track_id
feature_points->channels.push_back(u_of_point);  // channel1: 去畸变前 u 分量
feature_points->channels.push_back(v_of_point);  // channel2: 去畸变前 v 分量
feature_points->channels.push_back(velocity_x_of_point);  // channel3: 光流速度 x 分量
feature_points->channels.push_back(velocity_y_of_point);  // channel4: 光流速度 y 分量
pub_img.publish(feature_points);