VINS-Mono 代码详细解读——视觉跟踪 feature_trackers

目录

原理

代码框架

一、feature_tracker_node.cpp

1、程序入口 int main()

sensor_msgs::PointCloud 点云

sensor_msgs::PointCloudPtr  feature_points(new sensor_msgs::PointCloud)

sensor_msgs::Image

sensor_msgs::ImuConstPtr

2、回调函数 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

二、feature_tracker.cpp

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)

inBorder（）函数

reduceVector（）函数

rejectWithF() 函数

setMask() 函数

goodFeaturesToTrack（）函数再检测出新角点

addPoints() 函数

undistortedPoints() 函数

showUndistortion（）函数

---

原理

视觉：

1）提取Harris角点，KLT金字塔光流跟踪相邻帧；

2）2 维特征点先矫正为不失真的，然后在通过外点剔除后投影到一个单位球面上  ；

3）去除异常点：先进行F矩阵测试，通过RANSAC去除异常点；

4）关键帧选取：1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）2、当前帧跟踪到的特征点数量小于阈值视为关键帧；

代码框架

代码流程如下图：主要三个源程序，feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。

 

一、feature_tracker_node.cpp

主要分为两部分：int main（）函数为程序入口，void img_callback（）为ROS的回调函数，对图像进行特征点追踪，处理和发布。

1、程序入口 int main()

int main(int argc, char **argv)
{
    //ros初始化和设置句柄
    ros::init(argc, argv, "feature_tracker");
    ros::NodeHandle n("~");// 命名空间为/node_namespace/node_name

    //设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    
    // 1.读取yaml中的一些配置参数
    readParameters(n);

    // 2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
    for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

    // 3.判断是否加入鱼眼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");
        }
    }

    // 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时，执行回调函数img_callback
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

    // 5.发布feature点云，实例feature_points，跟踪的特征点，给后端优化用
    pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
    //发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用
    pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
    //发布restart
    pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();
    return 0;
}

1、其中，步骤1中读取配置参数，地址为config->euroc->euroc_config.yaml

2、步骤5中，三个发布的话题为下图所示，

feature_trackers是单独的一个模块，输入IMAGE_TOPIC(/cam0/image_raw)，输出三个话题topic：feature_img（跟踪的图像，之后RVIZ显示）、feature（跟踪的特征点信息，由/vins_estimator订阅并优化）、restart（特征跟踪出错，复位）

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

三种ROS数据结构：sensor_msgs::PointCloud、sensor_msgs::Image、std_msgs::Bool

sensor_msgs::PointCloud 点云

sensor_msgs::PointCloudPtr  feature_points(new sensor_msgs::PointCloud)

std_msgs/Header header #头信息
	#feature_points->header = img_msg->header;
	#feature_points->header.frame_id = "world";

geometry_msgs/Point32[] points #3D点(x,y,z)

sensor_msgs/ChannelFloat32[] channels  #[特征点的ID,像素坐标u,v,速度vx,vy]
	#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);
	#如img_msg->channels[0].values[i]表示第i个特征点的ID值

std_msgs/Header header #头信息
	#msg_match_points.header.stamp = ros::Time(time_stamp);

geometry_msgs/Point32[] points #3D点(x,y,z)

sensor_msgs/ChannelFloat32[] channels  #[重定位帧的平移向量T的x,y,z，旋转四元数w,x,y,z和索引值]
	#t_q_index.values.push_back(T.x());
	#t_q_index.values.push_back(T.y());
	#t_q_index.values.push_back(T.z());
	#t_q_index.values.push_back(Q.w());
	#t_q_index.values.push_back(Q.x());
	#t_q_index.values.push_back(Q.y());
	#t_q_index.values.push_back(Q.z());
	#t_q_index.values.push_back(index);
	#msg_match_points.channels.push_back(t_q_index);

sensor_msgs::Image

std_msgs/Header header #头信息
uint32 height         # image height, number of rows
uint32 width          # image width, number of columns
string encoding       # Encoding of pixels -- channel meaning, ordering, size
                      # taken from the list of strings in include/sensor_msgs/image_encodings.h
uint8 is_bigendian    #大端big endian(从低地址到高地址的顺序存放数据的高位字节到低位字节)和小端little endian
uint32 step           # Full row length in bytes
uint8[] data          # actual matrix data, size is (step * rows)

sensor_msgs::ImuConstPtr

Header header	#头信息

geometry_msgs/Quaternion orientation	#四元数[x,y,z,w]
float64[9] orientation_covariance		# Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity	#角速度[x,y,z]轴
float64[9] angular_velocity_covariance	# 对应协方差矩阵，Row major(行主序) about x, y, z axes

geometry_msgs/Vector3 linear_acceleration	#线性加速度[x,y,z]
float64[9] linear_acceleration_covariance	# 对应协方差矩阵 Row major x, y z 

2、回调函数 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。

pub_img.publish(feature_points);

pub_match.publish(ptr->toImageMsg())

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    // 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
    if (img_msg->header.stamp.toSec() - las