# Rescaling a local Map in VSLAM (orb slam)

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 Scale correction procedure from papers

• 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. – edwinem Feb 3 at 16:13