《ORB-SLAM3》IMU初始化

前言：

        刚接触了IMU传感器，除了IMU预积分的相关知识外，关于IMU的另一个重要知识点就是IMU的初始化。

IMU为什么需要初始化？

        IMU初始化是为了获得重力方向和IMU零偏的初始值。有了正确的重力方向才能消除IMU预积分中加速度计关于重力的影响，得到的IMU预积分数据才能保证准确。

ORB-SLAM3中的IMU初始化

1.ORB-SLAM3刚开始运行时，IMU还没有进行初始化，此时系统运行的纯视觉模式，地图初始化也是在这个阶段完成的，但是此时的地图坐标系（世界坐标系）缺乏重力信息。

2.当带有IMU预积分数据的关键帧超过一定数量时，开始进行IMU初始化。通过前面积累的关键帧IMU预积分数据可以求得重力在世界坐标系中的方向，进而求得重力坐标系到世界坐标系的旋转矩阵的初值。

3.当得到世界坐标系到重力坐标系的旋转矩阵之后，就可以将世界坐标系的Z轴与重力方向进行对其了。随后将每个关键帧的位姿进行矫正，同时矫正地图点的位姿和IMU预积分的结果。

    if (!mpCurrentKeyFrame->GetMap()->isImuInitialized())
    {
        Eigen::Matrix3f Rwg;
        Eigen::Vector3f dirG;
        dirG.setZero();

        int have_imu_num = 0;
        for(vector::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++)
        {
            if (!(*itKF)->mpImuPreintegrated)
                continue;
            if (!(*itKF)->mPrevKF)
                continue;

            have_imu_num++;
            // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij)  //note:核心中的核心
            dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
            // 求取实际的速度，位移/时间
            Eigen::Vector3f _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;
            (*itKF)->SetVelocity(_vel);
            (*itKF)->mPrevKF->SetVelocity(_vel);
        }

        if (have_imu_num < 6)
        {
            cout << "imu初始化失败, 由于带有imu预积分信息的关键帧数量太少" << endl;
            bInitializing=false;
            mbBadImu = true;
            return;
        }

        // dirG = sV1 - sVn + n*Rwg*g*t   //note:约等于 n*Rwg*g*t
        // 归一化，约等于重力在世界坐标系下的方向 （向量：重力方向在世界坐标系下三个轴的方向的投影）
        dirG = dirG/dirG.norm();
        // 原本的重力方向 （重力在重力坐标系下的方向）
        Eigen::Vector3f gI(0.0f, 0.0f, -1.0f);
        // 求“重力在重力坐标系下的方向”与的“重力在世界坐标系（纯视觉）下的方向”叉乘
        Eigen::Vector3f v = gI.cross(dirG);
        // 求叉乘模长
        const float nv = v.norm();
        // 求转角大小
        const float cosg = gI.dot(dirG);
        const float ang = acos(cosg);
        // v/nv 表示垂直于两个向量的轴  ang 表示转的角度，组成角轴
        Eigen::Vector3f vzg = v*ang/nv;
        // 获得重力坐标系到世界坐标系的旋转矩阵的初值
        Rwg = Sophus::SO3f::exp(vzg).matrix();
        mRwg = Rwg.cast();
        mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs;
    }

 LIO-SAM中的IMU初始化

在LIO-SAM中没有单独进行IMU的初始化的操作，LIO-SAM中是通过九轴IMU的RPY值作为第一帧的初始位姿，也就是说直接通过九轴IMU的RPY值将地图坐标系的Z轴与重力方向对其。

缺点：依赖九轴IMU的精度，若九轴IMU的初始RPY值不准确，则可能导致地图初始化失败。六轴IMU没有提供RPY值，因此在传感器倾斜安装的情况下，无法初始化。

// 提取imu姿态角RPY，作为当前lidar帧初始姿态角 
        if (currentImuTime <= timeScanCur)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit,&cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);    

// 如果关键帧集合为空，继续进行初始化（也就是第一帧）
        if (cloudKeyPoses3D->points.empty())
        {
            // 当前帧位姿的旋转部分，用激光帧信息中的RPY（来自imu原始数据）初始化  
            // note：这里的RPY是由九轴imu中的磁力计提供，是imu内部实现全局位姿的计算
            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;

            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;

            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
            return;
        }