VINS_FUSION入门系列－初始化

初始化

初始化前获得IMU的起始姿态,g2r（const Eigen::Vector &g）函数。这个函数主要是为了做一个重力对齐。之后会得到一个初始化的R0, 这个R0​存入滑动窗口的世界坐标系下的旋转Rs[0],具体参考:https://blog.csdn.net/huanghaihui_123/article/details/103075107

//初始第一个imu位姿
void Estimator::initFirstIMUPose(vector> &accVector)
{
    printf("init first imu pose\n");
    initFirstPoseFlag = true;
    //return;
    //计算加速度的均值
    Eigen::Vector3d averAcc(0, 0, 0);
    int n = (int)accVector.size();
    for(size_t i = 0; i < accVector.size(); i++)
    {
        averAcc = averAcc + accVector[i].second;
    }
    averAcc = averAcc / n;
    printf("averge acc %f %f %f\n", averAcc.x(), averAcc.y(), averAcc.z());

    Matrix3d R0 = Utility::g2R(averAcc);
    double yaw = Utility::R2ypr(R0).x();
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;//另初始的航向为0
    Rs[0] = R0;
    cout << "init R0 " << endl << Rs[0] << endl;
    //Vs[0] = Vector3d(5, 0, 0);
}

双目加IMU的初始化

前面博客讲到的双目的匹配这里就可以进行三角化,然后在进行PNP求解位姿,双目的初始化和原先的单目的初始化区别还是挺大的.按原先我的理解就是直接在单目的基础上把尺度给去掉,不需要估计,不太清楚这种初始化方式和我理解的初始化方式优缺点,毕竟大佬选择了这种简单的方式.

// stereo + IMU initilization
        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::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);

                // 对之前预积分得到的结果进行更新。
                // 预积分的好处查看就在于你得到新的Bgs，不需要又重新再积分一遍，可以通过Bgs对位姿，速度的一阶导数，进行线性近似，得到新的Bgs求解出MU的最终结果。
                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!");
            }
        }

对陀螺仪bias的校正
后续的博客更新优化和滑动窗口.

单目+IMU初始化

单目和IMU的初始化和原先的VINS_MONO步骤基本一样的,这里就不具体介绍了.

// 单目初始化 monocular + IMU initilization
        if (!STEREO && USE_IMU)
        {
            if (frame_count == WINDOW_SIZE)
            {
                bool result = false;

                //有外参且当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作
                if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)//initial_timestamp设为了0
                {
                    result = initialStructure(); //视觉惯性联合初始化
                    initial_timestamp = header;   //更新初始化时间戳
                }
                if(result)//如果初始化成功
                {
                    optimization();//先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
                    updateLatestStates();
                    solver_flag = NON_LINEAR;
                    slideWindow();//滑动窗口
                    ROS_INFO("Initialization finish!");
                }
                else//滑掉一帧
                    slideWindow();
            }
        }

纯双目的初始化

就是用前面的先三角化然后PNP求解位姿

        // stereo only initilization
        if(STEREO && !USE_IMU)
        {
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            optimization();

            if(frame_count == WINDOW_SIZE)
            {
                optimization();
                updateLatestStates();
                solver_flag = NON_LINEAR;
                slideWindow();
                ROS_INFO("Initialization finish!");
            }
        }

小结

初始化的目的就是获得一个公共的尺度s和估计bias可以使两个时刻之间IMU的旋转更准。因为旋转是通过积分得到的。加速度 bias 为何没有估计? 加速度bias一般很小，不一定能估计出来，与重力向量相比，量纲很小.初始时刻求得的IMU与重力加速度的R0可能是不准的，这样会不会导致求解IMU的位姿的时候不准确呢，答案是不会的，因为当下一时刻又会重新得到一个Rn,Rn是不断进行优化的，所以初始时刻求得的R0有误差影响不大。