How to set initial pose in ORB-SLAM3 Mono

[ProKaroly]

Does anyone have experience with setting the pose of the initial keyframe?

I'm attempting to translate the entire map using a translation vector. I've incorporated this translation during the MonocularMapInitialization process and have also translated the initial MapPoints. However, after the translation, the Tracking process fails. I suspect that the TrackReferenceKeyFrame() function, which estimates the pose of the third frame, might be encountering inaccuracies in the estimated pose, leading to the "Fail to track" error.

This is how I modified the initialization:

void Tracking::MonocularInitialization()
{

    if(!mbReadyToInitializate)
    {
        // Set Reference Frame
        if(mCurrentFrame.mvKeys.size()>100)
        {

            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
                mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;

            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

            if (mSensor == System::IMU_MONOCULAR)
            {
                if(mpImuPreintegratedFromLastKF)
                {
                    delete mpImuPreintegratedFromLastKF;
                }
                mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
                mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;

            }

            mbReadyToInitializate = true;

            return;
        }
    }
    else
    {
        if (((int)mCurrentFrame.mvKeys.size()<=100)||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0)))
        {
            mbReadyToInitializate = false;

            return;
        }

        // Find correspondences
        ORBmatcher matcher(0.9,true);
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

        // Check if there are enough correspondences
        if(nmatches<100)
        {
            mbReadyToInitializate = false;
            return;
        }

        Sophus::SE3f Tcw;
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)

        if(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated))
        {
            for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
            {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }

            // Set Frame Poses
            //mInitialFrame.SetPose(Sophus::SE3f());
            mCurrentFrame.SetPose(Tcw);
                        // Define the desired translation vector
            Eigen::Vector3f translationVector(0.0, 0.0, 210.0); // Adjust the values as needed

            // Apply the translation to the origin
            mInitialFrame.SetPose(Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector));

            // Translate map points
            for (size_t i = 0; i < mvIniP3D.size(); ++i) {
                mvIniP3D[i].x += translationVector.x(); // Translate x-coordinate
                mvIniP3D[i].y += translationVector.y(); // Translate y-coordinate
                mvIniP3D[i].z += translationVector.z(); // Translate z-coordinate
            }

            Sophus::SE3f currentFramePose = Tcw * Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector);

            // Set the pose of the current frame
            mCurrentFrame.SetPose(currentFramePose);

            CreateInitialMapMonocular();
        }
    }
}

void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);

    if(mSensor == System::IMU_MONOCULAR)
        pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL);

    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

    // Insert KFs in the map
    mpAtlas->AddKeyFrame(pKFini);
    mpAtlas->AddKeyFrame(pKFcur);

    for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
       // Eigen::Vector3f worldPos;

        // Define the 3D point worldPos
        Eigen::Vector3f worldPos(mvIniP3D[i].x, mvIniP3D[i].y, mvIniP3D[i].z);

        // Print the coordinates of the 3D point
        std::cout << "Original point coordinates: " << mvIniP3D[i].x << ", " << mvIniP3D[i].y << ", " << mvIniP3D[i].z << std::endl;

        // // Define the desired initial translation vector
         Eigen::Vector3f initialTranslation(0.0, 0.0, 210.0);

        // // Apply the translation directly to the point
        worldPos += initialTranslation;
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap());

        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        mpAtlas->AddMapPoint(pMP);
    `}`

[basedball]

If you only want to translate the output pose returned by TrackMonocular(), you could just replace the return statement with your code:

Eigen::Vector3f translationVector(0.0, 0.0, 210.0); // Adjust the values as needed
Sophus::SE3f currentFramePose = Tcw * Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector);
return currentFramePose;

If you actually want to shift everything in the map, you could try passing a translation vector into the Frame constructor used in TrackMonocular and set the pose at the top of the constructor. Changing the end of MonocularInitialization() to:

// Set the pose of the current frame
mCurrentFrame.SetPose(mInitialFrame.GetPose() * Tcw);
CreateInitialMapMonocular();

should set the second Frame correctly, and you won't have to manually change the MapPoints. Hope this helps.

[ProKaroly]

I would like to connect the maps. So you second approach would be better, but unfortunately during initialization the Triangulation() function do not take care of the pose of the initial Frame. It estimate the relative Transformation and use it as the pose of the second Frame.

[Narek-939]

If you only want to translate the output pose returned by TrackMonocular(), you could just replace the return statement with your code:

Eigen::Vector3f translationVector(0.0, 0.0, 210.0); // Adjust the values as needed
Sophus::SE3f currentFramePose = Tcw * Sophus::SE3f(Eigen::Quaternionf::Identity(), translationVector);
return currentFramePose;

If you actually want to shift everything in the map, you could try passing a translation vector into the Frame constructor used in TrackMonocular and set the pose at the top of the constructor. Changing the end of MonocularInitialization() to:

// Set the pose of the current frame
mCurrentFrame.SetPose(mInitialFrame.GetPose() * Tcw);
CreateInitialMapMonocular();

should set the second Frame correctly, and you won't have to manually change the MapPoints. Hope this helps.

Hi thank you for your reply, I have tried second version to merge maps, but got the same when wrote as bellow

mInitialFrame.SetPose(Sophus::SE3f()); mCurrentFrame.SetPose(Tcw); CreateInitialMapMonocular();

please can you explain me this part, how to merge maps correctly, if I get recently relocalization