4
$\begingroup$

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!

$\endgroup$
2
  • $\begingroup$ Did you find any way to inject GT into DSO. I am also looking for some similar solutions. Thanks. $\endgroup$
    – MThomas
    Jun 13, 2019 at 7:16
  • $\begingroup$ Hello @MThomas, thank you for your message. Unfortunately, I did not achieve this during my thesis. What I did is I have exported the estimated trajectory and did fusion with GPS in an uncoupled approach. You can read more about my work and the subsequent publications here, if you are interested: master.kalisz.co . After that I started working at university and currently I am taking a deeper look at the popular VSLAM algorithms. A student will start to work on the successor (LDSO). So hopefully I can share the findings soon. At this point I am sorry to disappoint you. Kind regards. $\endgroup$
    – BlenderEi
    Jun 14, 2019 at 19:56

1 Answer 1

2
$\begingroup$

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.

$\endgroup$
2
  • $\begingroup$ Hi @felipe, Thank you very much for your detailed answer! I like it a lot, it is very clear. I will try your suggestion soon. It seems to me like you either know the code base very well or you did something similar. Do you have any projects or papers on the topic? Or do you have an implementation of a sensor data fusion which you might want to share? It would be great to learn from your experience or your publication. $\endgroup$
    – BlenderEi
    May 5, 2020 at 10:34
  • 1
    $\begingroup$ Hi, I do have investigated DSO a lot both in my master thesis and in my current work. Unforturenately I can't share your any code. It's not difficult to draw the aforementioned conclusions if you read through DSO code. In fact the author Jacob Engel had leaved developers the interface to fuse with ground truth. The FrameHessian class has two member function named getPriorZero() which is the error between current state the prior state and getPrior() which is its related infomation matrix. You can check how these two functions are called in the backend. $\endgroup$
    – felipe7
    May 6, 2020 at 7:30

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.