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

本文链接：https://blog.csdn.net/qq_41694024/article/details/129668877

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

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

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在local