Merging SD-SLAM and Rosify Difodo IV: A new SD-SLAM
Some time pase from the last entrie, but the development has keep going. And now I can finally say that the integration is almost complete.
Last steps towards integration
With the new dataset the chance to test the new SD-SLAM with DIFODO integration starts. Before, just for testing purposes I use DIFODO when a certain frame was reach. The system will update odometry and keep working but SD-SLAM had finish and wont recover when we tried to came back.
First thing first, I had to choose when to use DIFODO. In the latest post I also discussed this and I got to the conclusion that when SD-SLAM tracking fails is when DIFODO will start working. Adding this to the code I could get DIFODO to start estimating the odometry as soon as the tracking from SD-SLAM failed.
But, once the texture or light was recover it will just simply keep working on DIFODO so no recover to the original ORB tracking from SD-SLAM was possible.
So now the next thing to do was choose a good metric to came back from DIFODO when the texture on the images was good enough. For this I had to change the state machine of the original algorithm.
The state machine of the original version of SD-SLAM was:
The system will initialized if the image had enough keypoints and it will add this frame as the first keyframe in the map and start the tracking of ORB. The system can fail tracking with ORB, this can happen when the texture information is not enough like in the cases where there is almost no light. If the tracking fails the systems changes to LOST state and tries to relocalize. Relocalization is a process where the system doesnt track anymore and compares each new image with previous saved keyframes trying to see if the scene is actually seeing has been seen before and is saved as a keyframe. If the system sees a scene that was seen before when the tracking was working it will relocalize and start trakcing again. But if the camera never return over a known place localization wont be estimated again.
Integrating DIFODO not only allows to “see” on the dark or when there is no texture information but also will allow to track continously and recover independently of having seen a place before. Since there wont be need for a LOST state since now SD-SLAM will never be lost it wont have to relocalize. Dont mistake relocalization with loop closure, although both works similar relocalization is a word used when tracking is lost and it sees a known place to start tracking again while loop closure is used when a place is seen again but during the tracking. Loop closure allows to reduce the error of the tracking that is made on each estimation, reduces the drift of the odometry and also helps to create a more accurate map by adjusting previous positions or keyframes.
Now instead the new state machine for SD-SLAM will be:
The main problem here was, how to recover from DIFODOTracking, under which condition. The answer is clear, just as soon as the image recovers enough texture to track with ORB-SLAM. While this is easy to say is a bit more complicated to do.
There can be texture on an image but it may not be enough to extract keypoints, for example an image full of lines. ORB tracking uses ORB which uses FAST algorithm to extract the keypoints. So the best way to be sure that there are enough points to start tracking again with ORB is infact to extract ORB keypoints. For SD-SLAM the extraction of keypoints is done for each image before the tracking. So, even if we are not tracking with ORB the keypoints are being extracted for each image. This will be seen like a computational waste but ORB keypoints are being used for SLAM algorithms due to the high speed when it comes to extract them.
In order to came back from DIFODO tracking a method similar to the initialization has been created. This method is done after DIFODO tracking and checks if the amount of keypoints in the image is enough. The amount of points used is the same amount of points that has been used to initialize ORB in the first place so it should work as if the algorithm was just started. A new keypoint will be created and the state will change to OK. With the next frame the tracking using ORB will be attempted and if this is completed succesfully the algorithm will keep tracking with ORB again.
New results
Now that the integration seems to be complete, is time to start testing the algorithm with the dataset that I have previously created. This dataset was created specifically to test this algorithm since there are no existing dataset that allows to test this due to the absense of algorithms that can handle this situations.
light_shutdown_ends_with_light_relocalization.bag
On this video, SD-SLAM has to manage fast motion that blurs the image (making it difficult to find keypoints) and also the lights turns off making impossible localize with visual information:
SD-SLAM youtube VIDEO - Light Shutdown and Fast Motion
several_textureless_relocalizations_changes.bag
I create this new rosbag since I didnt have one similar in the previous recorder sequences. This video sequence will be useful to test several relocalizations or better said loop closure when coming back from a textureless environment. In this scene the texture is loss several times, making the algotrithm change to DIFODO tracking and then the camera returns to a previously seen view. Is used to test that the loop closure is being correctly applied to all the keyframes. Here is a video to see how the algorithm performs in the difficult scenario.
SD-SLAM youtube VIDEO - Textureless views
The version used to generate this videos can be found here: SD-SLAM v1.0.1
Next Steps
As can be seen in the video on the second 00:20, there is a big jump in the estimated position just before changing to DIFODO. This is due to the fact that the tracking dont fails soon enough, in an attemp to keep tracking visually, but the keypoints matched are not enough to give a good estimation with ORB tracking. Around 30 keypoints were matched only in this case. This can be seen in more cases and also when the light is suddenly shut down the appearance of noisy points (Keypoints) in the map can be seen sometimes. A way to avoid this an change to DIFODO tracking sooner may be useful to be developed.