从零学习VINS-Mono/Fusion源代码（一）：主函数

从本文开始，从头阅读VINS源代码。
本节主要阅读feature_tracker_node.cpp中的主函数main()，代码实现了feature_tracker节点初始化，配置文件yaml参数读取，topic订阅与发布。
本文只探讨针孔模型。

VINS-Mono/Fusion代码学习系列：
从零学习VINS-Mono/Fusion源代码（一）：主函数
从零学习VINS-Mono/Fusion源代码（二）：前端图像跟踪
从零学习VINS-Mono/Fusion源代码（三）：IMU预积分公式推导
从零学习VINS-Mono/Fusion源代码（四）：误差卡尔曼滤波
从零学习VINS-Mono/Fusion源代码（五）：VIO初始化
从零学习VINS-Mono/Fusion源代码（六）：后端优化

1 系统节点结构

vins-mono源码：https://github.com/HKUST-Aerial-Robotics/VINS-Mono

首先，利用rqt_graph查看ros下系统整体节点图，给出了各个topic的发布和订阅关系。

2 主函数main()

主函数的部分主要是搭建一个ros框架，定义节点、订阅者、发布者，读取配置文件参数，通过回调函数对订阅到的图像信息进行处理。

其整体流程如下图所示：

int main(int argc, char **argv)
{
    ros::init(argc, argv, "feature_tracker");//节点名称
    ros::NodeHandle n("~");//n句柄
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    //读取配置参数
    readParameters(n);
	
	//在VINS-Mono中 NUM_OF_CAM=1，只有单目
    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读相机内参

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

	//订阅图像，回调函数img_callback
    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);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();
    return 0;
}

3 配置参数读取

• readParameters(n) 获取文件路径、topic、各类门限阈值

void readParameters(ros::NodeHandle &n)
{
    std::string config_file;
    config_file = readParam<std::string>(n, "config_file");//文件路径
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }
    std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");

    fsSettings["image_topic"] >> IMAGE_TOPIC;
    fsSettings["imu_topic"] >> IMU_TOPIC;
    MAX_CNT = fsSettings["max_cnt"];//每一帧图像提取到的最大特征点数量
    MIN_DIST = fsSettings["min_dist"];//特征点间最小距离（主要是为了使特征点分布均匀）
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    FREQ = fsSettings["freq"];//跟踪结果发布给后端的频率
    F_THRESHOLD = fsSettings["F_threshold"];//ransac门限阈值
    SHOW_TRACK = fsSettings["show_track"];
    EQUALIZE = fsSettings["equalize"];//图像均衡化
    FISHEYE = fsSettings["fisheye"];
    if (FISHEYE == 1)
        FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";
    CAM_NAMES.push_back(config_file);

    WINDOW_SIZE = 20;
    STEREO_TRACK = false;
    FOCAL_LENGTH = 460;//设定死的虚拟相机焦距
    PUB_THIS_FRAME = false;

    if (FREQ == 0)
        FREQ = 100;

    fsSettings.release();
}

• trackerData[i].readIntrinsicParameter(CAM_NAMES[i]) 依次读取各个相机内参

void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

CameraFactory::instance()->generateCameraFromYamlFile(calib_file)中

//generateCameraFromYamlFile中确定相机类型并构建模型
case Camera::PINHOLE:
    {
        PinholeCameraPtr camera(new PinholeCamera);

        PinholeCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);//读取yaml文件
        camera->setParameters(params);//把params赋值给mParameters
        return camera;
    }

params.readFromYamlFile(filename)中

bool
PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);

    if (!fs.isOpened())
    {
        return false;
    }

    if (!fs["model_type"].isNone())
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;

        if (sModelType.compare("PINHOLE") != 0)
        {
            return false;
        }
    }

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