VINS-Fusion源码逐行解析：readParameters函数与setParameter函数

readParameters函数是读取参数配置文件也就是我们提供的yaml文件的函数

//读取参数配置文件
void readParameters(std::string config_file)
{
    FILE *fh = fopen(config_file.c_str(),"r");// 尝试打开配置文件
    if(fh == NULL){// 如果文件打开失败
        ROS_WARN("config_file dosen't exist; wrong config_file path");// 输出警告信息
        ROS_BREAK();// 终止程序
        return;          
    }
    fclose(fh);// 关闭文件

    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);// 使用OpenCV的FileStorage打开配置文件
    if(!fsSettings.isOpened())// 如果配置文件打开失败
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;// 输出错误信息
    }

    fsSettings["image0_topic"] >> IMAGE0_TOPIC;// 读取图像0的主题名
    fsSettings["image1_topic"] >> IMAGE1_TOPIC;// 读取图像1的主题名
    MAX_CNT = fsSettings["max_cnt"];// 读取最大特征点数
    MIN_DIST = fsSettings["min_dist"]; // 读取特征点最小距离
    F_THRESHOLD = fsSettings["F_threshold"];// 读取基本矩阵的阈值
    SHOW_TRACK = fsSettings["show_track"];// 读取是否显示跟踪
    FLOW_BACK = fsSettings["flow_back"];// 读取是否进行反向光流跟踪

    MULTIPLE_THREAD = fsSettings["multiple_thread"];// 读取是否使用多线程

    USE_IMU = fsSettings["imu"];// 读取是否使用IMU
    printf("USE_IMU: %d\n", USE_IMU);
    if(USE_IMU)// 如果使用IMU
    {
        fsSettings["imu_topic"] >> IMU_TOPIC;// 读取IMU主题名
        printf("IMU_TOPIC: %s\n", IMU_TOPIC.c_str());
        ACC_N = fsSettings["acc_n"];// 读取加速度计噪声
        ACC_W = fsSettings["acc_w"];// 读取加速度计偏差噪声
        GYR_N = fsSettings["gyr_n"];// 读取陀螺仪噪声
        GYR_W = fsSettings["gyr_w"];// 读取陀螺仪偏差噪声
        G.z() = fsSettings["g_norm"]; // 读取重力加速度
    }

    SOLVER_TIME = fsSettings["max_solver_time"];// 读取最大求解时间
    NUM_ITERATIONS = fsSettings["max_num_iterations"];// 读取最大迭代次数
    MIN_PARALLAX = fsSettings["keyframe_parallax"];// 读取关键帧视差
    MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;// 计算最小视差

    fsSettings["output_path"] >> OUTPUT_FOLDER;// 读取输出路径
    VINS_RESULT_PATH = OUTPUT_FOLDER + "/vio.csv";// 设置VIO结果文件路径
    std::cout << "result path " << VINS_RESULT_PATH << std::endl;
    std::ofstream fout(VINS_RESULT_PATH, std::ios::out);// 创建结果文件
    fout.close();

    ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"];// 读取是否估计外参
    if (ESTIMATE_EXTRINSIC == 2)// 如果没有先验的外参
    {
        ROS_WARN("have no prior about extrinsic param, calibrate extrinsic param");// 输出警告信息
        RIC.push_back(Eigen::Matrix3d::Identity());// 将单位矩阵添加到RIC中
        TIC.push_back(Eigen::Vector3d::Zero());// 将零向量添加到TIC中
        EX_CALIB_RESULT_PATH = OUTPUT_FOLDER + "/extrinsic_parameter.csv";// 设置外参结果文件路径
    }
    else 
    {
        if ( ESTIMATE_EXTRINSIC == 1)// 如果优化外参
        {
            ROS_WARN(" Optimize extrinsic param around initial guess!");// 输出警告信息
            EX_CALIB_RESULT_PATH = OUTPUT_FOLDER + "/extrinsic_parameter.csv";// 设置外参结果文件路径
        }
        if (ESTIMATE_EXTRINSIC == 0)// 如果固定外参
            ROS_WARN(" fix extrinsic param ");// 输出警告信息

        cv::Mat cv_T;
        fsSettings["body_T_cam0"] >> cv_T;// 读取相机0到机体的变换矩阵
        Eigen::Matrix4d T;
        cv::cv2eigen(cv_T, T);// 将OpenCV矩阵转换为Eigen矩阵
        RIC.push_back(T.block<3, 3>(0, 0));// 提取旋转矩阵并添加到RIC中
        TIC.push_back(T.block<3, 1>(0, 3));// 提取平移向量并添加到TIC中
    } 
    
    NUM_OF_CAM = fsSettings["num_of_cam"];// 读取相机数量
    printf("camera number %d\n", NUM_OF_CAM);

    if(NUM_OF_CAM != 1 && NUM_OF_CAM != 2)// 如果相机数量不是1或2
    {
        printf("num_of_cam should be 1 or 2\n");// 输出错误信息
        assert(0);// 断言失败，终止程序
    }


    int pn = config_file.find_last_of('/');// 找到配置文件路径中的最后一个斜杠位置
    std::string configPath = config_file.substr(0, pn);// 提取配置文件路径
    
    std::string cam0Calib;
    fsSettings["cam0_calib"] >> cam0Calib;// 读取相机0的校准文件名
    std::string cam0Path = configPath + "/" + cam0Calib;// 构建相机0的校准文件路径
    CAM_NAMES.push_back(cam0Path);// 将相机0的校准文件路径添加到CAM_NAMES中

    if(NUM_OF_CAM == 2)// 如果有两个相机
    {
        STEREO = 1;// 设置为立体相机模式
        std::string cam1Calib;
        fsSettings["cam1_calib"] >> cam1Calib;// 读取相机1的校准文件名
        std::string cam1Path = configPath + "/" + cam1Calib; // 构建相机1的校准文件路径
        //printf("%s cam1 path\n", cam1Path.c_str() );
        CAM_NAMES.push_back(cam1Path);// 将相机1的校准文件路径添加到CAM_NAMES中
        
        cv::Mat cv_T;
        fsSettings["body_T_cam1"] >> cv_T;// 读取相机1到机体的变换矩阵
        Eigen::Matrix4d T;
        cv::cv2eigen(cv_T, T);// 将OpenCV矩阵转换为Eigen矩阵
        RIC.push_back(T.block<3, 3>(0, 0)); // 提取旋转矩阵并添加到RIC中
        TIC.push_back(T.block<3, 1>(0, 3));// 提取平移向量并添加到TIC中
    }

    INIT_DEPTH = 5.0;// 初始化深度
    BIAS_ACC_THRESHOLD = 0.1;// 加速度计偏差阈值
    BIAS_GYR_THRESHOLD = 0.1;// 陀螺仪偏差阈值

    TD = fsSettings["td"];// 读取时间延迟
    ESTIMATE_TD = fsSettings["estimate_td"];// 读取是否估计时间延迟
    if (ESTIMATE_TD)
        ROS_INFO_STREAM("Unsynchronized sensors, online estimate time offset, initial td: " << TD);// 输出时间延迟信息
    else
        ROS_INFO_STREAM("Synchronized sensors, fix time offset: " << TD);// 输出固定时间延迟信息

    ROW = fsSettings["image_height"];// 读取图像高度
    COL = fsSettings["image_width"]; // 读取图像宽度
    ROS_INFO("ROW: %d COL: %d ", ROW, COL);// 输出图像尺寸信息

    if(!USE_IMU)// 如果不使用IMU
    {
        ESTIMATE_EXTRINSIC = 0;// 固定外参
        ESTIMATE_TD = 0;// 固定时间延迟
        printf("no imu, fix extrinsic param; no time offset calibration\n");// 输出信息
    }

    fsSettings.release();// 释放文件资源
}

通过此函数，我们知道yaml文件中各个参数的作用

setParameter函数的作用是设置估计器的参数

//设置估计器的参数
void Estimator::setParameter()
{
    mProcess.lock();// 加锁，保证线程安全
    for (int i = 0; i < NUM_OF_CAM; i++)// 遍历所有相机
    {
        tic[i] = TIC[i];// 设置相机的平移外参
        ric[i] = RIC[i];// 设置相机的旋转外参
        cout << " exitrinsic cam " << i << endl  << ric[i] << endl << tic[i].transpose() << endl;// 输出相机外参信息
    }
    f_manager.setRic(ric);// 设置特征管理器的旋转外参
    // 设置投影因子的平方根信息矩阵，用于归一化误差项
    //sqrt_info是信息矩阵的sqrt，平方根信息矩阵用于将残差标准化，使得不同尺度的误差项可以在同一优化过程中处理。
    //FOCAL_LENGTH 是焦距，Matrix2d::Identity() 是 2x2 的单位矩阵
    //分别对应求解不同情况
    //ProjectionTwoFrameOneCamFactor：单目相机两帧之间的求解
    //ProjectionTwoFrameTwoCamFactor：双目相机两帧之间的求解
    //ProjectionOneFrameTwoCamFactor：双目相机的第一帧的求解
    ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;// 设置时间延迟
    g = G;// 设置重力加速度
    cout << "set g " << g.transpose() << endl;// 输出重力加速度信息
    featureTracker.readIntrinsicParameter(CAM_NAMES);// 读取相机的内参

    std::cout << "MULTIPLE_THREAD is " << MULTIPLE_THREAD << '\n';// 输出是否使用多线程的信息
    if (MULTIPLE_THREAD && !initThreadFlag)// 如果使用多线程且初始化线程标志为假
    {
        initThreadFlag = true;// 设置初始化线程标志为真
        processThread = std::thread(&Estimator::processMeasurements, this);// 创建并启动处理测量数据的线程
    }
    mProcess.unlock();// 解锁
}

该函数非常重要，当执行时，开启了一个Estimator类内的新线程：processMeasurements;此函数才是真正对数据进行处理的线程