0
$\begingroup$

I am trying to inject metric scale into a vslam algorithm, namely the popular ORB-SLAM2. I have read several papers(1,2) whereby people have successfully calculated scale factor from the actual height of the mounted camera from ground and 2 successive images.

Particularly, the gist of these are to correct the local map by rescaling the map by the factor s when various conditions are met.

In essence, both papers instructed to "transform key frames and 3D points to local coordinate centre, rescale by s, and then transform back to global world coordinate". I have added 2 images from the papers illustrating this scale correction.

I am not exactly certain how this rescaling should be done, and I have added my own code interpretations below. Based on my visualized output (1st image below), the rescaling definitely is not correct, as my rescaled local keyframes are no longer on the trajectory of the original keyframes.

Can someone point me to the right direction?

        cv::Mat TcwCorrected = mCurrentFrame.mTcw.clone();
        TcwCorrected.rowRange(0,3).col(3) = TcwCorrected.rowRange(0, 3).col(3) * scale / oldScale;
        mpLastKeyFrame->SetPose(TcwCorrected);
        mpLastKeyFrame->UpdateConnections();

        cv::Mat Twc = mCurrentFrame.mTcw.clone().inv();


        // This is for the KeyFrames
        for(int i = 0 ; i < (int)mvpLocalKeyFrames.size() ; i++)
        {

            cv::Mat Tic = mvpLocalKeyFrames[i]->GetPose() * Twc; //Tiw * Twc
            Tic.rowRange(0, 3).col(3) = Tic.rowRange(0,3).col(3) * scale  / oldScale;
            cv::Mat TiwCorrected = Tic * TcwCorrected;
            mvpLocalKeyFrames[i]->SetPose(TiwCorrected);

            mvpLocalKeyFrames[i]->UpdateConnections();

        }

        mVelocity.col(3).rowRange(0,3) = mVelocity.col(3).rowRange(0,3) * scale / oldScale;
        //these are for map points
        for(int i = 0 ; i < (int)mvpLocalMapPoints.size() ; i++)
        {
            cv::Mat poseHomog = cv::Mat::ones(4,1,CV_32F), pose;
            mvpLocalMapPoints[i]->GetWorldPos().copyTo(poseHomog.rowRange(0,3)); 

            poseHomog = Twc * poseHomog; //Tpc
            //float oldScale = cv::norm(poseHomog.rowRange(0,3).col(0));

            //point from current frame, transform back to global coordinate
            poseHomog.rowRange(0,3).col(0) = poseHomog.rowRange(0,3).col(0) * scale / oldScale;
            cv::Mat pGlobal =  TcwCorrected * poseHomog;
            pose = pGlobal.rowRange(0,3);

            mvpLocalMapPoints[i]->SetWorldPos(pose);
            mvpLocalMapPoints[i]->UpdateNormalAndDepth();
        }

Scale correction results enter image description here Scale correction procedure from papers enter image description hereenter image description here

$\endgroup$
  • 1
    $\begingroup$ Can't give you the correct answer right now, but what I would do is compare your unscaled trajectory against the ground truth with github.com/MichaelGrupp/evo (Sim3 alignment). This will allow you to first view how good your trajectory is and second what the scale factor should be. You can then compare that number with the number you compute and see how far off they are. Once you know your local trajectory is good you can then start figuring out where you are going wrong. $\endgroup$ – edwinem Feb 3 at 16:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.