How to properly initialize every new pose in a Visual SLAM algorithm (namely DSO)?

Question

My question is a bit specific, because it is linked to a certain algorithm. Therefore I didn't find any other solutions on how to go about this problem. If you could refer me to research papers, instructions or anything similar which is already available in regard to this problem, I would really appreciate it. Thank you in advance for reading my post and taking your time.

I am currently trying to find out how someone would go about supporting the pose estimation in a Visual SLAM algorithm, since the optimization procedure would overwrite that initial guess anyway. But what if I want to have the algorithm use the real world scale of the reconstructed camera trajectory? Suppose, I know the prefectly accurate trajectory of how the camera moved (ground truth). In my view it should be as easy as setting the new poses for every frame. Unfortunately my try to replace the camera poses with this information actually "broke" the algorithm (that is, it didn't help with the pose estimation, but lead to it failing entirely). Now, I am a bit sceptical as to whether or not this is even possible.

Let's make my question more specific:

There is an algorithm called Direct Sparse Odometry (compare: https://vision.in.tum.de/research/vslam/dso). It is not based on detecting and matching features (so called "indirect" methods) but operates on a direct comparison of pixel intensities (thus called a "direct" method). At the time of this writing the current open sourced version is tailored for monocular input videos / image sequences. You can see that there are actually three parts that are central to initializing the new pose:

1.) CoarseInitializer::setFirst()

This is called once as tracking of the camera movement starts.

Source: https://github.com/JakobEngel/dso/blob/master/src/FullSystem/CoarseInitializer.cpp#L771

2.) CoarseInitializer::trackFrame()

This is called for the first few frames to initialize the scene based on the beginning of the recorded sequence. To me it seems like it is used to get the rotation and scaling of the cameras right at the beginning. This procedure uses a high amount of points / pixels to estimate the initial configuration.

Source: https://github.com/JakobEngel/dso/blob/master/src/FullSystem/CoarseInitializer.cpp#L114

3.) CoarseTracker::trackNewestCoarse()

After initialization is completed (around 10 frames), the camera pose is being tracked across the sequence. However, the number of points used to track the camera is reduced signifcantly to speed up the tracking. This method is executed in a loop until the end of the recording.

Source: https://github.com/JakobEngel/dso/blob/master/src/FullSystem/CoarseTracker.cpp#L556

Question:

How would one incoorporate the ground truth in this algorithm? Or in general: Is replacing initial poses (basically the lines in the source code where the default constructor is called to define a pose) a good idea or does it need to be more sophisticated than that? Maybe it is even possible to get a measure of how well the estimation is (something like a hessian for the poses and not for the individual points) in order to compare, but let's clarify if the fundamental basics for such a comparison can be laid out first. ;)

Any input is highly appreciated. Thank you!

Answer 1

DSO initializes the scene and camera poses with a specific scale factor such that the average inverse depth of the pointHessians is one. After the initialization the first two frameHessians are led into the backend to do a bundle adjustment like optimization in which, however, the previous determined scale can change (because the absolute scale is not observale in a monocular VO). After the initialization the scale should stay constant but still unknown. If you manually set new frame poses with the GT as predicted values for the trackNewCoarse function, the scales are not consistant. That's why your system gets lost right after the initialization.

If you want do the fusion in a tightly coupled way, you need do following things: 1. Determined the absolute scale after the initialization. 2. Incorperate ground truth poses into the backend optimization, that is, add residuals to the objective function which can be simply defined as the SE3 error between the estimated pose and ground truth pose. For the implementation, you need to add extra quantities to the HFinal_top and bFinal_top matrices. 3. Incorperate ground truth poses into the frontend coarse tracking like in the backend.