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

轉載于:https://www.cnblogs.com/shang-slam/p/7147095.html