VINS（八）初始化

首先通过imu预积分陀螺仪和视觉特征匹配分解Fundamental矩阵获取rotationMatrix之间的约束关系，联立方程组可以求得外参旋转矩阵；

接下来会检测当前frame_count是否达到WINDOW_SIZE，确保有足够的frame参与初始化；

bool Estimator::initialStructure();

1. 保证imu充分运动，只需要考察线加速度的变化，计算窗口中加速度的标准差，标准差大于0.25则代表imu充分激励，足够初始化（这一部分在ios版本实现中被注释掉了，不知道为什么）：

    {
        map<double, ImageFrame>::iterator frame_it;
        Vector3d sum_g;
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
            double dt = frame_it->second.pre_integration->sum_dt;
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            sum_g += tmp_g;
        }
        Vector3d aver_g;
        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
        // Standard deviation of linear_acceleration
        double var = 0;
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
            double dt = frame_it->second.pre_integration->sum_dt;
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
            //cout << "frame g " << tmp_g.transpose() << endl;
        }
        var = sqrt(var / ((int)all_image_frame.size() - 1));
        //ROS_WARN("IMU variation %f!", var);
        if(var < 0.25)
        {
            ROS_INFO("IMU excitation not enouth!");
            //return false;
        }
    }

 2. 纯视觉初始化，对Sliding Window中的图像帧和相机姿态求解sfm问题：

　　a. 首先通过FeatureManeger获取特征匹配，考察最新的keyFrame和sliding window中某个keyFrame之间有足够feature匹配和足够大的视差（id为l），这两帧之间通过五点法恢复出R，t并且三角化出3D的feature point：

relativePose(relative_R, relative_T, l)

　　b. 3D的feature point和sliding window中的keyFrame的2D feature求解PnP，并且使用ceres优化：

sfm.construct(frame_count + 1, Q, T, l,
              relative_R, relative_T,
              sfm_f, sfm_tracked_points)

　　c. 所有的frame求解PnP

cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1)

3. imu与视觉对齐，获取绝对尺度

bool Estimator::visualInitialAlign()

　　a.  求解陀螺仪零偏metric scale，这里的metric scale指的是imu和sfm结果进行对齐需要的比例：

bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);

bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
    solveGyroscopeBias(all_image_frame, Bgs);

    if(SolveScale(all_image_frame, g, x))
        return true;
    else 
        return false;
}

　　b. 初始化成功，则对于imu数据需要repropogate，也就是从当前时刻开始预积分；同时通过三角化和上一步计算的scale可以获得每个feature的深度;

至此，视觉和imu已经对齐