VINS-Mono笔记02_前端特征点追踪feature_tracker

预备知识

ROS预定义消息格式

ROS节点间是以话题(topic)形式传递消息的,每个话题被发布时都要指定其消息格式.除基本的bool、int、string、time等基础数据格式外,ROS还预定义了一系列用于机器人开发的消息格式,它们被定义在头文件目录下,VINS-Mono用到了其中的Image、PointCloud和Imu.

图像消息格式Image

图像消息格式Image的定义如下：

Header header       	# 消息头

uint32 height         	# 图像高度
uint32 width          	# 图像宽度
string encoding       	# 图像编码
uint8 is_bigendian    	# 数据字节序
uint32 step           	# 一行图像数据对应的宽度
uint8[] data          	# 图像数据,长度为(step * height)

其中消息头Header也是ROS预定义的一种消息格式,定义如下:

uint32 seq				# 自增ID
time stamp				# 时间戳
string frame_id			# 对应帧的ID(对于双目或RGBD传感器,可能存在多个图像对应同一frame_id)

在C++中对应的类是sensor_msgs::Image,位于头文件sensor_msgs/Image.h中,ROS节点以其指针类型sensor_msgs::ImageConstPtr作为参数接收该消息.

// 接收Image格式消息的回调函数
void img_callback(const sensor_msgs::ImageConstPtr &img_ptr){	// 接受参数是sensor_msgs::Image类型的指针
  sensor_msgs::Image img = *img_ptr;		// 得到原始sensor_msgs::Image数据
    
  // 具体执行的动作
  // ...
}

调用函数cv_bridge::toCvCopy或cv_bridge::toCvShare都可以将ROS的图像消息类型sensor_msgs::Image转为OpenCV的图像类型cv_bridge::CvImage.区别在于cv_bridge::toCvCopy将源sensor_msgs::Image中的图像数据data复制了一份才进行格式转换,而cv_bridge::toCvShare直接对源sensor_msgs::Image中的图像数据data操作.

CvImagePtr cv_bridge::toCvCopy(	
  const sensor_msgs::Image& source,
  const std::string& encoding = std::string()	 
)

CvImagePtr cv_bridge::toCvCopy(
  const sensor_msgs::ImageConstPtr& source,
  const std::string& encoding = std::string()	 
)	

CvImageConstPtr cv_bridge::toCvShare(
  const sensor_msgs::Image& source,
  const boost::shared_ptr<void const>& tracked_object,
  const std::string& encoding = std::string()	 
)	

CvImageConstPtr cv_bridge::toCvShare(
  const sensor_msgs::ImageConstPtr& source,
  const std::string& encoding = std::string()	 
)	

实际上cv_bridge::toCvCopy和cv_bridge::toCvShare的返回值类型并非直接的OpenCV图像数据格式cv::Mat,而是封装了cv::Mat的智能指针CvImagePtr.

点云消息格式PointCloud

点云消息格式PointCloud的定义如下

Header header						# 消息头

geometry_msgs/Point32[] points		# 3D点数组
ChannelFloat32[] channels			# 通道数组,每个通道的长度应与points长度相同

消息头Header的定义与Image中的消息头相同.

3D点消息格式geometry_msgs/Point32.msg的定义如下:

float32 x
float32 y
float32 z

通道消息格式sensor_msgs/ChannelFloat32表示一个数组,用来存放自定义属性,PointCloud定义中的ChannelFloat32[] channels是一个Channel数组,也就是二维数组,其中存储的每个一维数组都对应一个自定义属性,长度与points相同.

string name				# 通道属性名
float32[] values		# 属性值列表,长度应与points相同

在C++中对应的类是sensor_msgs::PointCloud,位于头文件sensor_msgs/PointCloud.h中,ROS节点以其指针类型sensor_msgs::PointCloudConstPtr作为参数接收该消息.

ROS节点的格式

一个ROS节点本质上是一个独立的程序,对应一个main函数.ROS节点的main函数有一个约定俗成的格式:main函数一般包括以下步骤:

1. 初始化ROS节点.
2. 声明当前节点订阅的话题,并定义对应的回调函数,每收到一条话题数据,ROS调用一次回调函数.
3. 声明当前节点发布的话题.
4. 调用ros::spin(),将程序的控制权交给ROS调度.

例如feature_tracker包下的feature_tracker节点的源码feature_tracker_node.cpp如下:

#include 

ros::Publisher pub_img;
ros::Publisher pub_match;
ros::Publisher pub_restart;

// 图像消息的回调函数
void img_callback(const sensor_msgs::ImageConstPtr &img_ptr) {
    // 回调函数内容...
}

// 主函数
int main(int argc, char **argv) {
  // step1. 初始化ROS节点
  ros::init(argc, argv, "feature_tracker"); 	// 初始化名为feature_tracker的ROS节点
  ros::NodeHandle n("~");                    	// 声明该节点的句柄

  // 初始化相机
  // ...

  // step2. 声明当前节点订阅话题"/cam0/image_raw",缓冲队列长度为100,每收到一条该话题消息就调用一次img_callback
  ros::Subscriber sub_img = n.subscribe("/cam0/image_raw", 100, img_callback);

  // step3. 声明当前节点发布的话题
  pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);	// 发布话题/feature_tracker/feature
  pub_match = n.advertise<sensor_msgs::Image>("feature_img", 1000); // 发布话题/feature_tracker/feature_img
  pub_restart = n.advertise<std_msgs::Bool>("restart", 1000);       // 发布话题/feature_tracker/restart

  // step4. 将程序的控制权交给ROS调度
  ros::spin();
  return 0;
}

前端特征点追踪节点feature_tracker

节点feature_tracker接收相机图像话题/cam0/image_raw,输出3个话题:

• 特征点云/feature_tracker/feature,用于后端优化.
• 重启命令/feature_tracker/restart,在时间戳错乱时通知后端重启,正常情况下不被用到.
• 特征点可视化图/feature_tracker/feature_img,用于rviz可视化,不用于SLAM后端.

feature_tracker节点对输入图像的操作位于回调函数img_callback中,程序流程图如下:

否

是

否

是

是否起始帧

记录起始帧时间

是否时间戳错乱

重启VINS-Mono

根据发送频率判断
是否发布前一帧特征点追踪结果

对特征点进行光流追踪

(若之前步骤判断需要)
发布前一帧特征点追踪结果

step1. 起始帧判断

变量bool first_image_flag指示当前帧是否为起始帧,在程序启动时设为true,处理完起始帧后设为false.

bool first_image_flag = true;		// 当前帧是否为起始帧
double first_image_time;			// 起始帧时间戳
double last_image_time;				// 最新帧时间戳

void img_callback(const sensor_msgs::ImageConstPtr &img_msg) {
  // step1. 判断起始帧
  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;
  }

  // step2~5...
}

step2. 错乱时间戳判断

若当前帧与前一帧时间戳逆序或帧间隔大于一秒,则重启VINS-Mono,重启过程分为两步:

• 重置当前节点各变量.
• 向话题/feature_tracker/restart发送信号,通知其他节点重启.

bool first_image_flag = true;		// 当前帧是否为起始帧
double first_image_time;			// 起始帧时间戳
double last_image_time;				// 最新帧时间戳

void img_callback(const sensor_msgs::ImageConstPtr &img_msg) {
  // step1...

  // step2. 判断时间戳是否错乱
  if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) {
    ROS_WARN("image discontinue! reset the feature tracker!");
    // 重置当前节点变量
    first_image_flag = true;
    last_image_time = 0;
    pub_count = 1;
  	// 向话题/feature_tracker/restart发送信号,通知其他节点重启 
    std_msgs::Bool restart_flag;
    restart_flag.data = true;
    pub_restart.publish(restart_flag);
    return;
  }
  last_image_time = img_msg->header.stamp.toSec();

  // step3~5...
}

step3. 特征点发布频率控制

节点feature_tracker提取到的特征点会发布到话题/feature_tracker/feature上,发布的频率由配置文件Vins-Mono/config/euroc/euroc_config.yaml中的配置项freq指定.数据集发布相机图像话题/cam0/image_raw的频率未必与配置文件指定的发布频率相同(数据集发布图像的频率一般会高于节点feature_tracker发布特征点的频率),因此需要进行频率控制,抽帧发布特征点追踪结果.
$$\text{实际发布频率}=\frac{\text{已发布帧数}pub\_count}{\text{统计时长}(img\_msg\to header.stamp.toSec())-first\_image\_time}$$
通过计算当前实际发布频率pub_count/(img_msg->header.stamp.toSec()-first_image_time),将其与给定发布频率FREQ比较,决定是否发布当前帧特征点追踪结果,存入变量PUB_THIS_FRAME中.

系统长时间运行后,从数值上来说,实际频率计算式的分子pub_count和分母img_msg->header.stamp.toSec()-first_image_time都较大,这样系统对瞬时发布速率变化不敏感,容易造成瞬时数据拥堵(连续几帧都发布或连续几帧都不发布).为避免瞬时数据拥堵,需要周期性重置计数器pub_count和first_image_time.

实践上选择当实际频率十分接近给定频率时,重置计数器.

double first_image_time;			// 起始帧时间戳
int pub_count = 0;					// 已经发布的帧数
bool PUB_THIS_FRAME;				// 是否发布前一帧特征点
extern int FREQ;					// 给定发布频率

void img_callback(const sensor_msgs::ImageConstPtr &img_msg) {
  // step1~2...

  // step3. 特征点发布频率控制
  // step3.1. 计算实际发布频率,决定是否发布本帧
  if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ) {
    PUB_THIS_FRAME = true;
    // step3.2. 实际发布频率接近给定发布频率时,重置计数器pub_count和first_image_time
    if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ) {
      first_image_time = img_msg->header.stamp.toSec();
      pub_count = 0;
    }
  } else {
    PUB_THIS_FRAME = false;
  }

  // step3~5...
}

step4. 对特征点进行光流追踪

VINS-Mono中定义FeatureTracker类用以跟踪特征点,在feature_tracker节点中调用其readImage方法进行光流追踪.

FeatureTracker tracker;		// 光流追踪对象

void img_callback(const sensor_msgs::ImageConstPtr &img_msg) {
  // step1~3...

  // step4. 对特征点进行光流追踪
  cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);	// 将图片信息转为OpenCV格式
  tracker.readImage(imagePtr->image, img_msg->header.stamp.toSec());	// 追踪特征点
  
  // 为新提取的特征点设置ID
  for (unsigned int i = 0;; i++) {
    if (!tracker.updateID(i))
      break;
  }

    
  // step5...
}

具体来说,readImage()方法对特征点进行光流追踪的程序流程图如下:

是

否

直方图均衡化
cv::createCLAHE()

光流追踪
cv::calcOpticalFlowPyrLK()

剔除追踪丢失的特征点
reduceVector()

是否发布当前帧特征点
PUB_THIS_FRAME

三角化剔除外点
FeatureTracker::rejectWithF()

特征点均匀化及追加提取
FeatureTracker::setMask()

去畸变及计算速度
FeatureTracker::undistortedPoints()

下面具体说明每一步骤:

step4.1. 直方图均衡化cv::CLAHE::apply()

当图片过暗或过亮时,难以提取特征点,需要进行直方图均衡化(histogram equalization),OpenCV中用于直方图均衡化的API主要有2个: cv::equalizeHist() 和cv::CLAHE::apply(),前者进行全局直方图均衡化,而后者进行分块自适应直方图均衡化,在图像直方图有多个峰值时,后者更有效.

extern int EQUALIZE;		// 是否需要直方图均衡化

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {
  // step4.1. 直方图均衡化
  cv::Mat img;
  if (EQUALIZE) {
    cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
    clahe->apply(_img, img);
  } else {
    img = _img;  
  }
    
  // step4.2~4.6...
}

step4.2. 光流追踪cv::calcOpticalFlowPyrLK()

class FeatureTracker {
 public:
  cv::Mat prev_img;					// 前两帧图像
  cv::Mat cur_img;					// 前一帧图像
  cv::Mat forw_img;					// 当前帧图像
  vector<cv::Point2f> prev_pts;		// 特征点在前两帧中的坐标
  vector<cv::Point2f> cur_pts;		// 特征点在前一帧中的坐标
  vector<cv::Point2f> forw_pts;		// 特征点在当前帧中的坐标
}

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {
  // step4.1...
    
  // step4.2 光流追踪特征点
  // step4.2.1. 更新各变量,为光流追踪做准备
  if (forw_img.empty()) {	// 若forw_img为空,说明是起始帧
    prev_img = cur_img = forw_img = img;
  } else {					// 否则说明是普通帧
    forw_img = img;
  }
  forw_pts.clear();
  // step4.2.2. 进行光流追踪  
  if (cur_pts.size() > 0) {
    vector<uchar> status;	// 用于保存追踪状态,1表示成功,0表示失败
    cv::calcOpticalFlowPyrLK(cur_img, 				// 前一帧图像
                             forw_img, 				// 当前帧图像
                             cur_pts, 				// 特征点在前一帧中的坐标
                             forw_pts, 				// 用于存储特征点在当前帧中的坐标
                             status, 				// 用于存储每个特征点的追踪状态
                             cv::noArray(), 		// 用于存储每个特征点的追踪误差
                             cv::Size(21, 21), 		// 窗口大小
                             3						// 金字塔层级
                            );


    // step4.2.3. 剔除图像边界外的特征点
    for (int i = 0; i < int(forw_pts.size()); i++)
      if (status[i] && !inBorder(forw_pts[i]))
        status[i] = 0;
  
	// step4.3~4.6...
  }
}

step4.3. 剔除追踪丢失的特征点reduceVector()

特征点的状态由多个vector组成,每个vector的第i个元素表示第i个特征点的对应属性.

reduceVector()函数有两个重载版本: void reduceVector(vector &v, vector status)和void reduceVector(vector &v, vector status),两个重载版本的作用和原理相同,都是根据第二个参数status压缩第一个参数v,将v中在status中对应位置上1的变量放到v的前半部分,同时保留其相对位置.

class FeatureTracker {
 public:
  vector<int> ids;					// 特征点ID
  vector<cv::Point2f> prev_pts;		// 特征点在前两帧中的坐标
  vector<cv::Point2f> cur_pts;		// 特征点在前一帧中的坐标
  vector<cv::Point2f> forw_pts;		// 特征点在当前帧中的坐标
  vector<int> track_cnt;			// 特征点成功追踪帧数
  vector<cv::Point2f> cur_un_pts;	// 特征点归一化坐标系下的坐标
}

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {
  // step4.1~4.2...

  if (cur_pts.size() > 0) {
    vector<uchar> status;	// 用于保存追踪状态,1表示成功,0表示失败
    cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, cv::noArray(), cv::Size(21, 21), 3);

    // step4.3. 剔除追踪丢失的特征点
	reduceVector(prev_pts, status);
    reduceVector(cur_pts, status);
    reduceVector(forw_pts, status);
    reduceVector(ids, status);
    reduceVector(cur_un_pts, status);
    reduceVector(track_cnt, status);
  }

  // step4.4~4.6...
}

reduceVector()函数的实现如下:

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

step4.4. 三角化剔除外点FeatureTracker::rejectWithF()

若当前帧被发布,则调用FeatureTracker::rejectWithF()根据每个特征点在前一帧和当前帧的坐标进行三角化,再调用cv::findFundamentalMat()计算基础矩阵F,根据输出的status剔除外点.

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {
  // step4.1~4.3...

  if (PUB_THIS_FRAME) {
    // step4.4. 三角化剔除外点rejectWithF()
    rejectWithF();
      
    // step4.5~4.6...
  }
}

其中PinholeCamera::liftProjective()函数通过迭代方式去畸变,具体原理后文介绍.

因为三角化的目的是剔除外点,而不需要真正得到具体的三维点坐标,故在实现上使用一个焦距FOCAL_LENGTH为460的虚拟相机模型.这样计算得到的三维点坐标当然是错误的,但是得到的status,即每对匹配的特征点是否是外点的信息是大致准确的,参见作者的解答.

void FeatureTracker::rejectWithF() {
  if (forw_pts.size() >= 8) {	// 八点法计算基础矩阵F,至少需要8个点
    vector<cv::Point2f> un_cur_pts(cur_pts.size());		// 存储特征点在前一帧中的去畸变坐标
	vector<cv::Point2f> un_forw_pts(forw_pts.size());	// 存储特征点在当前帧中的去畸变坐标
    for (unsigned int i = 0; i < cur_pts.size(); i++) {
      // step4.4.1. 计算特征点在前一帧中的去畸变坐标
      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());	// 像素平面坐标
    }
	
    // step4.4.2. 调用cv::findFundamentalMat()计算基础矩阵F
    vector<uchar> status;
    cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
    // step4.4.3. 调用reduceVector()剔除外点
    reduceVector(prev_pts, status);
    reduceVector(cur_pts, status);
    reduceVector(forw_pts, status);
    reduceVector(cur_un_pts, status);
    reduceVector(ids, status);
    reduceVector(track_cnt, status);
}

step4.5. 特征点均匀化及追加提取FeatureTracker::setMask()

若当前帧被发布,则调用FeatureTracker::setMask()均匀化特征点,并调用cv::goodFeaturesToTrack()追加提取新特征点.

extern int MAX_CNT;		// 追踪特征点个数
cv::Mat mask;			// 特征点提取掩码,用于指示提取特征点的区域

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {
  // step4.1~4.3...

  if (PUB_THIS_FRAME) {
    // step4.4. 
    rejectWithF();
      
    // step4.5. 特征点均匀化及追加提取  
	int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());	// 添加新特征点个数
    if (n_max_cnt > 0) {
      // step4.5.1. 特征点稀疏化并设置特征点提取掩码: 现有特征点周围30像素处不提取新特征点
      setMask();
      // step4.5.2. 调用cv::goodFeaturesToTrack()追加提取新特征点
      cv::goodFeaturesToTrack(forw_img, 				// 图像
                              n_pts, 					// 用于保存提取的特征点坐标
                              MAX_CNT - forw_pts.size(),// 提取特征点的个数
                              0.01, 					// 特征点质量
                              MIN_DIST, 				// 特征点间最小距离
                              mask);					// 提取区域
 	  // step4.5.3. 添加新提取的特征点
      for (auto &p : n_pts) {	
    	forw_pts.push_back(p);
    	ids.push_back(-1);		// 新特征点的ID初始化为-1
    	track_cnt.push_back(1);	// 新特征点成功追踪帧数初始化为1
  	  }
    }
  }
    
  // step4.6...
}

FeatureTracker::setMask()方法进行特征点稀疏化的同时设置特征点提取掩码: 30像素圆域内选取成功追踪帧数最大的特征点.

void FeatureTracker::setMask() {
  // 1. 特征点提取掩码初始化为全1矩阵
  mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));

  // 2. 将特征点按成功追踪帧数从大到小排序
  vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;		// tuple<追踪帧数，坐标，特征点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])));
	sort(cnt_pts_id.begin(),
         cnt_pts_id.end(),
         [](const auto &a, const const auto &b) {
             return a.first > b.first;
         });

  // 3. 清空特征点信息
  forw_pts.clear();
  ids.clear();
  track_cnt.clear();

  // 4. 均匀化提取特征点: 周围30像素圆域内的其他特征点被抛弃
  for (auto &it : cnt_pts_id) {
    if (mask.at<uchar>(it.second.first) == 255) {	// 周围30像素圆域内没有其他特征点,则保存该特征点
	  // 4.1. 保留该特征点
      forw_pts.push_back(it.second.first);
      ids.push_back(it.second.second);
      track_cnt.push_back(it.first);
      // 4.2. 标记周围30像素圆域内掩码
      cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
    }
  }
}

step4.6. 去畸变及计算特征点速度FeatureTracker::undistortedPoints()

在去畸变前调整迭代变量prev_xxx、cur_xxx和forw_xxx,为处理下一帧做准备.

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) {
  // step4.1~4.5...

  // step4.6. 去畸变及计算特征点速度
  // 为下一帧迭代准备变量
  prev_img = cur_img;
  prev_pts = cur_pts;
  prev_un_pts = cur_un_pts;
  cur_img = forw_img;
  cur_pts = forw_pts;
  // 调用FeatureTracker::undistortedPoints()去畸变及计算特征点速度
  undistortedPoints();		
  prev_time = cur_time;
}

方法FeatureTracker::undistortedPoints()先将所有坐标点去畸变后投影到归一化相机平面上,再计算特征点速度.

void FeatureTracker::undistortedPoints() {
  cur_un_pts.clear();		// 存储特征点的去畸变坐标
  cur_un_pts_map.clear();	// 存储特征点ID到其去畸变坐标的映射
  
  // step4.6.1. 将特征点去畸变后投影到归一化相机平面上
  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())));
  }

  // step2. 计算特征点速度
  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 {					// ID为-1的特征点是本帧追加提取的特征点,没有速度
        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));
    }
  }
  // step4.6.3. 更新迭代变量,为下一帧计算做准备
  prev_un_pts_map = cur_un_pts_map;
}

step5. 发布前一帧特征点追踪结果

PUB_THIS_FRAME为true时,发布的其实是前一帧(而非最新帧)的特征点追踪结果.

int pub_count = 0;					// 已经发布的帧数
bool PUB_THIS_FRAME;				// 是否发布前一帧特征点

void img_callback(const sensor_msgs::ImageConstPtr &img_msg) {
  // step1~4...

  // step5. 发布前一帧特征点追踪结果
  if (PUB_THIS_FRAME) {
    pub_count++;
   	// step5.1. 将特征点封装成ROS点云格式
    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);
	auto &un_pts = tracker.cur_un_pts;
    auto &cur_pts = tracker.cur_pts;
    auto &ids = tracker.ids;
    auto &pts_velocity = tracker.pts_velocity;
    for (unsigned int j = 0; j < ids.size(); j++) {
      if (tracker.track_cnt[j] > 1) {
        int p_id = ids[j];
        hash_ids[i].insert(p_id);
        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);						// 特征点去畸变后归一化相机平面坐标
        id_of_point.values.push_back(i);							// 特征点ID
        u_of_point.values.push_back(cur_pts[j].x);					// 特征点x坐标
        v_of_point.values.push_back(cur_pts[j].y);					// 特征点y坐标
        velocity_x_of_point.values.push_back(pts_velocity[j].x);	// 特征点x方向速度
        velocity_y_of_point.values.push_back(pts_velocity[j].y);	// 特征点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);

	// step5.2. 起始帧没有速度,不发布
    if (!init_pub) {
      init_pub = 1;
    } else
      pub_img.publish(feature_points);
	
    // step5.3. 构建特征点可视化图
    if (SHOW_TRACK) {
      imagePtr = cv_bridge::cvtColor(imagePtr, sensor_msgs::image_encodings::BGR8);
      cv::Mat show_img = imagePtr->image;
      cv::cvtColor(imagePtr->image, show_img, CV_GRAY2RGB);
      // 越红追踪次数越多,越蓝追踪次数越少
      for (unsigned int j = 0; j < tracker.cur_pts.size(); j++) {
        double len = std::min(1.0, 1.0 * tracker.track_cnt[j] / WINDOW_SIZE);
        cv::circle(show_img, tracker.cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
        
      pub_match.publish(imagePtr->toImageMsg());
    }
  }    
}