Re: Real-Time Multi Robot Map Combination

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Real-Time-Multi-Robot-Map-Combination-tp5992p5994.html

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