Real-Time Multi Robot Map Combination

classic Classic list List threaded Threaded
10 messages Options
Reply | Threaded
Open this post in threaded view
|

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?
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

matlabbe
Administrator
This post was updated on .
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
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

Jacobmo
Awesome! thanks for the quick response. I will give it a shot and see how it works.
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

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!

Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

matlabbe
Administrator
Hi,

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

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

Jacobmo
This post was updated on .
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
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

matlabbe
Administrator
Hi,

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

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

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
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

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
Reply | Threaded
Open this post in threaded view
|

Re: Real-Time Multi Robot Map Combination

matlabbe
Administrator
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