双目加IMU的初始化

转载自:https://blog.csdn.net/hltt3838/article/details/109514591
备用查看

---

理论部分
对陀螺仪bias的校正

代码部分

        if(STEREO && USE_IMU)
        {
            //有了深度之后就可以进行PnP求解
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            
            // 双目三角化
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            
            if (frame_count == WINDOW_SIZE)
            {
                map<double, ImageFrame>::iterator frame_it;
                int i = 0;
                for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
                {
                    frame_it->second.R = Rs[i];
                    frame_it->second.T = Ps[i];
                    i++;
                }
                solveGyroscopeBias(all_image_frame, Bgs);
                for (int i = 0; i <= WINDOW_SIZE; i++)
                {
                    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
                }
                optimization();
                updateLatestStates();     //让此时刻的值都等于上一时刻的值,用来更新状态
                solver_flag = NON_LINEAR;
                slideWindow();
                ROS_INFO("Initialization finish!");
             }
          }