ORBSLAM3 --- Map::ApplyScaledRotation&Tracking::UpdateFrameIMU函数解析

目录

1.Map::ApplyScaledRotation 函数解析 -- 以融合地图为例

1.1 函数解析

2. Tracking::UpdateFrameIMU函数解析 -- 以融合地图为例

2.1 函数解析

---

1.Map::ApplyScaledRotation 函数解析 -- 以融合地图为例

1.1 函数解析

    // Step 3 利用前面计算的坐标系变换位姿，把整个当前地图（关键帧及地图点）变换到融合帧所在地图
    {
        // 把当前关键帧所在的地图位姿带到融合关键帧所在的地图
        // mSold_new = gSw2w1 记录的是当前关键帧世界坐标系到融合关键帧世界坐标系的变换
        float s_on = mSold_new.scale();
        Sophus::SE3f T_on(mSold_new.rotation().cast<float>(), mSold_new.translation().cast<float>());

        // 锁住altas更新地图
        unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);

        //cout << "KFs before empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl;
        // 队列里还没来得及处理的关键帧清空
        mpLocalMapper->EmptyQueue();
        //cout << "KFs after empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl;

        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
        //cout << "updating active map to merge reference" << endl;
        //cout << "curr merge KF id: " << mpCurrentKF->mnId << endl;
        //cout << "curr tracking KF id: " << mpTracker->GetLastKeyFrame()->mnId << endl;
        // 是否将尺度更新到速度
        bool bScaleVel=false;
        if(s_on!=1)  // ?判断浮点数和1严格相等是不是不合适？
            bScaleVel=true;
        // 利用mSold_new位姿把整个当前地图中的关键帧和地图点变换到融合帧所在地图的坐标系下
        mpAtlas->GetCurrentMap()->ApplyScaledRotation(T_on,s_on,bScaleVel);
        // 尺度更新到普通帧位姿
        mpTracker->UpdateFrameIMU(s_on,mpCurrentKF->GetImuBias(),mpTracker->GetLastKeyFrame());

        std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
    }

        这里主要是利用mSold_new位姿把整个当前地图中的关键帧和地图点变换到融合帧所在地图的坐标系下，mSold_new=gSw2w1，记录的是当前关键帧世界坐标系到融合关键帧世界坐标系的变换（活跃地图向非活跃地图的变换）。

// 恢复尺度及重力方向,设置关键帧的坐标(重力2相机) 设置地图点的坐标(世界坐标系2重力坐标系) 设置速度(世界坐标系2重力坐标系)
/** imu在localmapping中初始化，LocalMapping::InitializeIMU中使用，误差包含三个残差与两个偏置
 * 地图融合时也会使用
 * @param R 初始化时为Rgw
 * @param s 尺度
 * @param bScaledVel 将尺度更新到速度
 * @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F),缺省值默认为0,0,0
 */
void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel)
{
    unique_lock<mutex> lock(mMutexMap);

    // Body position (IMU) of first keyframe is fixed to (0,0,0)
    // y可以理解为带重力方向的坐标系  Loopclosing : T是指由当前帧坐标系到融合帧的变换
    Sophus::SE3f Tyw = T;
    Eigen::Matrix3f Ryw = Tyw.rotationMatrix();
    Eigen::Vector3f tyw = Tyw.translation();

    // 取出当前地图所有关键帧
    for (set<KeyFrame *>::iterator sit = mspKeyFrames.begin(); sit != mspKeyFrames.end(); sit++)
    {
        // 更新关键帧位姿
        /**
         * | Rw2w1  tw2w1 |   *   | Rw1c  s*tw1c  |     =    |  Rw2c     s*Rw2w1*tw1c + tw2w1  |
         * |   0      1   |       |  0       1    |          |   0                1            |
         * 这么做比正常乘在旋转上少了个s，后面不需要这个s了，因为所有mp在下面已经全部转到了w2坐标系下，不存在尺度变化了
         * 
         * | s*Rw2w1  tw2w1 |   *   | Rw1c    tw1c  |     =    |  s*Rw2c     s*Rw2w1*tw1c + tw2w1  |
         * |   0        1   |       |  0       1    |          |     0                1            |
         */
        KeyFrame *pKF = *sit;
        // 取出相机坐标系向世界坐标系的变换
        Sophus::SE3f Twc = pKF->GetPoseInverse();
        Twc.translation() *= s;

        // |  Ryc     s*Ryw*twc + tyw  |
        // |   0           1           |
        // Loopclosing :  相机坐标系 -> 世界坐标系1 -> 世界坐标系2  即把左图的帧向世界坐标系2投影
        Sophus::SE3f Tyc = Tyw * Twc;
        
        // 世界坐标系2下的相机坐标
        Sophus::SE3f Tcy = Tyc.inverse();
        // 重力坐标系向相机坐标系的转换
        pKF->SetPose(Tcy);
        // 更新关键帧速度,将视觉坐标系变换成重力坐标系乘以尺度
        Eigen::Vector3f Vw = pKF->GetVelocity();
        if (!bScaledVel)
            pKF->SetVelocity(Ryw * Vw);
        else
            pKF->SetVelocity(Ryw * Vw * s);
    }
    // mspMapPoints存储着所有的三维点
    for (set<MapPoint *>::iterator sit = mspMapPoints.begin(); sit != mspMapPoints.end(); sit++)
    {
        // 更新每一个mp在世界坐标系下的坐标
        MapPoint *pMP = *sit;
        if (!pMP || pMP->isBad())
            continue;
        // 跟重力对齐,
        pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);
        pMP->UpdateNormalAndDepth();
    }
    mnMapChange++;
}

        其实很简单，我解释一下：

        这里的是当前关键帧世界坐标系到融合关键帧世界坐标系的变换（世界坐标系1->世界坐标系2）。

        这里的是mpAtlas->GetCurrentMap()的帧，即世界坐标系1中的帧，是相机坐标系向世界坐标系1的变换。

        Tyc = Tyw * Twc，是相机坐标系到世界坐标系2的变换。

        Tcy是世界坐标系2向相机坐标系的变换，即关键帧在世界坐标系2下的坐标。

        pKF->SetPose(Tcy) 原来存储的是在世界坐标系1下的关键帧坐标，现在变成了在世界坐标系2下的坐标。

        pKF->SetVelocity(Ryw * Vw) 更改速度，原来是在世界坐标系1下的速度，现在要变到世界坐标系2下的速度。

        同样，对世界坐标系1下的地图点，也要变到世界坐标系2下：

        pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);

2. Tracking::UpdateFrameIMU函数解析 -- 以融合地图为例

2.1 函数解析

// 尺度更新到普通帧位姿
        mpTracker->UpdateFrameIMU(s_on,mpCurrentKF->GetImuBias(),mpTracker->GetLastKeyFrame());

/**
 * @brief 更新了关键帧的位姿，但需要修改普通帧的位姿，因为正常跟踪需要普通帧
 * localmapping中初始化imu中使用，速度的走向（仅在imu模式使用），最开始速度定义于imu初始化时，每个关键帧都根据位移除以时间得到，经过非线性优化保存于KF中.
 * 之后使用本函数，让上一帧与当前帧分别与他们对应的上一关键帧做速度叠加得到，后面新的frame速度由上一个帧速度决定，如果使用匀速模型（大多数情况下），通过imu积分更新速度。
 * 新的关键帧继承于对应帧
 * @param  s 尺度
 * @param  b 初始化后第一帧的偏置
 * @param  pCurrentKeyFrame 当前关键帧
 */
void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame)
{
    // 取出当前关键帧对应地图   LoopClosing:这里是指左边的活跃的地图、要融合的地图
    Map * pMap = pCurrentKeyFrame->GetMap();
    unsigned int index = mnFirstFrameId;

    // 每一帧的参考关键帧,mlpReferences作用是保存关键帧与普通帧的关系以用作位姿更新,因为我们只保存了关键帧的信息
    list <ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
    list <bool>::iterator lbL = mlbLost.begin();  // 对应帧是否跟踪丢失

    // mlRelativeFramePoses 存放的是Tcr
    // 三个变量一一对应
    // mlRelativeFramePoses用于输出位姿，因此初始化之前里面数据没有尺度，所以要更新下尺度
    // mlRelativeFramePoses存放 参考关键帧mlpReferences到当前关键帧的mcurrentFrame的Tcr
    // 主要是用于保存关键帧到普通帧的位姿
    for(auto lit=mlRelativeFramePoses.begin(),lend=mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lbL++)
    {
        if(*lbL)
            continue;

        // lRit是当前帧的参考关键帧
        KeyFrame* pKF = *lRit;

        while(pKF->isBad())
        {
            pKF = pKF->GetParent();
        }

        if(pKF->GetMap() == pMap)
        {
            (*lit).translation() *= s;
        }
    }
    // 设置偏置  LoopClosing:这里宋进来的是当前帧的IMU的偏置
    mLastBias = b;
    // 设置上一关键帧，如果说mpLastKeyFrame已经是经过添加的新的kf，而pCurrentKeyFrame还是上一个kf，mpLastKeyFrame直接指向之前的kf   LoopClosing:mpTracker->GetLastKeyFrame,这里是拿当前帧的偏置更新上一帧的偏置
    mpLastKeyFrame = pCurrentKeyFrame;
    // 更新偏置
    mLastFrame.SetNewBias(mLastBias);
    mCurrentFrame.SetNewBias(mLastBias);

    while(!mCurrentFrame.imuIsPreintegrated())
    {
        // 当前帧需要预积分完毕，这段函数实在localmapping里调用的
        usleep(500);
    }

    // TODO 如果上一帧正好是上一帧的上一关键帧（mLastFrame.mpLastKeyFrame与mLastFrame不可能是一个，可以验证一下）
    // LastFrame
    if(mLastFrame.mnId == mLastFrame.mpLastKeyFrame->mnFrameId)
    {
        mLastFrame.SetImuPoseVelocity(mLastFrame.mpLastKeyFrame->GetImuRotation(),
                                      mLastFrame.mpLastKeyFrame->GetImuPosition(),
                                      mLastFrame.mpLastKeyFrame->GetVelocity());
    }
    else
    {
        const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
        // 上一帧对应的上一关键帧的更新过后的状态 平移旋转速度 预积分(普通帧的上一个关键帧开始一直积分到当前普通帧的这段时间的预积分)
        // 对于每个普通帧存放一个预积分的量 mpImuPreintegrated 代表从其上一个关键帧开始到当前普通帧这段时间内的预积分
        // 还有一个关于普通帧的预积分 从上一普通帧到当前普通帧的预积分的量
        const Eigen::Vector3f twb1 = mLastFrame.mpLastKeyFrame->GetImuPosition();
        const Eigen::Matrix3f Rwb1 = mLastFrame.mpLastKeyFrame->GetImuRotation();
        const Eigen::Vector3f Vwb1 = mLastFrame.mpLastKeyFrame->GetVelocity();
        float t12 = mLastFrame.mpImuPreintegrated->dT;
        // 根据mLastFrame的上一个关键帧的信息（此时已经经过imu初始化了，所以关键帧的信息都是校正后的）以及imu的预积分重新计算上一帧的位姿
        // 其实就是利用一段的旋转/平移/速度(关键帧到普通帧)  *  普通帧的预积分
        mLastFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()),
                                      twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
                                      Vwb1 + Gz*t12 + Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
    }

    // 当前帧是否做了预积分
    // mCurrentFrame
    if (mCurrentFrame.mpImuPreintegrated)
    {
        const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);

        const Eigen::Vector3f twb1 = mCurrentFrame.mpLastKeyFrame->GetImuPosition();
        const Eigen::Matrix3f Rwb1 = mCurrentFrame.mpLastKeyFrame->GetImuRotation();
        const Eigen::Vector3f Vwb1 = mCurrentFrame.mpLastKeyFrame->GetVelocity();
        float t12 = mCurrentFrame.mpImuPreintegrated->dT;

        mCurrentFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()),
                                      twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
                                      Vwb1 + Gz*t12 + Rwb1*mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
    }

    mnFirstImuFrameId = mCurrentFrame.mnId;
}

        注释写的很详细，不再阐述。