VINS-Mono+Fusion源码解析系列（十四）：初始状态调整

1. 初始位姿调整

（1）大致流程

• 视觉惯性对齐：通过VisualIMUAlignment求解出陀螺仪零偏、每帧速度，修正后的加速度，以及平移的尺度s
• 将对齐后的关键帧位姿赋给滑窗中的相应变量（Rs和Ps）
• 执行f_manager.triangulate三角化：对于每个特征点，使用所有能观测到该特征点的关键帧进行三角化
• 结合对齐后求解出的陀螺仪零偏，重新计算滑窗中的预积分
• 将滑窗中所有的状态量对齐到第0帧的imu坐标系下，同时使用前面求解出来的尺度恢复特征点深度

（2）代码实现

bool Estimator::visualInitialAlign()
{
   
   
    TicToc t_g;
    VectorXd x;
/*
视觉惯性对齐
  all_image_frame[in]: 输入的所有帧信息
  Bags[out]: 计算出的陀螺仪零偏
  g[in]: 输入的重力加速度，在VisualIMUAlignment里面对其方向进行修正
  x[out]: 求解出的每帧速度，修正后的加速度，以及平移的尺度s
*/
    bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
    if(!result)
    {
   
   
        ROS_DEBUG("solve g failed!");
        return false;
    }

    // 首先把对齐后KF的位姿附给滑窗中的值，Rwi twc
    for (int i = 0; i <= frame_count; i++)
    {
   
   
/*
all_image_frame中每个元素的结构如下:
  first: header.stamp.toSec()时间戳
  second: ImageFrame结构
  ImageFrame中每个元素的结构如下:
    t: 时间戳
    姿态R:R_{c_0}^{b_i}，即枢纽帧c0到当前第i帧对应的imu之间的旋转姿态变换   (从下到上，从右到左读取)
    位置t:t_{c_0}^{b_i}，即枢纽帧c0到当前第i帧对应的imu之间的平移位置变换   (从下到上，从右到左读取)
    map<int, image>: it表示当前帧中特征点的索引, image表示对应特征点信息(xyz-uv-velocity)
    IntegrationBase: 当前帧的预积分信息
    is_key_frame: 当前帧是否为关键帧
*/
     // all_image_frame[Headers[i].stamp.toSec()]：表示从all_image_frame中通过时间戳取出关键帧
        Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;  // 取出关键帧的旋转
        Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;  // 取出关键帧的平移
        // 将对齐后KF的位姿赋给滑窗中的值
        Ps[i] = Pi;  // 枢纽帧c0到第i帧对应imu之间的旋转
        Rs[i] = Ri;  // 枢纽帧c0到第i帧对应imu之间的平移
        all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
    }
    
    VectorXd dep = f_manager.getDepthVector();  // 根据有效特征点数初始化这个动态向量
    for (int i = 0; i < dep.size(); i++)
        dep[i] = -1;    // 深度预设都是-1，其目的不在于获取逆深度，而是为了获取有效地图点的数目
    f_manager.clearDepth(dep);  // 特征管理器把所有的特征点逆深度也设置为-1
    
    Vector3d TIC_TMP[NUM_OF_CAM];  // 设置临时的平移外参变量
    for(int i = 0; i < NUM_OF_CAM; i++)
        TIC_TMP[i].setZero();  // 将每帧的初始平移外参设置为0  
    ric[0] = RIC[0];        // 获取初始的旋转外参
    f_manager.setRic(ric);  // 将初始的旋转外参赋给特征点管理器
    // 多约束三角化所有的特征点，注意，仍是带尺度模糊的
    f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
    
    double s = (x.tail<1>())(0);  // 取出尺度s(前面已求出)
    // 基于计算出的陀螺仪零偏Bgs[i]，重新计算滑窗中的预积分
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
   
   
        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
    }
    // 下面开始把所有的状态对齐到第0帧的imu坐标系  VIO的初始化是以第0帧作为平移的起始和0yaw角
    for (int i = frame_count; i >= 0; i--)
/*
  twi: 枢纽帧w到第i帧在imu坐标系下的平移  tw0: 枢纽帧w到第0帧在imu坐标系下的平移
  t0i: 第0帧到第i帧在imu坐标系下的平移, 也据说说将滑窗中所有帧的平移都对齐到第0帧
  t_{0i} = t_{wi} - t_{w0} = (st_{wc} - R_{wi}t_{ic}) - (st_{wc} - R_{w0}t_{0c})
*/
        Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);  
    int kv = -1;
    map<double, ImageFrame>::iterator frame_i;
    // 把求解出来KF的速度赋给滑窗中
    for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
    {
   
   
        if(frame_i->second.is_key_frame)  // 只有遍历到关键帧时，才进行如下处理
        {
   
   
            kv++;
            // v_{b_i}^{b_i} * R_{c_0}^{b_i} = v_{c_0}^{b_i}
            // v_{c_0}^{b_i}: 表示第i个关键帧在枢纽帧c0下的速度
            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
        }
    }
    // 把尺度模糊的3d点恢复到真实尺度的相机坐标系下
    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;
        it_per_id.estimated_depth *= s;  // 将特征点的深度 乘上 尺度，以恢复到真实尺度下
    }
    
    // 所有的P V Q全部对齐到第0帧的，同时和对齐到重力方向
    // 将枢纽帧对齐到真正的世界坐标系下, 得到枢纽帧相对于世界坐标系的旋转矩阵(旋转矩阵的yaw是0)
    // g是前面调整过的枢纽帧下的重力加速度，得到R_w_j， 对应的枢纽帧的yaw为0
    Matrix3d R0 = Utility::g2R(g);  
    // Rs[0]实际上是R_j_0  R0 * Rs[0] = Rw0
    double yaw = Utility::R2ypr(R0 * Rs[0]).x();  
    // 第一帧yaw赋0, 那么枢纽帧的yaw就为-yaw
    R0 = Utility::ypr2R(Eigen::Vector3d{
   
   -yaw, 0, 0}) * R0;  
    g = R0 * g;
    // Matrix3d rot_diff = R0 * Rs[0].transpose();
    Matrix3d rot_diff = R0;
    // 之前的Ps, Rs, Vs都是基于枢纽帧的，乘上R0后都对齐到重力方向下
    for (int i = 0; i <= frame_count; i++)
    {
   
   
        Ps[i] = rot_diff * Ps[i];
        Rs[i] = rot_diff * Rs[i];   // 全部对齐到重力下，同时yaw角对齐到第一帧(滑窗的起始帧)
        Vs[i] = rot_diff * Vs[i];
    }
    ROS_DEBUG_STREAM("g0     " << g.transpose());
    ROS_DEBUG_STREAM("my R0  " << Utility::R2ypr(Rs[0]).transpose()); 

    return true;
}

获取有效地图点的数目getFeatureCount

/**
 * @brief 得到有效的地图点的数目
 * 
 * @return int 
 */
int FeatureManager::getFeatureCount()
{
   
   
    int cnt = 0;
    for (auto &it : feature)  // 遍历所有的特征点
    {
   
   
		// 表示当前特征点it能够被滑窗中的几帧所看到
        it.used_num = it.feature_per_frame.size();  
        // 若当前特征点it能够被滑窗中不少于2帧所看到，且所被看到的第一帧要在滑窗中倒数第3帧之前
        if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)
        {
   
   
            cnt++;  // 则表明当前特征点为有效特征点，故有效特征点个数+1
        }
    }
    return cnt;
}