Real-Time Multi Robot Map Combination

[Jacobmo]

I am working with multiple robots connected to a ground station computer. Each robot is running their own version of RTAB-Map and building their own map. I am planning on combining those maps into a single map while they are still flying. I think I have found a way to do most of it, but before I dive in too deep, I wanted to know if there is a better way to do what I want

I want to pull the keyframe images and map information from each map by either subscribing to the map graph and keyframe image topics or by calling the get_map_data service from the main computer.

Then using bag of words find new matches between the two robots and then pass those to the graph and re-optimize.
Then rebuild the map with the new combined graph.

I have found some useful functions in your code that do a lot of this. I am just wondering if you have a way that makes it easy to do this without having to replicate a lot of the functionality that already exists in RTAB map.

It doesn't have to be entirely real time and it doesn't have to send the combined maps back to the agents. But I would like to be able to view a combined map on the base station computer that updates every 30 seconds or something like that.

If possible, I would prefer to view the combined map in RVIZ.
Do you have any advice?

[matlabbe]

Good question! I have some ideas how to do this online (having rtabmap working on each robot locally while having a "rtabmap server" merging all maps together in real-time), but still not have time yet to try them.

With the current package, if real-time is not a requirement, we could have a server subscribing to rtabmap/mapData of each robot, accumulating on its side the nodes (the last NodeData in "nodes" array in the msg contain all data of the latest frame added to map) of each robot separately (similar to MapAssembler node but with a MapData callback for each map). For example, a separated list of "Signature" for each robot. A Signature can be derived from NodeData with nodeDataFromROS() function.

std::list<Signature> nodesMap1;
std::list<Signature> nodesMap2;
float odomLinearVariance = 0.0001;
float odomAngularVariance = 0.0005;
MapsManager mapsManager;

//
// Merging request (a service or done each 30 sec), process each map one after the other
//
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); 
// See example from MapAssembler to get all parameters from rosparam or arguments

// Keep everything in RAM
parameters.at(rtabmap::Parameters::kDbSqlite3InMemory()) = "true";

Rtabmap rtabmap(parameters...); 
for(std::list<Signature>::iterator iter=nodesMap1.begin(); iter!=nodesMap1.end();++iter)
{
   rtabmap::SensorData data = iter->sensorData();
   data.uncompress();
   rtabmap.process(data, iter->pose(), odomLinearVariance, odomAngularVariance);
}
rtabmap.triggerNewMap(); // start a new session for the second robot
for(std::list<Signature>::iterator iter=nodesMap2.begin(); iter!=nodesMap2.end();++iter)
{
   rtabmap::SensorData data = iter->sensorData();
   data.uncompress();
   rtabmap.process(data, iter->pose(), odomLinearVariance, odomAngularVariance);
}

// Update and publish combined map
mapsManager.clear();
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
rtabmap. getGraph(poses, constraints, true, true);
poses = mapsManager.updateMapCaches(poses, rtabmap.getMemory(), false, false);
mapsManager.publishMaps(poses, ros::Time::now(), "map_combined");

Not that if the maps are not overlapping (no loop closures can be found between the robots), only the last map will be published as the combined map. The drawback of this approach is that we need to reprocess all data each time we want to combine them.

Regards,
Mathieu

[Jacobmo]

Awesome! thanks for the quick response. I will give it a shot and see how it works.

[Jacobmo]

I am running into a few errors with your sugestion. here is my current c++ code. do you know what I am doing wrong?

this line says  no matching conversion for functional-style cast from 'std::string' (aka 'basic_string<char>') to 'rtabmap::ParametersPair' (aka 'pair<basic_string<char>, basic_string<char> >')

 // Keep everything in RAM
  parameters.at(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory())) = "true";

also what else is supposed to be in the parenthases in the next line?
rtabmap::Rtabmap rtabmap(parameters...);


This is how I have set up my mapdata callback
void MapMerger::mapCallback1(const rtabmap_ros::MapData& msg)
{
  rtabmap_ros::NodeData node{msg.nodes.back()};
  rtabmap::Signature sig{rtabmap_ros::nodeDataFromROS(node)};
  nodesMap1.push_back(sig);
}

and when I try to step through the iteratator for the signature:

for(std::list<rtabmap::Signature>::iterator iter=nodesMap0.begin(); iter!=nodesMap0.end();++iter)
    {
       rtabmap::SensorData data = iter->data();
       data.uncompress();
       rtabmap.process(data, iter->pose(), odomLinearVariance, odomAngularVariance);
    }
    rtabmap.triggerNewMap(); // start a new session for the second robot

it says "no member named 'data' in 'rtabmap::Signature'

so it isnt letting me do any of that.

I will keep looking at it to figure it out but i figured if there were quick easy changes that you saw, it would speed it up. thank you!

[matlabbe]

Hi,

Sorry, I have written it without actually compiling it. I edited the example of this post for your problems.

cheers,
Mathieu

[Jacobmo]

I applied those changes, thank you, there were a few more changes that I added in that loop.

for(std::list<rtabmap::Signature>::iterator iter=nodesMap1.begin(); iter!=nodesMap1.end();++iter)
    {
       rtabmap::SensorData data = iter->sensorData();
       data.uncompressData();
       rtabmap.process(data, iter->getPose(), odomLinearVariance, odomAngularVariance);
    }

the data->uncomrpressData() and iter->getPose() is what I had to change. I assume those are the functions that your code was supposed to call.

also the line
Rtabmap rtabmap(parameters...);

is giving me an error saying that pack expansion does not contain any unexpanded parameter packs.

I think it needs to initialized with. If i do the following, it seems to work.
Rtabmap rtabmap;
rtabmap.init(parameters,"home/jacob/.ros/combomap.db");

then the mapsManager also needs to be initialized with
mapsManager.init(nh_,nh_private_,"map_merginator",true);

I got it subscribed and running and it seems like it is outputing a pointcloud2 message type called /scan_map that I can subscribe to and it has data in it, but I cant get it to show up in RVIZ. I am not super familiar with how pointcloud2 messages work, so it could just be something I am not doing right.

is there a way to get that to output to a rtabmap/mapCloud message instead?  

Thanks,
Jacob

[matlabbe]

Hi,

Can you post the full code of the node? It will be easier to test it here.

cheers,
Mathieu

[Jacobmo]

Yes, I am pretty sure I have it working how it is supposed to.

It will successfully merge maps, it is pretty touchy but that may be because I have only tried it with simulations in gazebo, which are a little more tricky to get right.

I am planning on improving the way that it collects the nodes for the graph because as of now when it begins optimizing the combined map, the callbacks hang and they end up not getting all of the information.

The code for the node is located in this repo
https://github.com/jacobmoroni/map_merger

If you want to launch the node as it is with pre-recorded bags, you can use the bag_map_merger.launch file.
you just need to change input parameters to reflect the bags that you use.

the map_merger.launch file is designed to run the node in real time while the robots are mapping.

Thank you,
Jacob Olson

[Jacobmo]

I got the node working now, I do have a question though. Is it possible to publish the combined message as a mapData message rather than a pointcloud2 so that I can cut out the ceiling and stuff to clean up the map? the pointcloud2 message type does not allow for this

[matlabbe]

Hi, thx for sharing!

To cut off the ceiling, you could use Grid/MaxObstacleHeight parameter. You are already parsing Grid parameters, so you may only have to add to your node:

<param name="Grid/MaxObstacleHeight" value="2"/>

It could be also possible to republish a MapData topic. See how it is done here. That way the whole map data is copied. It would be more efficient to just get the graph from latest statistics (rtabmap.getStatistics()) and add signatures for only new added data like here.

cheers,
Mathieu

[ekuruvilla]

Mathieu,

Could you give some suggestions on the best way to enhance this code to perform the loop closure.

Thank you,
Eapen

[matlabbe]

RTAB-Map uses a bag-of-words approach for loop closure detection. See this paper: https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/b/bc/TRO2013.pdf

Results will also vary depending on the visual feature used. That paper used SURF. More recently, I have done some comparisons between the features for localization, but for loop closure detection it also applies: https://github.com/introlab/rtabmap/tree/master/archive/2020-IlluminationInvariant

cheers,
Mathieu

[derektan1995]

Hi Mathieu,

You mentioned this in your previous post:

matlabbe wrote

Not that if the maps are not overlapping (no loop closures can be found between the robots), only the last map will be published as the combined map. The drawback of this approach is that we need to reprocess all data each time we want to combine them.

I wonder if there is a way to publish the entire map (not just the last map) even when no loop closures have been found yet?

Thanks,
Derek

[matlabbe]

If the maps don't have constraints between them (we don't know where they are located against each other), publishing them would generate two maps on over the other, so not really useful.

[derektan1995]

Hi Mathieu,

Thanks for your reply. I agree that this feature isn't very meaningful without any constraints. However, I am interested in getting RViz publish out the individual maps overlapping each other before any loop closure is found because I am unable to guarantee a loop closure early in the mapping process. I would still like to see the rough visualisation of the maps early on in the mapping process.

This question is related to the issue I posted in this post. I experienced parts of my map disappearing in the 2nd method mentioned there, and would like to gain insights as to why it is happening. In fact, I have already defined a User-Defined loop closure (kUserClosure) between the first node of each robot's mapData prior to the map merging process for initial pose initialization, so technically a constraint has already been established.

Been trying this out for awhile now, without any luck. Is there any way I could publish out the entire 3d map without old maps disappearing?

Regards,
Derek

[matlabbe]

Follow this discussion in this thread: http://official-rtab-map-forum.206.s1.nabble.com/Realtime-Map-Merger-Improvements-tp8200p8444.html