VINS-Mono源码分析1— feature_tracker(视觉前端)

ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

feature_tracker包的程序就是从这里开始一步一步往下执行的，这里是程序的入口。基本上所有ROS程序开始都有的东西。

readParameters(n);

它的功能就是读取参数文件的参数，默认情况下参数文件是～/VINS-Mono/config/euroc文件夹下的euroc_config.yaml文件。

std::string config_file;
config_file = readParam<std::string>(n, "config_file");

这段代码在parameters.cpp的readParameters(ros::NodeHandle &n) 函数下，readParam默认读取的对象是～/VINS-Mono/vins_estimator/launch文件夹下的euroc.launch文件的内容

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

读取相机参数，这部分具体读取参数的代码在camera_model包下的PinholeCamera.cc文件中，如下所示

PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)
{
   
    cv::FileStorage fs(filename, cv::FileStorage::READ);
    [此处略去部分代码]
    m_modelType = PINHOLE;
    fs["camera_name"] >> m_cameraName;
    m_imageWidth = static_cast<int>(fs["image_width"]);
    m_imageHeight = static_cast<int>(fs["image_height"]);

    cv::FileNode n = fs["distortion_parameters"];
    m_k1 = static_cast<double>(n["k1"]);
    m_k2 = static_cast<double>(n["k2"]);
    m_p1 = static_cast<double>(n["p1"]);
    m_p2 = static_cast<double>(n["p2"]);

    n = fs["projection_parameters"];
    m_fx = static_cast<double>(n["fx"]);
    m_fy = static_cast<double>(n["fy"]);
    m_cx = static_cast<double>(n["cx"]);
    m_cy = static_cast<double>(n["cy"]);

    return true;
}

上面的代码就是读取相机参数的详情。

if(FISHEYE)
    {
   
        ......
    }

默认针孔相机模型,这部分代码不执行，故不做解释。

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

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::spin();

订阅视觉话题IMAGE_TOPIC的消息，默认情况下它代表着一些特定的.bag文件发布的话题消息，比如MH_05_difficult.bag文件发布的话题"/cam0/image_raw"。定义pub_img、pub_match、pub_restart话题。
接下来执行回调函数img_callback()

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

如果是第一帧图像则执行上面代码，得到时间戳。不是第一帧图像不执行。

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;
    std_msgs::Bool restart_flag;
    restart_flag.data = true;
    pub_restart.publish(restart_flag);
    return;
}

判断图像帧在时间上是否连续，如果连续，则跳过此段代码，继续往下执行。如果不连续，则重新初始化。

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
   
    PUB_THIS_FRAME = true;
    // reset the frequency control
    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;

发布图像帧的频率控制，默认情况下发布频率为10，如果当前帧的发布频率小于10且非常接近10，则将当前帧的时间戳设为频率控制的起始时间。

cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding