GTSAM on multi-robot SLAM

[anyr...@gmail.com]

Hello. I am trying to use GTSAM for a multi-robot SLAM. I'm still a beginner in it as I am in GTSAM so I would really be grateful for any help.

I am trying to set up the system where each robot does not know other robot's location (I wanna start small with just two robots). So what I did is in a single factor graph, I added two priors, one at the origin for reference, and another at some arbitrary location. Then, from each prior, the robots start with its own trajectory and they eventually make inter-robot loop closures. Now, this is my question. Is it possible to optimize the graph in the same graph or do I have to make a separate new graph in which the two priors are at the known relative position?

Thank you in advance for your answer!

[Frank Dellaert]

You can create one graph but you *have* to create a betweenfactor between the two robots because GTSAM cannot optimize disconnected graphs (singular system). This can be a very loose relative pose prior, which you then replace with a true inter-loop robot closeure later. Or, you can start with two separate graphs and optimize them separately until you have a loop closure, then merge them.

Also: https://borg.cc.gatech.edu/projects/ddf-sam.html

FD

[anyr...@gmail.com]

I understand that the two separate trajectories have to be connected by a betweenfactor derived from inter-robot loop closures. But in the actual implementation using Matlab, it seemed that the priorfactor fixes the two trajectories locations. I would like to add "a very loose relative pose prior" just as you mentioned and replace it with a true relative pose prior. Is there any function available in gtsam if I were to do this using only one graph? 

Currently I am just using 

graph.add(PriorFactorPose2(symbol('x', 1), Pose2(0, 0, 0), priorModel)); % 1st Robot

graph.add(PriorFactorPose2(symbol('y', 1), Pose2(0, 20, 0), priorModel)); % 2nd Robot

where 20 is an arbitrary relative pose I gave.

Thank you!

[Frank Dellaert]

BetweenFactor