vins 解读_VINS-MONO理解与改进（二）

estimator.cpp解读

代码共计1288行，今天先看前一段

欢迎讨论，有误斧正

共有函数如下：

estimator.cpp是核心函数，串起了前端后端，并且体现了VINS的许多核心思想

具体函数代码解读&备注

1、void Estimator::setParameter()

用途：为求解器设置相机和imu之间的外参

主要思路：将parameter.cpp中的参数引入Estimator类中

void Estimator::setParameter()//设置相机和imu之间的外参
{
    
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
    
        tic[i] = TIC[i];
        ric[i] = RIC[i];//初始的相机和imu的外参
        
    }
    cout << "1 Estimator::setParameter FOCAL_LENGTH: " << FOCAL_LENGTH << endl;
    f_manager.setRic(ric);//设置Estimator和f_manager中的ric
    project_sqrt_info_ = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();//投影信息平方
    td = TD;
}

2、void Estimator::clearState()

用途：将当前求解器的参数重置

主要思路：清道夫

void Estimator::clearState()//清除状态
{
    
    for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
    
        Rs[i].setIdentity();
        Ps[i].setZero();
        Vs[i].setZero();
        Bas[i].setZero();
        Bgs[i].setZero();
        dt_buf[i].clear();
        linear_acceleration_buf[i].clear();
        angular_velocity_buf[i].clear();

        if (pre_integrations[i] != nullptr)
            delete pre_integrations[i];
        pre_integrations[i] = nullptr;
    }

    for (int i = 0; i < NUM_OF_CAM; i++)
    {
    
        tic[i] = Vector3d::Zero();
        ric[i] = Matrix3d::Identity();
    }

    for (auto &it : all_image_frame)
    {
    
        if (it.second.pre_integration != nullptr)
        {
    
            delete it.second.pre_integration;
            it.second.pre_integration = nullptr;
        }
    }

    solver_flag = INITIAL;
    first_imu = false,
    sum_of_back = 0;
    sum_of_front = 0;
    frame_count = 0;
    solver_flag = INITIAL;
    initial_timestamp = 0;
    all_image_frame.clear();
    td = TD;

    if (tmp_pre_integration != nullptr)
        delete tmp_pre_integration;
    
    tmp_pre_integration = nullptr;
    
    last_marginalization_parameter_blocks.clear();

    f_manager.clearState();

    failure_occur = 0;
    relocalization_info = 0;

    drift_correct_r = Matrix3d::Identity();
    drift_correct_t = Vector3d::Zero();
}

3、void Estimator::processIMU()

用途：计算预积分数据，并更新PVQ

主要思路：利用当前数据和之前的数据使用中值积分计算预积分，并更新PVQ数据

涉及变量：

double dt:时间间隔

const Vector3d &linear_acceleration：当前加速度

const Vector3d &angular_velocity：当前角速度

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    
    if (!first_imu)//first_imu为false表示当前上报的imu数据为第一个imu数据
    {
    
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    if (!pre_integrations[frame_count])//创建预积分对象，数据来自上次处理IMU的acc_0
    {
    //存疑，此处计算的预积分是0～frame_count的还是仅仅是frame——count的？
        pre_integrations[frame_count] = new IntegrationBase{
    acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    if (frame_count != 0)//当滑动窗口内有图像帧数据时，进行预积分 frame_count表示滑动窗口中图像数据的个数
    {
    
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;
        //Rs应当是从上一个时刻的imu坐标系到世界坐标系
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//此处的Rs是上一时刻的，acc_0不是初始数据情况下，亦是
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5