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已經對齊