[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——thd_BackEnd线程(内容|代码)

前面的两个线程thd_PubImuData、thd_PubImageData主要是获取IMU数据、获取图像数据以及进行特征提取和LK光流的跟踪。thd_BackEnd线程包括IMU预积分Estimator::processIMU和图像数据的进一步操作，其中包括初始化的一系列操作在Estimator::processImage。初始化完成后将进入后端进行滑动窗口的优化，进而得到优化后的状态估计结果。

run_euroc.cpp中的最后一个线程：

std::thread thd_BackEnd(&System::ProcessBackEnd, pSystem);

后端优化函数在system.cpp：

System::ProcessBackEnd();

1. 获取观测数据IMU&image

1.首先获取观测数据，包括图像特征，IMU数据，获取数据的时候使用的是互斥锁。

// thread: visual-inertial odometry
void System::ProcessBackEnd()
{    
    cout << "1 ProcessBackEnd start" << endl;    
    while (bStart_backend)    
    {        
        // cout << "1 process()" << endl;        
        vector<pair<vector<ImuConstPtr>, ImgConstPtr>> measurements; // 测量值是一些对IMU&IMG                
        unique_lock<mutex> lk(m_buf); // 没有加锁的my_mutex        
        con.wait(lk, [&] { // 等待图像数据和IMU数据是准备好的            
            return (measurements = getMeasurements()).size() != 0; // getMeasurements()获取测量值函数        
        });        
        if( measurements.size() > 1)
        {        
            cout << "1 getMeasurements size: " << measurements.size()             
                 << " imu sizes: " << measurements[0].first.size()            
                 << " feature_buf size: " <<  feature_buf.size() // 图像            
                 << " imu_buf size: " << imu_buf.size() << endl; // IMU        
         }
         ...
}

主要的功能函数System::getMeasurements()，代码如下：

vector<pair<vector<ImuConstPtr>, ImgConstPtr>> System::getMeasurements()
{    
    vector<pair<vector<ImuConstPtr>, ImgConstPtr>> measurements;
    while (true)    
    {        
        if (imu_buf.empty() || feature_buf.empty())        
        {            
            // cerr << "1 imu_buf.empty() || feature_buf.empty()" << endl;            
            return measurements;        
        }
        if (!(imu_buf.back()->header > feature_buf.front()->header + estimator.td)) // header指向时间戳，td:时间        
        {            
            cerr << "wait for imu, only should happen at the beginning sum_of_wait: "                 
                 << sum_of_wait << endl;            
            sum_of_wait++;            
            return measurements;        
        }
        if (!(imu_buf.front()->header < feature_buf.front()->header + estimator.td))        
        {            
            cerr << "throw img, only should happen at the beginning" << endl;            
            feature_buf.pop(); //删除队列首元素但不返回其值            
            continue;        
        }        
        // img_msg相当于第k帧        
        ImgConstPtr img_msg = feature_buf.front(); // 返回队首元素的值，但不删除该元素        
        feature_buf.pop();
        
        vector<ImuConstPtr> IMUs;        
        // 如果IMU的数据是在第k帧和第k+1帧之间，时间间隔为td        
        // 则从imu_buf中获取IMU的数据给IMUS        
        while (imu_buf.front()->header < img_msg->header + estimator.td)        
        {            
            IMUs.emplace_back(imu_buf.front());            
            imu_buf.pop();        
        }        
        // cout << "1 getMeasurements IMUs size: " << IMUs.size() << endl;        
        IMUs.emplace_back(imu_buf.front());        
        if (IMUs.empty())
        {            
            cerr << "no imu between two image" << endl;        
        }        
        // cout << "1 getMeasurements img t: " << fixed << img_msg->header        
        //     << " imu begin: "<< IMUs.front()->header         
        //     << " end: " << IMUs.back()->header        
        //     << endl;        
        // 按时间戳获取IMU和图像的数据        
        measurements.emplace_back(IMUs, img_msg);    
    }    
    return measurements;
}

2. 处理IMU数据

2.然后对于每个观测，如果IMU时间戳$t$在相邻两帧的时间戳之间，将当前时间设置为$t$,直接读取角速度和线加速度，然后执行estimator.processIMU()去处理IMU数据:

lk.unlock();        
m_estimator.lock(); // 卡时间戳 相同时间戳数据放一起        
for (auto &measurement : measurements) // 遍历数据        
{            
    auto img_msg = measurement.second; // 图像帧            
    double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0; // 初始化           
    for (auto &imu_msg : measurement.first)            
    {                
        double t = imu_msg->header; // imu的时间戳                
        double img_t = img_msg->header + estimator.td; // 第k+1帧的时间戳                
        if (t <= img_t) // 如果IMU的时间戳在两帧k\k+1之间                
        {                    
            if (current_time < 0)                        
                current_time = t;                     
            double dt = t - current_time;                    
            assert(dt >= 0);                    
            current_time = t; // 上一个IMU数据的时间戳
                                
            dx = imu_msg->linear_acceleration.x(); // a                    
            dy = imu_msg->linear_acceleration.y();                    
            dz = imu_msg->linear_acceleration.z();                    
            rx = imu_msg->angular_velocity.x(); //w                    
            ry = imu_msg->angular_velocity.y();                    
            rz = imu_msg->angular_velocity.z();
                                
            estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); // 处理IMU数据                    
            // printf("1 BackEnd imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);                
        }

3.如果imu的时间戳$t$大于第k+1帧的图像时间戳img_t
对于处于边界位置的IMU数据，是被相邻两帧共享的，而且对前一帧的影响会大一些。也就是说稍大于第k+1相机帧时间戳的IMU数据仍然要算到第k到k+1相机帧之间，则IMU预积分需要用到的时间间隔应该用k+1相机帧img_t的时间戳减去上一个imu的时间戳current_time来计算。

在这里，对数据线性分配。利用先前current_time到img_t以及img_t到t的时间差设置权重，然后将加权后的IMU数据用于后面estimator.processIMU()处理。具体代码如下：

// t大于第k+1帧的图像时间戳`img_t`
else                
{                    
    double dt_1 = img_t - current_time; // current_time < img_time < t                   
    double dt_2 = t - img_t;                     
    current_time = img_t;                    
    assert(dt_1 >= 0);                    
    assert(dt_2 >= 0);                    
    assert(dt_1 + dt_2 > 0);                    
    double w1 = dt_2 / (dt_1 + dt_2); // 利用时间差设置权重                    
    double w2 = dt_1 / (dt_1 + dt_2);                    
    dx = w1 * dx + w2 * imu_msg->linear_acceleration.x();                    
    dy = w1 * dy + w2 * imu_msg->linear_acceleration.y();                    
    dz = w1 * dz + w2 * imu_msg->linear_acceleration.z();                    
    rx = w1 * rx + w2 * imu_msg->angular_velocity.x();                    
    ry = w1 * ry + w2 * imu_msg->angular_velocity.y();                    
    rz = w1 * rz + w2 * imu_msg->angular_velocity.z();                    
    estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));                    
    //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);                
}

3. 处理图像数据

然后，对于图像数据，读取其空间点坐标，像素坐标，以及速度。然后，执行

estimator.processImage(image, img_msg->header); // 处理图像数据 

详细流程为：

// TicToc t_s;            
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image; // 处理图像数据            
for (unsigned int i = 0; i < img_msg->points.size(); i++) // 图像特征的遍历            
{                
    int v = img_msg->id_of_point[i] + 0.5;                
    int feature_id = v / NUM_OF_CAM;              
    int camera_id = v % NUM_OF_CAM;// NUM_OF_CAM：相机个数1/2(单目还是双目)                
    double x = img_msg->points[i].x(); // 归一化平面上的坐标                
    double y = img_msg->points[i].y();                
    double z = img_msg->points[i].z();                
    double p_u = img_msg->u_of_point[i]; // 像素坐标                
    double p_v = img_msg->v_of_point[i];                
    double velocity_x = img_msg->velocity_x_of_point[i]; // 速度                
    double velocity_y = img_msg->velocity_y_of_point[i];                
    assert(z == 1);                
    Eigen::Matrix<double, 7, 1> xyz_uv_velocity;                
    xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;                
    image[feature_id].emplace_back(camera_id, xyz_uv_velocity);            
}            
TicToc t_processImage;            
estimator.processImage(image, img_msg->header); // 处理图像数据  
                      
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)            
{                
    Vector3d p_wi;                
    Quaterniond q_wi;                
    q_wi = Quaterniond(estimator.Rs[WINDOW_SIZE]);                
    p_wi = estimator.Ps[WINDOW_SIZE];                
    vPath_to_draw.push_back(p_wi);                
    double dStamp = estimator.Headers[WINDOW_SIZE];                
    cout << "1 BackEnd processImage dt: " << fixed << t_processImage.toc() << " stamp: " <<  dStamp << " p_wi: " << p_wi.transpose() << endl;                
    ofs_pose << fixed << dStamp << " " << p_wi.transpose() << " " << q_wi.coeffs().transpose() << endl;            
    }        
  }        
  m_estimator.unlock();    
}

其中，IMG_MSG的结构体为：

//image for vio    
struct IMG_MSG 
{    
    double header;    
    vector<Vector3d> points;    
    vector<int> id_of_point;    
    vector<float> u_of_point; // 特征点的像素值    
    vector<float> v_of_point;    
    vector<float> velocity_x_of_point; // 特征点的速度    
    vector<float> velocity_y_of_point;
};

thd_BackEnd线程框架分析完了，现在回过头来：

4. 细节

4.1. estimator.processIMU()

关于estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz))的函数解析：
要注意dt的取值。

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{   
    // 第一个IMU数据的处理    
    if (!first_imu) // 初始值为false    
    {        
        first_imu = true;        
        acc_0 = linear_acceleration;        
        gyr_0 = angular_velocity;    
     }    
     // IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];    
     // 是一个数组，里面存放着(WINDOW_SIZE + 1)个指针，指针指向的类型是IntegrationBase    
     // 如果是新的一帧,则新建一个预积分项目    
     if (!pre_integrations[frame_count])    
     {        
         pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};   
     }    
     // frame_count是窗内图像帧的计数    
     // 一个窗内有多个相机帧，每个相机帧之间又有多个IMU数据    
     if (frame_count != 0)    
     {   
        // push_back()这个成员函数最后调用了另一个成员函数propagate()，且原封不动地把自己的三个输入参数都传递了过去        
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);// k+1 重载        
        //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);
        // 使用中值法求解当前时刻PVQ，对应公式        
        int j = frame_count;        
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;         
        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 * (un_acc_0 + un_acc_1);        
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;        
        Vs[j] += dt * un_acc;    
    }    
    // 让此时刻的值等于上一时刻的值，为下一次计算做准备    
    acc_0 = linear_acceleration;    
    gyr_0 = angular_velocity;
}

其中，关于push_back():

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)    
{        
    dt_buf.push_back(dt);        
    acc_buf.push_back(acc);        
    gyr_buf.push_back(gyr);        
    propagate(dt, acc, gyr);    
}

关于IntegrationBase类的结构、IMU预积分的知识点以及更详细的代码解析在另一篇博客——指路

4.2 estimator.processImage()

对于图像数据，读取其空间点坐标，像素坐标，以及速度:

Eigen::Matrix<double, 7, 1> xyz_uv_velocity;                
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;                
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);

然后执行下面的函数，处理图像数据：

TicToc t_processImage;            
estimator.processImage(image, img_msg->header); // 处理图像数据，header指向帧对应的时间戳

此函数的代码在estimator.cpp中，其主要流程见下图：

4.2.1 判定次新帧是否是关键帧

判断像素点的视差(第一个图像到第二个图像像素运动了多少像素单位)，超过阈值则判定为新的关键帧。若次新帧是关键帧，则marg掉老帧；否则marg掉前一帧。

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double header)
{    
    //ROS_DEBUG("new image coming ------------------------------------------");    
    // cout << "Adding feature points: " << image.size()<<endl; 
    // 判断次新帧是否是关键帧   
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))         
        marginalization_flag = MARGIN_OLD; // 是关键帧，marg掉老帧    
    else        
        marginalization_flag = MARGIN_SECOND_NEW; // 不是关键帧，marg掉前一帧

其中，feature_manager.cpp中的f_manager.addFeatureCheckParallax(frame_count, image, td)的解析如下：

首先为什么要计算视差？自然是为了降低计算量。VINS中为了控制优化计算量，只对当前帧之前某一部分帧进行优化(滑动窗口内)，而不是全部历史帧，局部优化帧数量的大小就是窗口大小。

而滑动窗口的大小是提前设置好的，固定不变。因此，当有新帧添加进来时，需要去除旧帧(边缘化Marginalization)。但是边缘化有两个不同的对象——删去最旧的帧（MARGIN_OLD)、删去刚刚进来窗口倒数第二新帧(MARGIN_SECOND_NEW)，为此我们可以对当前帧与之前帧进行视差比较，如果是当前帧变化很小，则次新帧就不设置为关键帧，就会删去倒数第二新帧(MARGIN_SECOND_NEW)，如果变化很大，则次新帧就被设置为关键帧，此时就删去最旧的帧（MARGIN_OLD)。

代码中，我们通过检测两帧之间的视差以及特征点数量决定新帧是否作为关键帧。

① 基础结构体的介绍

1.有必要先说一下image的类型，函数输入的是图像帧上的特征点，但是会把能观测到这个特征点的所有帧也都放进去，从左到右，第一个索引是特征点ID，第二个索引是观测到该特征点的相机帧 ID。

2.FeaturePerFrame的结构体：

指的是每帧基本的数据：特征点[x,y,z,u,v,vx,vy]和td：IMU与cam同步时间差

class FeaturePerFrame
{
public:
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)  
    {    
        // _point 每帧的特征点[x,y,z,u,v,vx,vy]和td IMU和cam同步时间差    
        point.x() = _point(0); // 归一化坐标    
        point.y() = _point(1);    
        point.z() = _point(2);    
        uv.x() = _point(3); // 像素坐标    
        uv.y() = _point(4);    
        velocity.x() = _point(5); // 速度    
        velocity.y() = _point(6);    
        cur_td = td; // IMU和相机的同步时间差  
    }  
    double cur_td;   
    Vector3d point;  
    Vector2d uv;  
    Vector2d velocity;  
    double z; // 特征点的深度  
    bool is_used; // 是否被用了  
    double parallax; // 视差  
    MatrixXd A; //变换矩阵  
    VectorXd b;  
    double dep_gradient;
};

3.FeaturePerId类的结构体：

指的是某feature_id下的所有FeaturePerFrame，即能观测到该ID特征点的所有的帧。常用feature_id和观测第一帧start_frame、最后一帧endFrame()

// 某feature_id下的所有FeaturePerFrame
class FeaturePerId
{
public:  
    const int feature_id; // 特征点ID索引  
    int start_frame; //首次被观测到时，该帧的索引  
    vector<FeaturePerFrame> feature_per_frame; //所有观测到该特征的图像帧的信息，包括图像坐标、特征点的跟踪速度、空间坐标等属性

    int used_num; //该特征出现的次数  
    bool is_outlier; //是否是外点  
    bool is_margin; // 是否边缘化  
    double estimated_depth; //逆深度  int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;

    Vector3d gt_p;
    FeaturePerId(int _feature_id, int _start_frame)      
    :  feature_id(_feature_id), start_frame(_start_frame),        
       used_num(0), estimated_depth(-1.0), solve_flag(0)  
    {  
    }
    int endFrame(); // 返回最后一次观测到这个特征点的帧数ID
};

② addFeatureCheckParallax()函数介绍

计算视差、判断是否是关键帧的大体流程为：

1. 遍历该帧上的所有特征点，查看此特征点是否在feature这个list中
   1.1 若不在list中，则新建一个，并在feature的list容器最后添加：
   - - FeaturePerId：特征点ID，首次观测到特征点的图像帧ID
   - - FeaturePerFrame：每帧基本的数据-特征点[x,y,z,u,v,vx,vy]&td(IMU与cam同步时间差)
   1.2 若在list中，则直接在FeaturePerFrame中添加此特征点在此帧的位置和其他信息，并统计此帧中被追踪的特征点的数目。
2. 判断是否是关键帧
   2.1 若追踪次数小于20或者窗口内帧的数目小于2，是关键帧
   2.2 计算每个特征在次新帧和次次新帧的视差，计算总视差后求平均视差。平均视差大于阈值的是关键帧。

// 特征点进入时检查视差，是否为关键帧        
// map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image  
// 注意image的类型，见前面解析！       
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{     
    double parallax_sum = 0; // 所有特征点视差总和    
    int parallax_num = 0; // 计算视差的次数    
    last_track_num = 0; // 在此帧上被跟踪特征点的个数    
    for (auto &id_pts : image)// 遍历所有特征点    
    {        
        // FeaturePerFrame：每帧基本的数据-特征点[x,y,z,u,v,vx,vy]和td：IMU与cam同步时间差        
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        
        // 1.1迭代器寻找feature这个list中是否有这feature_id        
        // 如果没在,则将<FeatureID,Start_frame>存入到Feature列表中；否则统计数目
        // list<FeaturePerId> feature;               
        int feature_id = id_pts.first; // 特征点ID！！！注意image的类型        
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)                          
        {            return it.feature_id == feature_id;                        
        });
        // 1.2 如果没有，则新建一个，并在feature管理器的list容器最后添加：FeaturePerId、FeaturePerFrame        
        if (it == feature.end())        
        {            
            // list            
            // 添加 <FeatureID,Start_frame>:（特征点ID，首次观测到特征点的图像帧ID）            
            feature.push_back(FeaturePerId(feature_id, frame_count));             
            // 添加 每帧基本的数据-特征点[x,y,z,u,v,vx,vy]和td：IMU与cam同步时间差            
            feature.back().feature_per_frame.push_back(f_per_fra);        
        }        
        // 1.3 有的话，在FeaturePerFrame添加此特征点在此帧的位置和其他信息，并统计数目。        
        else if (it->feature_id == feature_id)        
        {            
            it->feature_per_frame.push_back(f_per_fra);            
            last_track_num++; // 此帧中被跟踪的特征点的个数        
        }    
    }    
    // 2. 若追踪次数小于20或者窗口内帧的数目小于2，是关键帧    
    if (frame_count < 2 || last_track_num < 20)        
        return true;    
    // 3.计算每个特征在次新帧和次次新帧中的视差    
    for (auto &it_per_id : feature)    
    {        
        // 观测该特征点的：起始帧小于倒数第三帧，终止帧要大于倒数第二帧，保证至少有两帧能观测到。        
        if (it_per_id.start_frame <= frame_count - 2 &&            
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)        
        {            
            // 总视差：该特征点在两帧的归一化平面上的坐标点的距离ans            
            parallax_sum += compensatedParallax2(it_per_id, frame_count);            
            parallax_num++;// 计算视差的次数        
        }    
    }
    // 4.1 第一次加进去的，是关键帧    
    if (parallax_num == 0)     
    {        
        return true;    
    }    
    else    
    {             
        //  4.2 平均视差大于阈值的是关键帧        
        return parallax_sum / parallax_num >= MIN_PARALLAX;    
    }
}

其中，最关键的是计算视差的函数compensatedParallax2(it_per_id, frame_count)。

③ compensatedParallax2()函数介绍

计算某个特征点it_per_id在次新帧和次次新帧的视差ans。

判断观测到该特征点的frame中倒数第二帧和倒数第三帧的共视关系 实际是求取该特征点在两帧的归一化平面上的坐标点的距离ans

// 计算视差：该特征点在两帧的归一化平面上的坐标点的距离ans
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{    
    //check the second last frame is keyframe or not    
    //parallax betwwen seconde last frame and third last frame    
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame]; // 次次新帧-倒数第三帧    
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame]; // 次新帧-倒数第二帧
    double ans = 0; // 视差    
    Vector3d p_j = frame_j.point; // 3D路标点（倒数第二帧j）
    double u_j = p_j(0); // 归一化坐标    
    double v_j = p_j(1);
    
    Vector3d p_i = frame_i.point; // 3D路标点 (倒数第三帧i)     
    Vector3d p_i_comp;
    //int r_i = frame_count - 2;    
    //int r_j = frame_count - 1;    
    //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;    
    p_i_comp = p_i;    
    double dep_i = p_i(2); // Z    
    double u_i = p_i(0) / dep_i; // 归一化坐标    
    double v_i = p_i(1) / dep_i;    
    double du = u_i - u_j, dv = v_i - v_j;
    
    double dep_i_comp = p_i_comp(2);    
    double u_i_comp = p_i_comp(0) / dep_i_comp;    
    double v_i_comp = p_i_comp(1) / dep_i_comp;    
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
    // 算斜边    
    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));
    
    return ans;
}

4.2.2 填充imageframe的容器并更新临时预积分初始值

// 2. 填充imageframe的容器以及更新临时预积分初始值    
// ImageFrame类包括特征点、时间、位姿Rt、预积分对象pre_integration、是否关键帧    
ImageFrame imageframe(image, header);    
imageframe.pre_integration = tmp_pre_integration; // 在processIMU中已经准备好    
all_image_frame.insert(make_pair(header, imageframe));    
// 更新临时预积分初始值
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

其中，ImageFrame的结构体如下：

class ImageFrame
{
public:    
    ImageFrame(){};    
    ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &_points, double _t) : t{_t}, is_key_frame{false}    
    {        
        points = _points;    
    };    
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> points;    
    double t;    
    Matrix3d R;    
    Vector3d T;    
    IntegrationBase *pre_integration;    
    bool is_key_frame;
};

可以看到ImageFrame类包括特征点、时间、位姿Rt、预积分对象pre_integration、是否关键帧。

4.2.3 外参标定

// 3. 如果没有外参则标定IMU到相机的外参    
if (ESTIMATE_EXTRINSIC == 2) // 对外参一无所知    
{        
    cout << "calibrating extrinsic param, rotation movement is needed" << endl;        
    if (frame_count != 0)        
    {            
        // 得到给定两帧之间的对应特征点3D坐标            
        vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);            
        Matrix3d calib_ric;            
        //pre_integrations[frame_count]->delta_q是使用imu预积分获取的旋转矩阵,calib_ric为要计算的相机到IMU的旋转            
        if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)) // 标定外参数的旋转            
        {               
             ric[0] = calib_ric; // 标定好的外参                
             RIC[0] = calib_ric;                
             ESTIMATE_EXTRINSIC = 1; // 得到了外参，可对整个vins系统进行初始化            
         }        
     }    
 }

外参标定的详细代码解析可见我的另一篇博客——指路

4.2.4 系统未初始化，则进行初始化

if (solver_flag == INITIAL) // 判断系统是否已经进行初始化    
{           
     // frame_count是滑动窗口中图像帧的数量，一开始初始化为0，滑动窗口总帧数WINDOW_SIZE=10        
     // 确保有足够的frame参与初始化,否则增加图像帧
    if (frame_count == WINDOW_SIZE)        
    {            
        bool result = false;            
        //外参初始化成功且当前帧时间戳距离上次初始化时间戳大于0.1秒，就进行初始化操作            
        if (ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)            
        {                
            // cout << "1 initialStructure" << endl;                
            //对视觉和惯性单元进行初始化                
            result = initialStructure();                
            initial_timestamp = header;            
        }
        ...
        // 初始化成功，进行非线性优化。
        ...
    }
    else            
        frame_count++; //图像帧数量+1
 }    
 else    
 {        
     // 若已经进行初始化，则进行紧耦合的非线性优化
     // 5. 紧耦合的非线性优化
 }

指路：
VINS-Mono代码精简版代码详解——初始化2视觉初始化(内容|代码)
VINS-Mono代码精简版代码详解——初始化3视觉IMU对齐(内容|代码)
VINS-Mono代码精简版代码详解——初始化4visualInitialAlign()(内容|代码)

4.2.5 初始化完成，进行非线性优化

如果初始化完成则进行非线性优化、边缘化操作、滑动窗口以及剔除特征中估计失败的点。如果初始化失败，则直接滑动窗口。

if (result) // result==true,初始化完成——>非线性优化            
{                
    //先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿                
    solver_flag = NON_LINEAR;// 初始化更改为非线性                
    solveOdometry(); // 4.3 非线性化求解里程计                
    slideWindow(); // 4.4 滑动窗                
    f_manager.removeFailures();// 4.5 剔除feature中估计失败的点（solve_flag == 2）0 haven't solve yet; 1 solve succ; 2 solve fail;                
    cout << "Initialization finish!" << endl;                
    // 4.6 初始化窗口中PVQ                
    last_R = Rs[WINDOW_SIZE];                
    last_P = Ps[WINDOW_SIZE];                
    last_R0 = Rs[0];                
    last_P0 = Ps[0];            
}            
else                
    slideWindow(); //初始化失败则直接滑动窗口        
}

① solveOdometry()

主要内容包括三角化操作，以及后端的优化(构建problem求解器进行舒尔补求解、边缘化操作)。

void Estimator::solveOdometry()
{    
    if (frame_count < WINDOW_SIZE)//确保有足够的frame参与       
        return;    
    if (solver_flag == NON_LINEAR)    
    {        
        TicToc t_tri;        
        f_manager.triangulate(Ps, tic, ric); // 三角化操作        
        //cout << "triangulation costs : " << t_tri.toc() << endl;                
        backendOptimization(); // 后端优化    
     }
  }

1.1 三角化triangulate()

f_manager.triangulate()函数属于这篇博客中的第二种方法——线性三角化方法。

1.2 backendOptimization()

后端优化的状态向量包括滑动窗口内的所有相机状态（位置P、旋转Q、速度V、加速度偏置$b_a$、陀螺仪偏置$b_w$）、相机到IMU的外参、所有3D点的逆深度，具体形式如下：

后端优化的整体代码如下：

void Estimator::backendOptimization()
{    
    TicToc t_solver;    // 借助 vins 框架，维护变量    
    vector2double(); // 系统数据类型转换：将向量变成double数组指针的形式    
    // 构建求解器    
    problemSolve(); // 求解最小二乘问题    
    // 优化后的变量处理下自由度    
    double2vector(); // 第一帧返回飘移前的状态，使其前后一致    
    
    // 维护 marg    
    TicToc t_whole_marginalization;    
    if (marginalization_flag == MARGIN_OLD)    
    {        
        vector2double();
        MargOldFrame();
        std::unordered_map<long, double *> addr_shift; // prior 中对应的保留下来的参数地址        
        for (int i = 1; i <= WINDOW_SIZE; i++)        
        {            
            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];            
            addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];        
        }        
        for (int i = 0; i < NUM_OF_CAM; i++)            
            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];        
        if (ESTIMATE_TD)        
        {            
            addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];        
        }    
    }    
    else    
    {        
        if (Hprior_.rows() > 0)        
        {
             vector2double();
             MargNewFrame();
             std::unordered_map<long, double *> addr_shift;            
             for (int i = 0; i <= WINDOW_SIZE; i++)            
             {                
                 if (i == WINDOW_SIZE - 1)                    
                     continue;                
                 else if (i == WINDOW_SIZE)                
                 {                    
                     addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];                    
                     addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];                
                 }                
                 else                
                 {                    
                     addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];                    
                     addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];                
                 }            
             }            
             for (int i = 0; i < NUM_OF_CAM; i++)                
                 addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];            
             if (ESTIMATE_TD)            
             {                
                 addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];            
             }        
         }    
     }    
 }

1.2.1 系统数据类型转换vector2double()

将向量变成double数组指针的形式:由相机的旋转和平移Ps,Rs转换为para_Pose，速度Vs,加速度偏置Bas,陀螺仪偏置Bgs转换为para_SpeedBias，相机到IMU的外参tic,ric转换为para_Ex_Pose，逆深度dep转换成para_Feature，时间戳td转换成para_Td。代码如下：

void Estimator::vector2double()
{    
    for (int i = 0; i <= WINDOW_SIZE; i++)     
    {        
        para_Pose[i][0] = Ps[i].x(); // 平移        
        para_Pose[i][1] = Ps[i].y();        
        para_Pose[i][2] = Ps[i].z();
                
        Quaterniond q{Rs[i]};        
        para_Pose[i][3] = q.x(); // 旋转        
        para_Pose[i][4] = q.y();        
        para_Pose[i][5] = q.z();        
        para_Pose[i][6] = q.w();
        
        para_SpeedBias[i][0] = Vs[i].x(); // 速度        
        para_SpeedBias[i][1] = Vs[i].y();        
        para_SpeedBias[i][2] = Vs[i].z();
        
        para_SpeedBias[i][3] = Bas[i].x(); // 加速度偏置        
        para_SpeedBias[i][4] = Bas[i].y();        
        para_SpeedBias[i][5] = Bas[i].z();
        
        para_SpeedBias[i][6] = Bgs[i].x(); // 角速度偏置        
        para_SpeedBias[i][7] = Bgs[i].y();        
        para_SpeedBias[i][8] = Bgs[i].z();    
    }    
    for (int i = 0; i < NUM_OF_CAM; i++) // 外参数    
    {        
        para_Ex_Pose[i][0] = tic[i].x();        
        para_Ex_Pose[i][1] = tic[i].y();        
        para_Ex_Pose[i][2] = tic[i].z();
                
        Quaterniond q{ric[i]};        
        para_Ex_Pose[i][3] = q.x();        
        para_Ex_Pose[i][4] = q.y();        
        para_Ex_Pose[i][5] = q.z();        
        para_Ex_Pose[i][6] = q.w();    
    }
    VectorXd dep = f_manager.getDepthVector(); // 逆深度    
    for (int i = 0; i < f_manager.getFeatureCount(); i++)        
         para_Feature[i][0] = dep(i);    
     if (ESTIMATE_TD)        
         para_Td[0][0] = td; // 时间戳
 }

1.2.2 构建求解器problemSolve()

在这个函数当中添加残差：
包括先验残差，IMU测量残差，camera测量残差。注意这里IMU项和camera项之间是有一个系数，这个系数就是他们各自的协方差矩阵：IMU的协方差是预积分的协方差(IMUFactor::Evaluate，中添加IMU协方差，求解jacobian矩阵)，而camera的测量残差则是一个固定的系数($\frac{f}{1.5}$)

主要步骤如下：

(1) 声明和引入鲁棒核函数

void Estimator::problemSolve()
{    
    // a. 声明和引入鲁棒核函数    
    backend::LossFunction *lossfunction;    
    lossfunction = new backend::CauchyLoss(1.0); // 鲁棒核函数
    ...    

(2) 构建 problem

    // step1. 构建 problem    backend::Problem 
    problem(backend::Problem::ProblemType::SLAM_PROBLEM);    
    vector<shared_ptr<backend::VertexPose>> vertexCams_vec;    
    vector<shared_ptr<backend::VertexSpeedBias>> vertexVB_vec;    
    int pose_dim = 0;

(3) 添加外参数节点
如果外参数已经估计优化好，则设置该节点为Fix。

    shared_ptr<backend::VertexPose> vertexExt(new backend::VertexPose());    
    {        
         Eigen::VectorXd pose(7);        
         pose << para_Ex_Pose[0][0], para_Ex_Pose[0][1], para_Ex_Pose[0][2], para_Ex_Pose[0][3], para_Ex_Pose[0][4], para_Ex_Pose[0][5], para_Ex_Pose[0][6];        
         vertexExt->SetParameters(pose);
         
        //  如果外参数已经估计优化好，则设置该节点为Fix。        
        if (!ESTIMATE_EXTRINSIC)        
        {                      
            vertexExt->SetFixed();        
        }        
        else
        {            
            //ROS_DEBUG("estimate extinsic param");        
        }        
        problem.AddVertex(vertexExt);        
        pose_dim += vertexExt->LocalDimension();    
    }

(4)添加相机位姿和IMU速度以及偏置节点

   // b. 添加各种待优化量X——位姿优化量    
   for (int i = 0; i < WINDOW_SIZE + 1; i++) // 还包括最新的第11帧    
   {        
       shared_ptr<backend::VertexPose> vertexCam(new backend::VertexPose());        
       Eigen::VectorXd pose(7);        
       pose << para_Pose[i][0], para_Pose[i][1], para_Pose[i][2], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5], para_Pose[i][6];        
       vertexCam->SetParameters(pose);        
       vertexCams_vec.push_back(vertexCam);        
       problem.AddVertex(vertexCam);        
       pose_dim += vertexCam->LocalDimension();
       
       shared_ptr<backend::VertexSpeedBias> vertexVB(new backend::VertexSpeedBias());        
       Eigen::VectorXd vb(9);        
       vb << para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2],            
           para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5],            
           para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8];        
       vertexVB->SetParameters(vb);        
       vertexVB_vec.push_back(vertexVB);        
       problem.AddVertex(vertexVB);        
       pose_dim += vertexVB->LocalDimension();    
   }

(5)添加IMU预积分对应的约束边

其中，vertexCams_vec[i]和vertexCams_vec[j]分别为两个时刻的body坐标系的全局位姿估计，对应的是para_Pose。vertexVB_vec[i]和vertexVB_vec[j]分别为两个时刻的速度，陀螺仪和加速度偏置，对应的是para_SpeedBias。在增加了约束边之后，在接下来的非线性优化中就要对该边的残差进行计算,相关函数为EdgeImu::ComputeResidual()

 // 添加IMU预积分对应的约束边    
 for (int i = 0; i < WINDOW_SIZE; i++)    
 {        
     int j = i + 1;        
     if (pre_integrations[j]->sum_dt > 10.0)            
         continue;
     std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[j]));        
     std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
             
     edge_vertex.push_back(vertexCams_vec[i]); // P、q        
     edge_vertex.push_back(vertexVB_vec[i]); // v、bias        
     edge_vertex.push_back(vertexCams_vec[j]);        
     edge_vertex.push_back(vertexVB_vec[j]);        
     imuEdge->SetVertex(edge_vertex);        
     problem.AddEdge(imuEdge);    
 }

(6) 添加Visual Factor

遍历每一个特征：

   // Visual Factor    
   //used_num; //该特征出现的次数    
   vector<shared_ptr<backend::VertexInverseDepth>> vertexPt_vec;    
   {        
       int feature_index = -1;        
       // 遍历每一个特征        
       for (auto &it_per_id : f_manager.feature)        
       {            
           it_per_id.used_num = it_per_id.feature_per_frame.size();            
           if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))                
               continue;
               
           ++feature_index;
           int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;            
           Vector3d pts_i = it_per_id.feature_per_frame[0].point;

先添加每个特征的逆深度节点：

// 添加每个特征的逆深度节点            
shared_ptr<backend::VertexInverseDepth> verterxPoint(new backend::VertexInverseDepth());            
VecX inv_d(1);            
inv_d << para_Feature[feature_index][0];            
verterxPoint->SetParameters(inv_d);            
problem.AddVertex(verterxPoint);            
vertexPt_vec.push_back(verterxPoint);

然后针对每个特征，遍历其所有观测，构建所有可以观测到同一特征的帧之间的约束边，包括的信息：逆深度，边的顶点对应的IMU全局位姿，外参数，信息矩阵，loss function核函数，代码如下：

 for (auto &it_per_frame : it_per_id.feature_per_frame)            
 {                
     imu_j++;                
     if (imu_i == imu_j)                    
         continue;
         
     Vector3d pts_j = it_per_frame.point;
     std::shared_ptr<backend::EdgeReprojection> edge(new backend::EdgeReprojection(pts_i, pts_j));                
     std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;                
     edge_vertex.push_back(verterxPoint); // 逆深度                
     edge_vertex.push_back(vertexCams_vec[imu_i]); // 相机P、V                
     edge_vertex.push_back(vertexCams_vec[imu_j]);                
     edge_vertex.push_back(vertexExt); // 相机到IMU的外参数
     edge->SetVertex(edge_vertex);                
     edge->SetInformation(project_sqrt_info_.transpose() * project_sqrt_info_); // 信息矩阵
                
     edge->SetLossFunction(lossfunction); // 核函数                
     problem.AddEdge(edge);            
 }

(7) 添加先验

  // 先验    
  {        
  // 已经有 Prior 了        
  if (Hprior_.rows() > 0)        
  {            
      // 外参数先验设置为 0. TODO:: 这个应该放到 solver 里去弄            
      //            Hprior_.block(0,0,6,Hprior_.cols()).setZero();            
      //            Hprior_.block(0,0,Hprior_.rows(),6).setZero();
      problem.SetHessianPrior(Hprior_); // 告诉这个 problem            
      problem.SetbPrior(bprior_);            
      problem.SetErrPrior(errprior_);            
      problem.SetJtPrior(Jprior_inv_);            
      problem.ExtendHessiansPriorSize(15); // 但是这个 prior 还是之前的维度，需要扩展下装新的pose        
  }   
}

(8) 非线性优化求解——LM方法

problem.Solve(10); // 求解

相关详细代码解析之前总结过，指路——单目BA求解代码解析的2.2.3部分。

(9)更新先验，H先验不用更新-后续会有更改

$???$

if (Hprior_.rows() > 0)
{
    bprior_ = problem.GetbPrior(); // 保存一下        
    errprior_ = problem.GetErrPrior();
}

(10) 位姿更新

优化求解完毕后，对窗口的IMU位姿、速度以及偏置向量进行更新。

    // update parameter    
    for (int i = 0; i < WINDOW_SIZE + 1; i++)    
    {        
        VecX p = vertexCams_vec[i]->Parameters(); // imu的P、Q        
        for (int j = 0; j < 7; ++j)        
        {            
            para_Pose[i][j] = p[j];        
        }
        VecX vb = vertexVB_vec[i]->Parameters(); // imu的速度、bias        
        for (int j = 0; j < 9; ++j)        
        {            
            para_SpeedBias[i][j] = vb[j];        
        }    
    }

(10)对每个特征的逆深度进行更新

for (int i = 0; i < vertexPt_vec.size(); ++i)    
{        
    VecX f = vertexPt_vec[i]->Parameters();        
    para_Feature[i][0] = f[0]; // 逆深度
}

至此，Estimator::problemSolve()的代码已经全部完成。

1.2.3 double2vector();

优化后的变量处理下自由度，第一帧返回飘移前的状态，使其前后一致

double2vector();

详细代码略。

1.2.4 边缘化

在之前判断过新来帧是否是关键帧，从而确定边缘化选项(MARGIN_OLD / MARGIN_SECOND_NEW）

// 判断是否是关键帧   
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))         
        marginalization_flag = MARGIN_OLD; // 是关键帧，marg掉老帧    
    else        
        marginalization_flag = MARGIN_SECOND_NEW; // 不是关键帧，marg掉前一帧

相关边缘化的理论知识以及代码解析可以看我的另一篇博客，指路——边缘化(内容|代码)。

至此，backendOptimization()函数解析全部完成，solveOdometry()函数解析也完成啦★,°:.☆(￣▽￣)/$:.°★ 。

② 实现滑窗的更新

前面已经进行了滑窗的预更新，相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合现在调用的slideWindow()函数才能实现真正的滑窗移动。
具体操作为：
如果次新帧是关键帧，则边缘化最老帧，将其看到的特征点和IMU数据转化为先验信息，如果次新帧不是关键帧，则舍弃视觉测量而保留IMU测量值，从而保证IMU预积分的连贯性。

slideWindow();

1. 若边缘化掉最老帧
   将最旧的pose移出Sliding Window，将滑窗内的所有信息前移一格，移进新帧。将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化。

void Estimator::slideWindow()
{
    TicToc t_margin;
    // 如果边缘化掉的是最老帧
    if (marginalization_flag == MARGIN_OLD)
    {
        // 1. 保存最老帧信息
        double t_0 = Headers[0]; // 最老帧的时间戳
        back_R0 = Rs[0];
        back_P0 = Ps[0];
        if (frame_count == WINDOW_SIZE)
        {
            for (int i = 0; i < WINDOW_SIZE; i++)
            {
                // 2. 滑窗内所有信息前移
                Rs[i].swap(Rs[i + 1]);

                std::swap(pre_integrations[i], pre_integrations[i + 1]); // IMU预积分量

                dt_buf[i].swap(dt_buf[i + 1]); // 帧之间的时间间隔dt
                linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]); // 线性加速度
                angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]); // 线性角速度

                Headers[i] = Headers[i + 1]; // 时间戳
                Ps[i].swap(Ps[i + 1]); // IMU状态量
                Vs[i].swap(Vs[i + 1]);
                Bas[i].swap(Bas[i + 1]);
                Bgs[i].swap(Bgs[i + 1]);
            }
            // 假设滑动窗口内一共有10帧
            // 3.1 滑窗内第10帧信息给了11帧（第10、11帧相同都是新来的）
            Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
            Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1]; 
            Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
            Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
            Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
            Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
            // 3.2 新实例化一个IMU预积分给到第11帧
            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
            // 3.3 清空第11帧的三个buf
            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();
            // 4. 最老帧之前预积分和图像都删除(包括最老帧)
            if (true || solver_flag == INITIAL) // 系统是否进行了初始化
            {
                // 4.1 删除最老帧的预积分信息
                map<double, ImageFrame>::iterator it_0;
                it_0 = all_image_frame.find(t_0); // 最老帧
                delete it_0->second.pre_integration; 
                it_0->second.pre_integration = nullptr; // 空指针常量-指针类型
                // 4.2 最老帧之前的IMU预积分都删除
                for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
                {
                    if (it->second.pre_integration)
                        delete it->second.pre_integration;
                    it->second.pre_integration = NULL;
                }
                // 4.3 最老帧和之前图像帧都删除
                all_image_frame.erase(all_image_frame.begin(), it_0); 
                all_image_frame.erase(t_0);
            }
            slideWindowOld();
        }
    }

其中，slideWindowOld()函数：

// real marginalization is removed in solve_ceres()
void Estimator::slideWindowOld()
{    
    // 1、统计一共多少次merg滑窗第一帧情况    
    sum_of_back++;
    
    bool shift_depth = solver_flag == NON_LINEAR ? true : false;    
    if (shift_depth) // 非线性优化    
    {        
        // back_R0、back_P0为窗口中最老帧的位姿        
        // Rs[0]、Ps[0]为滑动窗口后第0帧(imu)的位姿，即原来的第1帧        
        // ric中存放的是相机到IMU的旋转，tic中存放的是相机到IMU的平移,在相机-IMU外参标定部分求得        
        Matrix3d R0, R1;        
        Vector3d P0, P1;        
        R0 = back_R0 * ric[0]; // 滑窗原来第0帧(要边缘化掉)        
        R1 = Rs[0] * ric[0]; // 滑窗原来第1帧（现在最老的第0帧）        
        P0 = back_P0 + back_R0 * tic[0];        
        P1 = Ps[0] + Rs[0] * tic[0];         
        // 首次在原来最老帧出现的特征点转移到现在的最老帧        
        f_manager.removeBackShiftDepth(R0, P0, R1, P1);    
    }    
    else         
        // 当最新一帧是关键帧，merg滑窗内最老帧        
        f_manager.removeBack();
}

① f_manager.removeBackShiftDepth(R0, P0, R1, P1);

详细代码如下：

// 边缘化最老帧时，处理特征点保存的帧号，将起始帧是最老帧的特征点的深度值进行转移，转移到现在的最老帧
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{    
    for (auto it = feature.begin(), it_next = feature.begin();         
         it != feature.end(); it = it_next)    
    {        
        it_next++;
        
       // 若第一次出现该特征点的帧不是最老帧        
       if (it->start_frame != 0)             
           it->start_frame--; // 可共视该特征点的所有帧id前移        
       else        
       {  
           // 若第一次出现该特征点的帧是最老帧            
           Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  // 该特征点在最老帧上的归一化坐标[x,y,1]            
           it->feature_per_frame.erase(it->feature_per_frame.begin()); // 移除最老帧  
                     
           // 特征点只在最老帧被观测，则直接移除            
           if (it->feature_per_frame.size() < 2)            
           {                
               feature.erase(it); // 移除特征点                
               continue;            
           }            
           else            
           {                
               // pts_i为特征点在最老帧坐标系下的三维坐标                
               // w_pts_i为特征点在世界坐标系下的三维坐标                
               // 将其转换到在下一帧坐标系下的坐标pts_j                
               Eigen::Vector3d pts_i = uv_i * it->estimated_depth;                
               Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;                
               Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P); // 转换到下一帧坐标系下的三维坐标                
               double dep_j = pts_j(2); // 深度                
               if (dep_j > 0)                    
                   it->estimated_depth = dep_j;                
               else                    
                   it->estimated_depth = INIT_DEPTH;            
           }        
       }          
   }
}

② f_manager.removeBack();

相关代码如下：

void FeatureManager::removeBack()
{    
    for (auto it = feature.begin(), it_next = feature.begin();         
        it != feature.end(); it = it_next)    
    {        
        it_next++;        
        
        // 若第一次出现该特征点的帧不是最老帧        
        if (it->start_frame != 0)            
            it->start_frame--; // 可共视该特征点的所有帧id前移        
        else        
        {   
            // 若第一次出现该特征点的帧是最老帧            
            it->feature_per_frame.erase(it->feature_per_frame.begin()); // 移除feature_per_frame中最老帧对应的FeaturePerFrame            
            if (it->feature_per_frame.size() == 0) // 没有帧观测到该特征点                
                feature.erase(it); // 直接剔除该特征点        
        }    
    }
}

2. 若边缘化掉次新帧
   边缘化次新帧，但不删除IMU信息

    else
    {
        // 如果边缘化掉的是次新帧
        if (frame_count == WINDOW_SIZE)
        {
            for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
            {
                // 取出最新一帧的信息
                double tmp_dt = dt_buf[frame_count][i];
                Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
                Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
                
                // 保留IMU约束，‘次新帧’和前一帧的IMU预积分转化为最新帧和前二帧的预积分
                pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
                // 将最新帧的dt、加速度、角速度信息复制到次新帧上
                dt_buf[frame_count - 1].push_back(tmp_dt);
                linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
                angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
            }
            // 将最新帧的IMU信息复制到次新帧上
            Headers[frame_count - 1] = Headers[frame_count];
            Ps[frame_count - 1] = Ps[frame_count];
            Vs[frame_count - 1] = Vs[frame_count];
            Rs[frame_count - 1] = Rs[frame_count];
            Bas[frame_count - 1] = Bas[frame_count];
            Bgs[frame_count - 1] = Bgs[frame_count];
            // 新实例化一个IMU预积分给到第WINDOW_SIZE帧
            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
            // 清空第WINDOW_SIZE帧的三个buf
            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();
            // 边缘化掉次新帧
            slideWindowNew();
        }
    }
}

其中， slideWindowNew()详细代码如下：

// real marginalization is removed in solve_ceres()
void Estimator::slideWindowNew()
{    
    sum_of_front++;    
    f_manager.removeFront(frame_count); // 边缘化次新帧，对特征点在次新帧的信息移除
}

void FeatureManager::removeFront(int frame_count)
{    
    for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)    
    {        
        it_next++;        
        // 对于特征点，其起始帧为最新帧的，将帧滑动成次新帧        
        if (it->start_frame == frame_count)        
        {            
            it->start_frame--;        
        }        
        else        
        {            
            int j = WINDOW_SIZE - 1 - it->start_frame;            
            // 如果次新帧之前已经跟踪结束则什么都不做            
            if (it->endFrame() < frame_count - 1)                
                continue;            
            // 如果在次新帧仍被跟踪，则删除feature_per_frame中次新帧对应的FeaturePerFrame            
            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);            
            // 如果feature_per_frame为空，则直接删除特征点            
            if (it->feature_per_frame.size() == 0)                
                feature.erase(it);        
        }    
    }
}

【补充】
关于f_manager的相关函数可以查看这个链接——feature_manager.cpp
参考博客----滑动窗slideWindow()

③ 剔除feature中估计失败的点

（solve_flag == 2）0 haven’t solve yet; 1 solve succ; 2 solve fail;

f_manager.removeFailures();

详细代码如下：

// 剔除feature中估计失败的点void 
FeatureManager::removeFailures()
{    
    for (auto it = feature.begin(), it_next = feature.begin();         
         it != feature.end(); it = it_next)    
    {        
        it_next++;        
        if (it->solve_flag == 2) // 0 haven't solve yet; 1 solve succ; 2 solve fail;            
            feature.erase(it);    
    }
}

④ 剔除帧、滑动窗口后，初始化窗口中的PVQ

last_R = Rs[WINDOW_SIZE];                
last_P = Ps[WINDOW_SIZE];                
last_R0 = Rs[0];                
last_P0 = Ps[0];

4.2.6 紧耦合的非线性优化

        // 5. 紧耦合的非线性优化        
        TicToc t_solve;        
        // 5.1 非线性化求解里程计        
        solveOdometry(); // ！！！不断推断相机在三维空间的运动姿态        
        //ROS_DEBUG("solver costs: %fms", t_solve.toc());
        // 5.2 故障检测与恢复,一旦检测到故障，系统将切换回初始化阶段        
        if (failureDetection())        
        {           
            failure_occur = 1;            
            clearState(); // 清空状态            
            setParameter(); // 重设参数            
            return;        
        }
        TicToc t_margin;        
        slideWindow(); // 滑动窗口        
        f_manager.removeFailures(); // 剔除feature中估计失败的点             
        key_poses.clear();
                
        for (int i = 0; i <= WINDOW_SIZE; i++)            
            key_poses.push_back(Ps[i]); // 关键位姿的位置 Ps
            
        last_R = Rs[WINDOW_SIZE];        
        last_P = Ps[WINDOW_SIZE];        
        last_R0 = Rs[0];        
        last_P0 = Ps[0];    
}

① solveOdometry();

见前。

② failureDetection()

检测故障，如若出现故障则将系统切换回初始化阶段。

        if (failureDetection())        
        {            
            // ROS_WARN("failure detection!");            
            failure_occur = 1;            
            clearState(); // 清空状态            
            setParameter(); // 重设参数            
            // ROS_WARN("system reboot!");            
            return;        
        }

详细代码如下：

bool Estimator::failureDetection()
{    
    if (f_manager.last_track_num < 2)    
    {        
        //ROS_INFO(" little feature %d", f_manager.last_track_num);        
        //return true;    
    }    
    if (Bas[WINDOW_SIZE].norm() > 2.5)    
    {        
        //ROS_INFO(" big IMU acc bias estimation %f", Bas[WINDOW_SIZE].norm());        
        return true;    
    }    
    if (Bgs[WINDOW_SIZE].norm() > 1.0)    
    {        
        //ROS_INFO(" big IMU gyr bias estimation %f", Bgs[WINDOW_SIZE].norm());        
        return true;    
    }    
    /*    
    if (tic(0) > 1)    
    {        
        //ROS_INFO(" big extri param estimation %d", tic(0) > 1);        
        return true;    
    }    
    */    
    Vector3d tmp_P = Ps[WINDOW_SIZE];    
    if ((tmp_P - last_P).norm() > 5)    
    {        
        //ROS_INFO(" big translation");        
        return true;    
    }    
    if (abs(tmp_P.z() - last_P.z()) > 1)    
    {        
        //ROS_INFO(" big z translation");        
        return true;    
    }    
    Matrix3d tmp_R = Rs[WINDOW_SIZE];    
    Matrix3d delta_R = tmp_R.transpose() * last_R;    
    Quaterniond delta_Q(delta_R);    
    double delta_angle;    
    delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;    
    if (delta_angle > 50)    
    {        
        //ROS_INFO(" big delta_angle ");        
        //return true;    
    }    
    return false;
}

void Estimator::setParameter()
{    
    for (int i = 0; i < NUM_OF_CAM; i++)    
    {        
        tic[i] = TIC[i]; // 相机到IMU的平移量        
        ric[i] = RIC[i];        
        // cout << "1 Estimator::setParameter tic: " << tic[i].transpose()        
        //     << " ric: " << ric[i] << endl;    
    }    
    cout << "1 Estimator::setParameter FOCAL_LENGTH: " << FOCAL_LENGTH << endl;    
    f_manager.setRic(ric);    
    project_sqrt_info_ = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();    
    td = TD;
}

VINS解析全部完成！！！撒花★,°:.☆(￣▽￣)/$:.°★ 。