VINS代码逐行解析

VINS代码逐行解析1

1、程序开始首先读取参数：

int main(int argc, char* *argv)
{
    // ./run_eruroc  /home/zlc/VIOFiles/MH_05_difficult/mav0/  ./config/
    sData_path = argv[1];
    sConfig_path = argv[2];
    pSystem.reset(new System(sConfig_path));    // sConfig_path = ./config/

2、下面是开启4大线程：

std::thread the_BackEnd(&System::ProcessBackEnd, pSystem);    // 后端优化线程,  ProcessBackend函数能够启动是因为在创建 System类对象的时候，bStart_backend=true
std::thread thd_PubImuData(PubImuData);                                             // 
std::thread thd_PubImageData(PubImageData);

#ifdef __linux__
std::thread thd_Draw(&System::Draw, pSystem);
#elif __APPLE__
DrawIMGandGLinMainThrd();
#endif

3、程序结束：

    thd_PubImuData.join();
    thd_PubImageData.join();
    // thd_BackEnd.join();
    // thd_Draw.join();
    cout << "main end ... see you ... " << endl;
    return 0;
}

---

一、PSystem对象的创建， System类对象创建

  pSystem.reset(new System(sConfig_path));    // sConfig_path = ./config/

System类构造函数如下：

System::System(string sConfig_file_) : bStart_backend(true) // 开启后端
{
    string sConfig_file = sConfig_file_ + "euroc_config.yaml";
    cout << "1 System() sConfig_file: " << sConfig_file << endl;        // 输出中的第一句打印： 打印配置文件 sConfig_path = ./config/euroc_config.yaml
   
     readParameters(sConfig_file);       // parameters.cpp 文件中的函数

    trackerData[0].readIntrinsicParameter(sConfig_file);

    //readParameters(sConfig_file);

    estimator.setParameter();
}

1、详细看**readParameters(sConfig_file)**函数，在parameter.cpp文件中，主要是读取euroc_config.yaml配置文件中设置的参数。需要格外注意的是，没有使用初始化函数来估计外部参数。

void readParameters(string config_file)
{
    // 1. 读取文件
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    // 2.根据读取文件内容设置参数
    FOCAL_LENGTH = 460;             // 设置焦距
    SOLVER_TIME = fsSettings["max_solver_time"];
    NUM_ITERATIONS = fsSettings["max_num_iterations"];  // 限制最大迭代次数,保证实时性
    MIN_PARALLAX = fsSettings["keyframe_parallax"];     // 最小共视关系(最小视差), 10个像素
    MIN_PARALLAX = MIN_PARALLAX / FOCAL_LENGTH;     // 真实世界最小视差=最小视差/焦距=10.0/460.0
    string OUTPUT_PATH;
    fsSettings["output_path"] >> OUTPUT_PATH;
    VINS_RESULT_PATH = OUTPUT_PATH + "/vins_result_no_loop.txt";

    // IMU 和 相机坐标系下的外部参数变换
    ESTIMATE_EXTRINSIC = fsSettings["estimate_extrinsic"];  // ESTIMATE_EXTRINSIC = 0, 不需要进行参数估计
    if ( ESTIMATE_EXTRINSIC == 2 )
    {   .......   }
    else
    {
        if ( ESTIMATE_EXTRINSIC == 0 )      // 不需要估计camera和IMU之间的外部旋转和平移参数
        {
            cout << "fix extrinsic param " << endl;
        }
        // Extrinsic parameter between IMU and camera 相机和IMU之间的变换, 旋转矩阵和平移矩阵
        cv::Mat cv_R, cv_T;
        fsSettings["extrinsicRotation"] >> cv_R;
        fsSettings["extrinsicTranslation"] >> cv_T;
        Eigen::Matrix3d eigen_R;        Eigen::Vector3d eigen_T;
        cv::cv2eigen(cv_R, eigen_R);    cv::cv2eigen(cv_T, eigen_T);
        Eigen::Quaterniond Q(eigen_R);  // 转换为四元数
        eigen_R = Q.normalized();
        RIC.push_back(eigen_R);         // 确定IMU与camera的旋转矩阵
        TIC.push_back(eigen_T);         // 确定IMU与camera的平移矩阵
    }

    INIT_DEPTH = 5.0;
    BIAS_ACC_THRESHOLD = 0.1;
    BIAS_GYR_THRESHOLD = 0.1;

    // 设置td 和 tr
    TD = fsSettings["td"];   // initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) 读图像时间 + td = 真实图像时间
    ESTIMATE_TD = fsSettings["estimate_td"];    // 相机和IMU同步校准得到的时间差

    // 是否为卷帘门相机
    ROLLING_SHUTTER = fsSettings["rolling_shutter"];   // 0: global shutter camera, 1: rolling shutter camera
    else
    {   TR = 0;     // 全局相机, 这里tr = 0   }

    fsSettings["image_topic"] >> IMAGE_TOPIC;
    fsSettings["imu_topic"] >> IMU_TOPIC;   // 字符流, 所以用 ">>"
    MAX_CNT  = fsSettings["max_cnt"];       // 特征跟踪中最大特征点数
    MIN_DIST = fsSettings["min_dist"];      // 两个特征之间的最小距离

    FREQ = fsSettings["freq"];              // 控制发布次数的频率
    F_THRESHOLD = fsSettings["F_threshold"];
    
    SHOW_TRACK = fsSettings["show_track"];  // =1, publish tracking image as topic

    EQUALIZE = fsSettings["equalize"];      // =1, if image is too dark or light, trun on equalize to find enough features
    FISHEYE = fsSettings["fisheye"];            // =0, 不是鱼眼镜头

    CAM_NAMES.push_back(config_file);       // // 相机参数配置文件名

    STEREO_TRACK = false;
    PUB_THIS_FRAME = false;

    if ( FREQ == 0 )
    {
        FREQ = 10;
    }
    fsSettings.release();

    // 3.打印刚设置的参数
    cout << "1 readParameters: "
              ... ... << endl;
}

2、System类构造函数中的trackerData[0].readIntrinsicParameter(sConfig_file);

/* 读取相机标定的内参 */
void FeatureTracker::readIntrinsicParameter(const string& calib_file)
{
    cout << "read parameter of camera " << calib_file << endl;      // calib_file = sConfig_path = ./config/euroc_config.yaml
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);      //  见2、1节和2、2节解析 
}

2、1 CameraFactory::instance()创建不同的相机类型和校准模型实例，注意，这里是为了读取相机参数，肯定与校准相关，所以这里调用这个CameraFactory类。

CameraFactory::instance(void)       //  不同的相机类型和校准模型
{                                                                      //  重新创建相机实例
    if (m_instance.get() == 0)
        m_instance.reset(new CameraFactory);
    return m_instance;
}

2、2 真正通过generateCameraFromYamlFile()函数开始读取内参：

CameraPtr CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);        // 文件操作

    Camera::ModelType modelType = Camera::MEI;   // 默认为MEI模型, 后面会赋值为针孔模型
    if (!fs["model_type"].isNone())
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;     // euroc_config.yaml配置文件中： model_type: PINHOLE
        if (boost::iequals(sModelType, "kannala_brandt"))
        else if (boost::iequals(sModelType, "scaramuzza"))
        else if (boost::iequals(sModelType, "pinhole"))
            modelType = Camera::PINHOLE;
    }
    switch (modelType)
    {
        case Camera::KANNALA_BRANDT:
        {   }
        case Camera::PINHOLE:
        {
            PinholeCameraPtr camera(new PinholeCamera);    // 创建PinholeCamera相机对象
            PinholeCamera::Parameters params = camera->getParameters();   // 获得参数读取指针, 具体函数见后面
            params.readFromYamlFile(filename);       // 读取相机内参K矩阵和投影参数 函数
            camera->setParameters(params);               // 设置相机参数
            return camera;
        }
        case Camera::SCARAMUZZA:
        {   }
        case Camera::MEI:
        default:
        {   }
    }
    return CameraPtr();
}

2.2.1 获得参数读取指针：

const PinholeCamera::Parameters& PinholeCamera::getParameters(void) const   // 用到这个函数返回 参数引用
{
    return mParameters;
}

2.2.2 其中的getParameters函数如下，在PinholeCamera.cpp函数中：

bool PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)   // 从配置文件euroc_config.yaml中读取畸变参数矩阵和相机内参K矩阵
{
        cv::FileStorage fs(filename, cv::FileStorage::READ);
        m_modelType = PINHOLE;
        fs["camera_name"] >> m_cameraName;      // euroc_config.yaml 配置文件中：  相机名称： camera_name: camera
        m_imageWidth = static_cast<int>(fs["image_width"]);     // image_width:  752  列宽
        m_imageHeight = static_cast<int>(fs["image_height"]);   // image_height: 480  行高
        cv::FileNode n = fs["distortion_parameters"];
        m_k1 = static_cast<double>(n["k1"]);
        ... ...
        m_p2 = static_cast<double>(n["p2"]);
        n = fs["projection_parameters"];
        m_fx = static_cast<double>(n["fx"]);
        ... ...
        m_cy = static_cast<double>(n["cy"]);
        std::cout << "1 PinholeCamera "
            << "\n  m_cameraName: " << m_cameraName
            ... ...<< std::endl << std::endl;
        return true;
}

2.2.3 camera->setParameters(params); 设置相机参数函数如下：

void PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)
{
        mParameters = parameters;
        if ((mParameters.k1() == 0.0) &&   (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) &&  (mParameters.p2() == 0.0))    // 畸变参数为0, 说明没有畸变矫正
            m_noDistortion = true;
        else
            m_noDistortion = false;
            
            m_inv_K11 = 1.0 / mParameters.fx();         // 设置内参矩阵的逆,  在畸变投影模型中会用到
            m_inv_K13 = -mParameters.cx() / mParameters.fx();
            m_inv_K22 = 1.0 / mParameters.fy();
            m_inv_K23 = -mParameters.cy() / mParameters.fy();
}

3、System类构造函数中的estimator.setParameter();，开始设置相机和IMU之间的外部变换参数：

void Estimator::setParameter()
{
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            tic[i] = TIC[i];
            ric[i] = RIC[i];
        }
        cout << "1 Estimator::setParameter FOCAL_LENGTH: " << FOCAL_LENGTH << endl;
        f_manager.setRic(ric);    // 只设置相机与IMU之间的旋转参数
        project_sqrt_info_ = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
        td = TD;   // 相机和IMU同步校准得到的时间差   △△△△△   ☆☆☆☆☆
}