ORB-SLAM3源码阅读笔记(8)-LocalMapping

在LocalMapping.cc这个文件中主要做了两件事，一是局部关键帧和地图点的优化，二是imu的初始化。
根据ORB-SLAM3论文和代码，imu的初始化一共有三个阶段：

• 纯视觉的MAP估计。系统运行2s，地图中大概有10个关键帧和几百个地图点时，进行一次纯视觉的局部BA，优化关键帧位姿和地图点。
• 仅imu的MAP估计。估计imu的速度、bias(初始化过程中假设不变)、重力方向旋转矩阵Rwg(将第一帧相机确定的世界坐标系的负z轴旋转到与重力方向重合的方向)、尺度s。通过预积分构建imu残差，利用图优化的方法估计状态量。
• 视觉imu联合MAP估计。将视觉残差和imu残差联合起来，进行全局优化，优化相机位姿、imu速度、bias、地图点。

1、LocalMapping线程流程

2、InitializeIMU( )

算法流程：

• 首先检查地图中的关键帧数量和已运行时间，关键帧大于10帧，时间大于1s(非单目)/2s(单目)。
• 计算速度V和重力方向cvRwg的初始值。

if (!mpCurrentKeyFrame->GetMap()->isImuInitialized())//imu还未初始化
    {
        cv::Mat cvRwg;//世界坐标系(第一帧相机)到重力方向的旋转矩阵
        cv::Mat dirG = cv::Mat::zeros(3,1,CV_32F);//重力方向的估计值(在经过cvRwg变换前的坐标系下)
        for(vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++)
        {
            if (!(*itKF)->mpImuPreintegrated)//没有预积分，跳过
                continue;
            if (!(*itKF)->mPrevKF)//没有前一帧，跳过
                continue;
            dirG -= (*itKF)->mPrevKF->GetImuRotation()*(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();//世界坐标系下速度负值累加
            cv::Mat _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT;//平均速度
            (*itKF)->SetVelocity(_vel);//设置优化前速度初始值
            (*itKF)->mPrevKF->SetVelocity(_vel);//设置优化前速度初始值
        }
        dirG = dirG/cv::norm(dirG);//归一化，只取方向，???不明白为什么这个可以作为重力方向的估计值？？？
        cv::Mat gI = (cv::Mat_<float>(3,1) << 0.0f, 0.0f, -1.0f);//重力方向(在经过cvRwg变换后的坐标系下)
        cv::Mat v = gI.cross(dirG);//两个单位向量叉乘，等到垂直于这两个向量的向量v
        const float nv = cv::norm(v);//取模
        const float cosg = gI.dot(dirG);//两个单位向量点乘，得到夹角的cos
        const float ang = acos(cosg);//gI和dirG的夹角
        cv::Mat vzg = v*ang/nv;//gI到dirG之间的旋转向量
        cvRwg = IMU::ExpSO3(vzg);//指数映射，旋转向量->旋转矩阵
        mRwg = Converter::toMatrix3d(cvRwg);
        mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs;
    }
    else//imu已初始化(InitializeIMU在VIBA1和VIBA2中也会调用，那时候imu是已经初始化一次了)
    {
        mRwg = Eigen::Matrix3d::Identity();//直接设为单位阵
        mbg = Converter::toVector3d(mpCurrentKeyFrame->GetGyroBias());
        mba = Converter::toVector3d(mpCurrentKeyFrame->GetAccBias());
    }

进行纯imu的BA。

Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);

上一步估计出了尺度s和重力方向Rwg，然后矫正位姿、速度和地图点的尺度，并将位姿、速度、地图点进行旋转，使原来的坐标系的负z轴与重力方向重合，这样imu数据变换到world坐标系之后可以直接减去重力g。

void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScaledVel, const cv::Mat t)
{
    unique_lock<mutex> lock(mMutexMap);
    // Body position (IMU) of first keyframe is fixed to (0,0,0)
    cv::Mat Txw = cv::Mat::eye(4,4,CV_32F);
    R.copyTo(Txw.rowRange(0,3).colRange(0,3));//R就是Rwg
    cv::Mat Tyx = cv::Mat::eye(4,4,CV_32F);
    cv::Mat Tyw = Tyx*Txw;//Tyw就是世界坐标系到z轴对齐重力方向坐标系的变换矩阵，其实就是施加了Rwg旋转
    Tyw.rowRange(0,3).col(3) = Tyw.rowRange(0,3).col(3)+t;
    cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3);
    cv::Mat tyw = Tyw.rowRange(0,3).col(3);
    //遍历所有关键帧，乘上尺度、对齐z轴
    for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(); sit!=mspKeyFrames.end(); sit++)
    {
        KeyFrame* pKF = *sit;
        cv::Mat Twc = pKF->GetPoseInverse();//关键帧相机到世界的变换矩阵
        Twc.rowRange(0,3).col(3)*=s;//平移向量乘上尺度
        cv::Mat Tyc = Tyw*Twc;//变换到z轴对齐后的坐标系，现在相当于相机到新世界坐标系的变换
        cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F);//新世界坐标系到相机坐标系的变换
        Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t();
        Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3);
        pKF->SetPose(Tcy);//更新关键帧位姿
        cv::Mat Vw = pKF->GetVelocity();
        if(!bScaledVel)
            pKF->SetVelocity(Ryw*Vw);
        else
            pKF->SetVelocity(Ryw*Vw*s);//速度也旋转到新的坐标系，再乘上尺度
    }
    for(set<MapPoint*>::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++)
    {//所有地图点都旋转到新的世界坐标系，并乘上尺度
        MapPoint* pMP = *sit;
        pMP->SetWorldPos(s*Ryw*pMP->GetWorldPos()+tyw);
        pMP->UpdateNormalAndDepth();
    }
    mnMapChange++;//地图改变次数+1
}

• UpdateFrameIMU( )。1.更新tracking线程中存储的相对位姿mlRelativeFramePoses(每一帧相对于参考关键帧的位姿)。2.更新当前帧和上一帧的位姿(感觉这一步在这里不是必需的，不过UpdateFrameIMU()在其他地方也有调用，所以可能这一步在其他地方用到)。
• 将所有关键帧的bImu标志位置为true。
• 进行全局BA：FullInertialBA( )
• imu初始化完成，将标志位置为true：mpAtlas->SetImuInitialized()。

3、ScaleRefinement( )

此函数用于在初始化完成之后每隔10s优化一次尺度和重力方向。

void LocalMapping::ScaleRefinement()
{
    if (mbResetRequested)
        return;
    // Retrieve all keyframes in temporal order
    //感觉取出这些关键帧没啥用
    list<KeyFrame*> lpKF;
    KeyFrame* pKF = mpCurrentKeyFrame;
    while(pKF->mPrevKF)
    {
        lpKF.push_front(pKF);
        pKF = pKF->mPrevKF;
    }
    lpKF.push_front(pKF);
    vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
    while(CheckNewKeyFrames())
    {
        ProcessNewKeyFrame();
        vpKF.push_back(mpCurrentKeyFrame);
        lpKF.push_back(mpCurrentKeyFrame);
    }
    const int N = vpKF.size();
    mRwg = Eigen::Matrix3d::Identity();//重力方向初始值
    mScale=1.0;//尺度初始值
    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    //这个函数和前面纯imu初始化的函数同名，但是参数不同，这里只优化尺度和重力方向，其他都设置为固定fixed
    Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale);
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    if (mScale<1e-1) // 1e-1
    {   //尺度太小
        cout << "scale too small" << endl;
        bInitializing=false;
        return;
    }
    // Before this line we are not changing the map
    unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    if ((fabs(mScale-1.f)>0.00001)||!mbMonocular)
    {   //利用优化的尺度和重力方向更新关键帧位姿、imu速度、地图点
        mpAtlas->G更新etCurrentMap()->ApplyScaledRotation(Converter::toCvMat(mRwg).t(),mScale,true);
       //更新tracking线程中保存的相对位姿
        mpTracker->UpdateFrameIMU(mScale,mpCurrentKeyFrame->GetImuBias(),mpCurrentKeyFrame);
    }
    std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
    for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
    {  (*lit)->SetBadFlag();
        delete *lit;
    }
    mlNewKeyFrames.clear();
    double t_inertial_only = std::chrono::duration_cast<std::chrono::duration<double> >(t1 - t0).count();
    mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex();
    return;
}