VINS-FUSION代码解读【2】——参数读取和求解器参数设置

原创已于 2022-09-05 10:06:02 修改 · 1.5k 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#人工智能 #c++ #计算机视觉

于 2022-08-31 22:15:50 首次发布

c++ 同时被 2 个专栏收录

15 篇文章

订阅专栏

slam代码解读

2 篇文章

订阅专栏

VINS-FUSION代码解读【2】——参数读取

VINS-FUSION代码解读【1】——程序入口

上一篇博客将rosNodeTest.cpp这个程序入口的代码进行了简单的解析。根据顺序我们先来看

    // 读取YAML配置参数
	readParameters(config_file);

读了这个函数我们就可以更清晰的了解到参数文件每个参数的作用


/**
 * 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);
    //上面打开文件却并没有用就关闭了

    //实例化一个FileStorage类对象,以只读的方式打开参数文件
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }
    //读取对应名称的参数，在编写自己的参数文件时可以参照
    //图像的话题
    fsSettings["image0_topic"] >> IMAGE0_TOPIC;
    fsSettings["image1_topic"] >> IMAGE1_TOPIC;
    //特征跟踪中的最大特征数
    MAX_CNT = fsSettings["max_cnt"];
    //两个特征点最小的距离
    MIN_DIST = fsSettings["min_dist"];
    //ransac 阈值 (pixel)
    F_THRESHOLD = fsSettings["F_threshold"];
    //是否将跟踪图像发布为主题
    SHOW_TRACK = fsSettings["show_track"];
    // 是否执行正向和反向光流以提高特征跟踪精度
    FLOW_BACK = fsSettings["flow_back"];
    //是否开启多线程
    MULTIPLE_THREAD = fsSettings["multiple_thread"];
    //是否使用IMU
    USE_IMU = fsSettings["imu"];
    printf("USE_IMU: %d\n", USE_IMU);
    if(USE_IMU)
    {
        //IMU的话题
        fsSettings["imu_topic"] >> IMU_TOPIC;
        printf("IMU_TOPIC: %s\n", IMU_TOPIC.c_str());
        //IMU内参：
        //加速度计测量噪声
        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"];
    }
    //求解器最大迭代时间单位ms
    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";
    std::cout << "result path " << VINS_RESULT_PATH << std::endl;
    std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
    fout.close();
    /**
     * 是否进行相机和IMU在线标定
     * 0：不进行标定有一个准确的外参
     * 1：进行标定，初步猜测外在参数，将围绕最初的外参，对外参进行优化。
     * 2：进行标定，没有外参，完全依靠VINS系统的在线标定
     */
    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());
        TIC.push_back(Eigen::Vector3d::Zero());
        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;
        //外参是对相机坐标系到IMU坐标系的变换矩阵（注意
        fsSettings["body_T_cam0"] >> cv_T;
        Eigen::Matrix4d T;
        cv::cv2eigen(cv_T, T);
        RIC.push_back(T.block<3, 3>(0, 0));
        TIC.push_back(T.block<3, 1>(0, 3));
    } 
    //相机数量，根据相机数量判断使用双目还会四单目
    NUM_OF_CAM = fsSettings["num_of_cam"];
    printf("camera number %d\n", NUM_OF_CAM);

    if(NUM_OF_CAM != 1 && NUM_OF_CAM != 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;
    std::string cam0Path = configPath + "/" + cam0Calib;
    CAM_NAMES.push_back(cam0Path);

    if(NUM_OF_CAM == 2)
    {
        //双目相机标志位置1
        STEREO = 1;
        std::string cam1Calib;
        fsSettings["cam1_calib"] >> cam1Calib;
        std::string cam1Path = configPath + "/" + cam1Calib; 
        //printf("%s cam1 path\n", cam1Path.c_str() );
        CAM_NAMES.push_back(cam1Path);
        
        cv::Mat cv_T;
        //右目变换到左目的坐标变换
        fsSettings["body_T_cam1"] >> cv_T;
        Eigen::Matrix4d T;
        //转成eigen格式
        cv::cv2eigen(cv_T, T);
        RIC.push_back(T.block<3, 3>(0, 0));
        TIC.push_back(T.block<3, 1>(0, 3));
    }
    //初始深度
    INIT_DEPTH = 5.0;
    //加速度计偏置阈值
    BIAS_ACC_THRESHOLD = 0.1;
    //陀螺仪偏置阈值
    BIAS_GYR_THRESHOLD = 0.1;
    //IMU和相机之间传感器时间偏移
    TD = fsSettings["td"];
    //是否在线估计相机和imu之间的时间偏移
    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)
    {
        ESTIMATE_EXTRINSIC = 0;
        ESTIMATE_TD = 0;
        printf("no imu, fix extrinsic param; no time offset calibration\n");
    }
    //释放文件读取类
    fsSettings.release();
}

关于外参可以根据给的代码中的pdf更好地帮助我们去理解。
vins-fusion的外参示例图

estimator.setParameter();

接下来的是estimator的设置参数，主要是设置求解器的参数

/*
 * 为求解器设置参数
*/
void Estimator::setParameter()
{
    mProcess.lock();
    // 外参，body_T_cam
    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;
    }
    //设置Estimator和f_manager中的旋转外参
    f_manager.setRic(ric);
    /**  
     * 
    * 将信息矩阵(海森矩阵，Hessian Matrix)初始化为特征提取噪声 2*2
    * sqrt_info是信息矩阵的sqrt，信息矩阵是协方差矩阵的逆。
    * 设置假定图像特征点提取存在1.5个像素的误差。
    * 港科大实验室在issues中的解释为什么是1.5https://github.com/HKUST-Aerial-Robotics/VINS-Mobile/issues/78
    * 这三个类分别对应求解不同情况
    * 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();
    // 相机和IMU之间的因时钟不一致还有硬件传输导致的时间差
    td = TD;
    //G{0.0, 0.0, 9.8};
    g = G; 
    cout << "set g " << g.transpose() << endl;
    //  读取文件camera内参
    featureTracker.readIntrinsicParameter(CAM_NAMES);

    std::cout << "MULTIPLE_THREAD is " << MULTIPLE_THREAD << '\n';
    //如果允许开启多线程，且在此之前没有开
    if (MULTIPLE_THREAD && !initThreadFlag)
    {
        initThreadFlag = true;
        // 开启优化每一帧对应的图像和IMU的结果的线程
        processThread = std::thread(&Estimator::processMeasurements, this);
    }
    mProcess.unlock();
}