Manage previously built maps, merge maps

[youknowimcomingwhenyouhearmehumming]

Update

I couldn't reopen the original issue, so I made this new issue. I have tried both disabling the reset and also setting the appropriate time stamp, but it still fails. I have created a link with two video, theirs logs, and the used map of the first video that is loaded into the second video, so you can see the exact error yourself. I have implemented the code lines from "suggested solution" into your code. The link is: https://mab.to/ogkuUgu5z.

Let me know if you need more from me.

Thank you so much for responding to these issues - it really makes it so much more fun to work with this repository compare to other SLAM repository :)

Describe the bug

You have two videos from the same building. The only differences between the videos is the start and end are from different rooms inside this building. However, the middle part of both videos are from the exact same rooms in the building.

In one run a map is created and save from one of the videos .

Then in another run with the other video, the map is loaded into the system. The system is able to initialize as normally but right after the initialization, the system looses the feature tracking. When the video gets to the middle part, the system can recognize features from the map and feature tracking is regained. From this point the tracking and mapping module works perfectly so even when the video gets to the different end-part it keeps performing feature tracking and creating new key frames.

The value of fixing this bug

Imagine a robot that often have to move around in the same building - if it could use a previously recorded map its navigation would be much more stable. E.g. if the light in one of the usually corridors is out, the SLAM system would normally lose the tracking if the robot went trough this corridor. However, if the SLAM system had access to a previously map it could recover its location perfectly on the other side of the dark corridor.

To Reproduce

In the first run with one of the videos use "SLAM.save_map_database("map_of_stella.msg")" to save the map.

In the second run with the other video use "SLAM.load_map_database("map_of_stella.msg");" to load the map.
(IMPORTANT: do NOT disable mapping module with "SLAM.disable_mapping_module();")
You will then see the following error messages "[info] tracking lost: frame xx".

If you keep letting it run it will continue with "[info] tracking lost within 5 sec after initialization" and the whole system will reset. This resetting will delete the map you just loaded. You can deactivate the resetting but it does not help to solve the problem.

Suggested solution

I noticed a part of the problem is in the tracking_module.cc with the following lines:

  // pass all of the keyframes to the mapping module
    assert(!is_stopped_keyframe_insertion_);
    const auto keyfrms = map_db_->get_all_keyframes();
    for (const auto& keyfrm : keyfrms) {
        mapper_->queue_keyframe(keyfrm);
    }

This code loads all the key-frames from the previously map in a randomized order.

Therefore, I suggest the following code which makes sure that the key frames are loaded in the correct order:

    // pass all of the keyframes to the mapping module
    assert(!is_stopped_keyframe_insertion_);
    auto keyfrms = map_db_->get_all_keyframes();

    std::sort(keyfrms.begin(), keyfrms.end(),
              [&](std::shared_ptr<stella_vslam::data::keyframe>& keyfrm_1,
                  std::shared_ptr<stella_vslam::data::keyframe>& keyfrm_2) { return *keyfrm_1 > *keyfrm_2; });

However, this solution is not enough to solve the problem.

Environment

Hardware: [PC]
CPU: [AMD Ryzen 7 5800X 8-Core Processor]
OS: [Ubuntu 22.04]
In my case it is not necessary to processes the video in real time

[ymd-stella]

The system is able to initialize as normally but right after the initialization, the system looses the feature tracking. When the video gets to the middle part, the system can recognize features from the map and feature tracking is regained. From this point the tracking and mapping module works perfectly so even when the video gets to the different end-part it keeps performing feature tracking and creating new key frames.

I think the behavior is completely acceptable. Please describe the expected behavior. Do not ignore the issue template.

[youknowimcomingwhenyouhearmehumming]

Thanks for the fast answer :)

I would expect the following:

1. the map from the other video is loaded
2. the initialization takes place
3. the tracking continues based on the initialization
4. at some point, some of the current tracked features can be recolonized in the preloaded map. Therefore, the two maps are merged together into a combined map/point cloud. You could argue that this is a bit like loop detection with the following correction of the map.
5. the tracking and mapping continues for the rest of the video based on this combined point cloud.

Don't you agree with this procedure?

[ymd-stella]

It looks like the same feature that VINS-Mono has. This is not a bug but a lack of feature. The label and title has been changed.

[ymd-stella]

The following additional features are required

• Manage previously built maps
• Merge previously built maps with the current map

[youknowimcomingwhenyouhearmehumming]

Yes I very much agree with your take on the issue and your splitting of the task :) As said, I think the functionality in the loop detection with the following correction of the map could be re-use, when implementing this new feature. I'm very exited for this feature - do you have an idea if it's something that will be worked on in the near future or is it a bit further down the line?

[ymd-stella]

For most applications, once a map has been created with sufficient coverage, there is no need to update the map further. It is sufficient to simply do the tracking. Therefore, this feature is unnecessary.
If you think this feature is useful, please provide a more concrete example that would actually be useful.

[youknowimcomingwhenyouhearmehumming]

Thanks for your quick answer. The feature will be useful in changing environments, example:

1. warehouses where the goods are often moved around
2. Outdoor use where e.g. the tree looses their leaves in the winter time.
3. Construction sites with daily changes

[ymd-stella]

I think you are overestimating the value of this feature. It does nothing to change the performance for changing environments.

I think the impact is just to reduce a little bit of work for mapping, and I will implement it after I finish other more prioritized tasks. (Of course, contributions are welcome.)

[KwojB]

Hey @ymd-stella, is the map merging feature still in the works?