Reset origin

[weidongguo]

I know that ORB_SLAM makes the first keyframe's pose as the origin of its coordinate system.

Right now, I want to reset the origin to be the current pose of the drone/camera whenever I press a key.

To do so, I made a function called System::Recenter() and trigger it whenever I press a key.

Let M = mTracker->mCurrentFrame.mTcw = the inverse of the current camera's transformation relative to world

Let keyFrames = mpMap -> GetAllKeyFrames());
Let mapPoints =mpMap -> GetAllMapPoints()

In System::Recenter(),

1. I transform all the mapPoints with M
2. transform the pose of each keyframe with M.

After calling System::Recenter(), three scenarios can possible happen.

1. the translation part of the camera's pose successfully goes to (0.000xxx, 0.000yyy, 0.000zzz) immediately.

2. What happened in (1) doesn't happen immediately. It takes like 5~10 seconds.

3. Lose Track. Relocalization won't be able to recover the tracking.

My questions is, why scenario 2 and 3 can possibly happen?

---

Remark:
A) The first thing I do in System::Recenter() is to stop the LocalMapper
and the last thing is to release it.

B) Under Scenario 1 and 2, the reading of the camera's pose is indeed close to (0, 0, 0) after moving the drone around and then put it back to where I set as the origin in the physical world.