I have implemented a stable monocular slam tracking system, based on ORBSLAM2. I am trying to find a way to add real-world distance/scale to this.
At the same time, I am running a (less stable) stereo slam system.
What i would like to do is:
Using the correct scale of the stereo data, figure out the scale factor that is need to correct the mono to real-world scale, and multiply the output by this factor.
I do not want to adjust the slam algorithm, I need to take the two output streams (currently an Eigen::Vector3f for each stream) and find an algorithm that I can run for 30 seconds or so to find the scale difference, and therefore the factor that I need to multiply the mono by.
I am currently:
//Start system. //Store both values as Eigen::Vector3f; Eigen::Vector3f initialWrong; Eigen::Vector3f initialTruth; //sleep the thread for a few seconds, and move the sensors. // get the distances float stopWrong = CurrentWrong - initialWrong; float stopTruth = CurrentTruth - initialTruth; scaleFactor = stopTruth / stopWrong ;
This works, within a margin of error, but is very manual. I am looking for a more automatic / iterative way to do this that will minimize the error as much as possible.
How can I use a ground truth to scale monocular slam in real time?
Any thought or tips here would be greatly appreciated. Thanks.