Realtime Map Merger Improvements

[derektan1995]

Hi Mathieu,

Hope you are well. I have fully setup RTABMap on my robots and it has been working very well. I am currently trying out the RTABMap Map Merger package, and had some success with it. I would like to hear from you on how I can achieve robots pose initialization in SLAM mode, at the start of the Map Merger process.  

Map Merger Package
Map Merger Package (Github)
RTABMap Forum Post

Here is a video of the results:
Point Cloud Map Merging Video

Here is database of the results:
mapmerge.db

As you can see from the video, we have 2 robots with RTABMap SLAM nodes running. As the map is being built up, mapData message is published to the RTABMap server, which will merge the map together with the package linked above. Unfortunately, both robots will start at the origin, which results in the map looking like this:



After a loop closure is detected, both maps will be shifted to the correct location on 'map' frame. The merged map will then look like this:



I wonder if it is possible to initialize the robots to specific transforms in the global map frame right at the beginning in SLAM mode? It should look something like this for 2 robots (Assuming that the map has yet to be built):



I attempted to use the initialpose topic, but realized that it was only designed to be used in Localization mode. I also attempted to start the robot in localization mode, set their initial pose, and then use the service call to switch to Mapping mode. This didn't work either. Looking forward to hearing from you.

Thanks,
Derek

[derektan1995]

Hi Mathieu,

I have been thinking: Can we possibly use Apriltags to force a loop closure? I got some inspiration from this post.

http://official-rtab-map-forum.206.s1.nabble.com/Best-Practices-for-Quick-Automatic-Localization-td6881.html#a6899

Here are the possible steps:
1) On the Map Server, initialize a database with a few nodes containing apriltags, located at the areas where the robots starts mapping.
2) When each of the robots starts mapping, they will see the april tag and record it in their infividual mapData msg.
3) The map merger server will receive these mapData msg. Given that it is on Multi-session mode, it will correlate the apriltags from the initialized database, and those from the mapData msg from the robot, forcing a loop closure.

What you do think? Thanks!
Derek

[derektan1995]

Hi Mathieu,

Perhaps another option to consider as well. What about forcing the initialisation of the robot pose by manually shifting the first node of each robot's pose based on it's initial location? Are there any convenient helper functions for this without us breaking the functionality of RTABMap? Thanks.

Cheers,
Derek

[matlabbe]

Hi Derek,

For the first and third questions, if you want to start from different origins, offset the odometry (you could add offset parameters to your odometry node). This could be also done by adding a TF static transform with arbitrary offset as parent to odometry frame of the robot, and use this offset frame as odom_frame_id for rtabmap. Other option, if you use rtabmap's odometry, you can call service /rtabmap/reset_odom_to_pose.

For the second question, this is similar of the approach I was thinking, but this could also work with visual loop closure detection too, though less complicated with AprilTag. The map merger node would have regenerate the graph with constraints between the apriltags, or without re-optimizing the graph, find the pose of the apriltag in each graph received, then merge accordingly to their position in each map.

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

Thanks for the reply! Using the prompts you given, I played around a little and managed to make it work. I did this by using manually adding a User-Defined link (kUserClosure) between the starting poses of each robot. I tried adding a Global Loop closure (kGlobalClosure) and that works well too.

I am now trying to optimize the map merger code for speed. The final goal would be similar to the 3d map reconstruction from a single robot, where each update can be performed within the specified 1s update rate. There are 2 approaches to map merger that I have explored thur far:


Method 1: Reseting the robot's memory prior to each map merger attempt

1) Map merger node receives mapData from both robots
2) Reset rtabmap's memory
3) Add & process robot1's nodes first, then robot2's nodes, and so on.
4) Map Manager retrieves and publishes map

The map merger process here works well. This method is significantly slower because it takes a long time to process all of the mapData's nodes from the beginning for each map merger attempt. So method 2 seeks to address this.


Method 2: Gradually accumulate robots' mapData nodes without any memory reset (Code is similar to the one on the github page here)

1) Map merger node receives mapData from both robots
3) Add & process only new nodes (robot1's nodes first, then robot2's nodes, and so on).
4) Map Manager retrieves and publishes map

This method is significantly faster since it removes the need to re-process a large number of nodes. However, I am unsure why old portions of my map keeps disappearing. Old portions of the map reappears only after a loop closure is detected, but very soon after, the same issue would resurface. This was why I asked the question in this post.

Here is a video illustrating that issue: Video of Old Map Disappearing
Database file: map_merge_disappearing_old_maps.db

I wonder if there is there any way for us to prevent the issue of disappearing portions of the map? The final goal would be the 3d map reconstruction from a single robot, where each update can be performed within the specified 1s update rate. I wonder if that is possible?

Much Thanks,
Derek

[derektan1995]

Hi Mathieu,

I took some time digging deeper into the disappearing map issue mentioned above. I noticed a few observations:

1) As the map is continuously merged with time, the nodes contained in the Memory class does continue to increase. I verified by printing out all node IDs using the Memory::getAllSignatureIds method as well as the Memory::dumpMemory method.

2) The reason why the RTABMap suddenly 'forgot' old parts of the map happened here:
rtabmap::getGraph -> rtabmap::optimizeCurrentMap -> memory::getNeighborsId. Interestingly, despite the memory growing as shown in (1), memory::getNeighborsId only retrieves some of the latest nodes in memory, not all of them.

3) One possible reason could be missing link constraints between the nodes in the map. It might be due to the Rtabmap::triggerNewMap called. However, I did establish a global loop closure between the first nodes of robot 1 and robot 2. Besides, I also attempted to continuously add such loop closures as the map grew, but the map disappearance issue still persisted.

4) I attempted to artificially include all node IDs from memory Memory::getAllSignatureIds instead of using memory::getNeighborsId. However, this method did not work. I kept getting errors of GTSAM not being able to optimize graph.

I am quite loss as to why memory::getNeighborsId only retrieves some of the latest nodes in memory, not all of them. Do you happen to have insights to this matter?

Regards,
Derek

[derektan1995]

Hi Mathieu,

I wonder if you have any updates on this issue? I looked into this issue further and suspect that this line of code is causing the old map to disappear. However, if I were to remove this line of code, the map seems to be distorted.



When I view the map of the database, it seems to make sense:



Is there an alternative method that would allow us to achieve such a functionality without the map disappearing?  

Much thanks!

Regards,
Derek

[matlabbe]

Hi Derek,

I am not used to this map merger node, but what I see from the database you shared is that there are 21 sessions in it, a new session seems triggered after 2 nodes from each robot are received. Ideally, there should be only 2 sessions, one for each robot linked by one link at the beginning. Without more knowledge on that map merger node, I cannot really help more.

If you only need to see the two maps in rviz created at the same time (without adding loop closures between them online), you could start 2 rtabmap in parallel, than add a static_transform_publisher between the 2 map frames.

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

Thanks for the reply. The reason why we have 2-3 nodes per session is that we keep track and merge in only new nodes that have been added to mapData that's being published by each of the robot. This is done to speed up the merger process, because the conventional method would be to merge all nodes over and over again. Ideally I would want only 2 sessions, but I'm not sure why so many sessions were created. Maybe it's because of the trigger new map function called, yet the merged map will be messed up without it.  

A side question. Is it possible to use 3D lidar pointcloud for scan matching (libpointmatcher), and yet output the 3D map based on RGBD from the depth camera? I only see 1 single parameter called Grid/FromDepth that controls both functionality.

Regards,
Derek

[matlabbe]

Hi Derek,

Grid/FromDepth is independent of the scan matching approach. You can set Grid/FromDepth=true and subscribe_scan_cloud=true with Icp/Strategy=1 at the same time.

Trigger new map creates a new session each time it is called.

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

Thanks for your reply, as always.

matlabbe wrote

Grid/FromDepth is independent of the scan matching approach. You can set Grid/FromDepth=true and subscribe_scan_cloud=true with Icp/Strategy=1 at the same time.

Initially, I had Grid/FromDepth=false and subscribe_scan_cloud=true with Icp/Strategy=1 at the same time. I was able to output both a 2d pointcloud compressed gridmap with a 3d pointcloud octomap.

I tried changing to Grid/FromDepth=true, while keeping everything else the same. However, I now have both 2d gridmap and pointcloud octomap both derived from my RGBD camera.

I desire to have the 2d gridmap derived from my lidar pointcloud, while the 3d octomap derived from RGBD camera source. Is that possible?

Regards,
Derek

[derektan1995]

Hi Mathieu,

I've thought of a few solutions to have both 3D depth maps and 3D pointcloud maps published.

1) I can write a separate RTAB node to subscribe to the mapData published by the main RTAB node running on the robot. This separate node will have parameter FromDepth set such that it publishes the other type of map.

2) I can edit the mapmanager class to publish both types of maps that is independent of the FromDepth parameter.

What do you think?

Much thanks,
Derek

[matlabbe]

Hi,

You can use map_assembler node (which use MapsManager interface) to generate a second map with different parameters. See http://official-rtab-map-forum.206.s1.nabble.com/Setting-maximum-cloud-depth-from-cloud-map-topic-tp7393p7453.html

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

That's a great suggestion, thanks.

I recently noticed something interesting about rtabmapviz. While my robot is performing RTABMap locally on its own computer, I have rtabmapviz running on my own laptop connected to the robot in the same ROS network via WiFi. Instead of subscribing to the 3d map output (or mapData) from the robot, I could simply subscribe to its RGBD depth cloud data and observe the map reconstruction on rtabmapviz on my own laptop.

My suspicion is that rtabmapviz is subscribing to the robot's /tf topic, and renders the depth cloud at the position based on the /tf corrected by RTABMap. I find this very beneficial because I do not have to subscribe to entire pointcloud map topics that will congest my network, but instead can subscribe to lighter-weight depthcloud data at each timestep. Please correct me if my understanding is wrong.

I would like to ask if I could use RTABMapviz to construct the 3d map in the similar manner, but with multiple robots? Ideally, I would like to subscribe to multiple robot's depth cloud and /tf topics (with namespacing) and visualize everything on the same visualizer.

Regards,
Derek

[matlabbe]

Hi Derek,

It is more efficient to subscribe to mapData from rtabmapviz than the raw point clouds. The mapData contains only data (compressed) from the latest frame added to map. rtabmapviz will accumulate them (in its cache) and reconstruct the 3d map on its side. On loop closure, the mapData contains the modified graph (which is only some KB of data) and rtabmapviz re-correct the position of each cloud previously received in its cache without having to re-download data of each node.

When subscribing to camera/point cloud data with rtabmapviz, it is only used to show the current data published by the robot at odometry rate. This is very similar to RVIZ and subscribing to robot's topics.

For multi robots, rtabmapviz cannot subscribe to more than one mapData stream. You would need as many rtabmapviz than the number of robots. To show the maps in a single visualizer, try RVIZ, which can use many rtabmap_ros/MapCloud plugins in parallel, each subscribing to different mapData topic. The only (major) problem you will have is that the TF tree should contain links between the map frames of the robots to visualize them at the same time:

<node pkg="tf" type="static_transform_publisher" name="world_map_robot1" args="0 0 0 0 0 0 world robot1/map 100" /> 
<node pkg="tf" type="static_transform_publisher" name="world_map_robot2" args="15 0 0 0 0 0 world robot2/map 100" /> 

then set /world as fixed frame in RVIZ global options. In this example, the map from robot2 will appear 15 meters in front of the map of robot 1. Configured this way, the maps are independent and may overlap if robot1 moves along x axis towards map of robot2. Ideally, the world->robotX/map transforms would not be static, but they would be published by a node trying to detect loop closures between the robot and adjust the transforms when this happens.

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

Thanks for your response, as always!

matlabbe wrote

It is more efficient to subscribe to mapData from rtabmapviz than the raw point clouds. The mapData contains only data (compressed) from the latest frame added to map. rtabmapviz will accumulate them (in its cache) and reconstruct the 3d map on its side. On loop closure, the mapData contains the modified graph (which is only some KB of data) and rtabmapviz re-correct the position of each cloud previously received in its cache without having to re-download data of each node.

Interesting, I did not know that the /mapData topic only contains the lastest frame added to the map. Let's imagine a scenario where, say, the computer hosting the multi-robot RViz is unable to subscribe to a robot's /mapData topic for an extended period of time. After that duration, the computer (i.e. more specifically, the rtabmap_ros/MapCloud plugin) manages to re-subscribe to the robot's /mapData topic. Since we have not been subscribing to the earlier frames, and that we are now re-subscribing to the latest frame, will that result in a 'big hole' in the map displayed on RViz? Are there ways for the MapCloud plugin to recover the missing nodes during the period of 'loss-comms', given that the robot still have all the nodes present in its rtabmap memory?

Thanks as always,
Derek

[matlabbe]

Hi Derek,

Good question! No there was no mechanism to recover not received data in MapCloud plugin automatically, unless by clicking "Download map" action. However "Download map" will download all the map again, which may not be efficient (in terms of bandwidth and time of transmission) if we missed only some nodes. Also in some cases, when the map is big, we just cannot call "Download map" because services in ROS have a maximum of data allowed before failing.

I implemented an approach (in this commit) to ask rtabmap to republish data of some nodes. To test it, I updated MapCloud plugin to use that new feature. MapCloud will publish on /rtabmap/republish_node_data topic with ids of nodes for which it didn't cache the data yet. How it works is that rtabmap is listening on /rtabmap/republish_node_data topic, then will republish data of nodes listed in this topic during the next rtabmap update, up to max_nodes_republished parameter (default 2). The parameter max_nodes_republished is used to limit the bandwidth for each rtabmap update. I tested with demo robot mapping rosbag:

# Create a map first.

$ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
$ rosbag play --clock demo_mapping.bag

# Then restart in localization mode without downloading the map in MapCloud plugin,
# you will see clouds of the map incrementally be downloaded.

$ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false localization:=true
$ rosbag play --clock demo_mapping.bag

Issue: this approach doesn't work with rtabmapviz, as some refactoring would be needed.

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

Thanks for your reply and your elegant solution. Everything will look great on Rviz on a multi-robot map setup, even in the event of a loss-comms scenario.

I am still pondering more about my original question on how we can further improve the realtime map_merger capabilities of rtabmap. You mentioned in this post that we have to reprocess all map data from all robot's map nodes accumulated each time we want to combine them. However, this approach is simply too slow especially as the map on each robot grows large with time. From my testing, it takes about 10s per map merge after 2 robots have accumulated map nodes over 1 minute of operation. Are there any way to incrementally build the merged map similar to how the mapCloud plugin does it, as opposed to having to reprocess all nodes?  (I recently noticed ETH Zurich coming up with something similar for pointcloud merging)

Perhaps one way is to grow each robot's map in a sequential order. In our current implementation, we are processing all of robot 1's nodes first (e.g. node 1 to node 30), then robot 2's nodes (e.g. node 31 to node 60). Whenever there are new nodes coming from robot 1, we can insert them between node 30 and 31. Similar for robot 2, new nodes can be inserted beyond node 60. That way, rtabmap will not detect odometry jumps and hence cause the disappearing map issue. I wonder if this approach is feasible? If so, how do we go about adding nodes to specific positions on the pose graph? 



Looking forward to discussing more with you, as always.

P.s. A very simple implementation of map_merger node for your reference.

Regards,
Derek

[matlabbe]

Hi,

If map merger node subscribes to MapData of each robot, it can then have the graph and node data to reconstruct each map. One thing required is how loop closures are detected between maps. A naive way could be to compare a new received MapData (last node) with nodes from all the other maps. When a loop closure is detected, it keeps in cache inter map constraints. It creates an Optimizer and feed graphs of all maps with the inter constraints, thus optimizing the whole global map. It could then re-split the global graph in each individual map to send pose updates to corresponding MapCloud plugins in rviz.

To find loop closures between maps, you may also create multiple rtabmap objects inside the map merger and feed them the corresponding MapData. However, each local rtabmap will try to find loop closures and re-optimize the map, but this process has been already done on each robot (and MapData already contains this info). You may configure those rtabmap objects in appearance-based mode only and disable loop closure detection inside them, thus only the BOW vocabulary will be created. A new public function to Rtabmap object would have to be added to call the Bayes Filter and get an hypothesis with external data (descriptors that also need to be quantized to local rtabmap vocabulary).

Other way (without modifying rtabmap code) similar to what you have is to have single rtabmap object, but in appearance-based mode only (RGBD/Enabled=false) and feed it all images from all robots in any order. Rtabmap will still try to find loop closures between those images, even if it assumes that images are normally consecutive. In this appearance-based mode, rtabmap will only returns that ID A matches with ID B with high probability, but won't return a transform. The transform will have to be computed externally, using directly RegistrationVis or RegistrationIcp classes between node data of the loop closure. You would have to offset IDs of the nodes that you receive based on the robot, and use Mem/GenerateIds=false to make sure that rtabmap use same IDs that you know from the outside. Example: all nodes from robot 2 are offset by 1000, so that 1002 is the second node of map 2. For map 3, they are offset by 2000... This assumes you may never get more than 1000 nodes per map.

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

Thanks for the detailed explanation. I'm currently trying out the second approach, but along the way, I chanced upon a question not directly related to this.

It seems from your suggested approach of map merging that we do not consider the optimized poses and constraints from individual robots. This is because we only extract sensor and pose data from the mapData topic (last node). This is understandably so because the intention of this map_merger node is to collect the robots' mapData incrementally (last node), feed it into the rtabmap class object, and to introduce new constraints considering mapData from all robots.  

I am confused as to how this rtabmap class could derive new constraints. In the case of ICP links, robots typically use their lidar scans to perform ICP against the map scans. However, on the central server collecting mapData messages, we are no longer able to subscribe to any lidar topics. I'm also unsure if there is a mechanism to use the lidar scans from the mapData (last node) to derive ICP constraints in this case.

As a result, I noticed that my merged map looks skewed, possibly due to the lack of constraints. On the contrary, my individual mapData Rviz maps that are overlayed together provides a more accurate reconstructed scene. Any insights to this?

Thanks as always,
Derek