GPS posePrior and Wi-Fi generated Range-Only constraints with g2o

[MathieuNass]

Hi Mathieu Labbe,

Great work on RTAB-map, because of you I have a very interesting subject for my MSc. thesis, namely "Adding GPS and range only Wi-Fi based constraints to a graph-based visual 2D SLAM algorithm". So, thank you very much! The plan for my thesis is to have an active Wi-Fi sniffer generate 1d constraints (e.g. distance from current pose) for RTAB-MAP in order to determine the perceived locations of the routers. These locations could in turn be used for localisation using Wi-Fi only. Furthermore, it could speed up loop-closure detection because MAC-addresses disambiguate similar locations, comparable to the implications of GPS or april tags. As specified in the title everything will be done in 2D space as, for me, this is complicated enough already.

I'll ask my questions first and list my approach after:

• For the GPS you've already provided me with a lot of information on the forums and the wiki but it does not work as I expected it to work. When using g2o, I inspected the data that is given to optimize or optimizeBA functions in optimizerG2O.cpp and never did I observe a link of type kPosePrior. How is or can GPS be incorporated in the graph optimisation?

• Is adding another typeName to the enumerator of link enough for the loop-closure process to consider these links as traversible when selecting keyframes?

• In the end, I would like RTAB-map to be able to add range-only constraints to the mapGraph for optimisation and keyframe selection (e.g. traversible by the function that uses RGBD/ProximityMaxGraphDepth). Can I please have your opinion on the planned approach (below)? Do you think that if I implement these changes as described I will be able to achieve the aforementioned goal?

The approach: I built g2o and rtabmap from source and implemented my range-only constraint in g2o, the levenberg solvers are able to handle these very well. Now I need to integrate this in rtabmap so it can be used in the optimisation process as well as in the loop closure process.

The additions/changes I've planned:

• In rtabmap/sensordata: Make range only landmarks available, e.g. make fields for from_id, to_id, measurement, covariance.
• In rtabmap_ros/corewrapper: I will add a subscriber to the a topic that posts mac-adresses with associated RSS with a call back function
• In rtabmap_ros/corewrapper: the callback function will set the sensordata fields with the data extracted from the message. A emperically found kernel function will be used to generate a measurement and covariance from the RSS.
• In rtabmap/link: Add new typeName to accomodate my rangeonly constraint
• In rtabmap/rtabmap::Process: Add links between poses and landmarks using the Mac-adresses as Id's with links as specified from the sensor data in order to be able to optimise in g2o.
• In rtabmap/optimizerG2O: When the new link typeName is detected, generate a range-only constraint according to the fields in the link object.

Thanks again for the continued amazing work, I hope that I will be able to contribute something to SLAM research this way.

Best regards, Mathieu Nass

[matlabbe]

It is a great idea to add Wifi constraint in the graph. I think you could use landmark links directly. Which kind of g2o edge are you using for range-only constraints? You could make use of the covariance to detect which kind of landmark is used. I do this already to differentiate between landmarks XY and landmarks XY+Yaw. If the Yaw covariance is >=9999, then I add a XY constraint. In your case, if you set covariance of Y to >= 9999 too, assuming only X value is valid, then you know you should use range-only constraint. Note that in OptimizerG2O, the type of link is not used, the covariance is used to switch between appropriate links.

Adding correct pose for the landmark: https://github.com/introlab/rtabmap/blob/3505611fb57c80da870f11545416293e08ecf7be/corelib/src/optimizer/OptimizerG2O.cpp#L387-L412 Adding corresponding link type: https://github.com/introlab/rtabmap/blob/3505611fb57c80da870f11545416293e08ecf7be/corelib/src/optimizer/OptimizerG2O.cpp#L575-L616

To use landmark links, the id of the mac address would need to be negative.

For your previous questions:

For the GPS you've already provided me with a lot of information on the forums and the wiki but it does not work as I expected it to work. When using g2o, I inspected the data that is given to optimize or optimizeBA functions in optimizerG2O.cpp and never did I observe a link of type kPosePrior. How is or can GPS be incorporated in the graph optimisation?

GPS is used as prior here: https://github.com/introlab/rtabmap/blob/3505611fb57c80da870f11545416293e08ecf7be/corelib/src/Memory.cpp#L5202 then normally the prior links will show up in optimize(). Optimizer/PriorsIgnored should be false to be actually used in optimization.

Is adding another typeName to the enumerator of link enough for the loop-closure process to consider these links as traversible when selecting keyframes?

Not sure what you mean by "traversible when selecting keyframes?" You can experiment with Apriltags to see what happens with landmark links.

In the end, I would like RTAB-map to be able to add range-only constraints to the mapGraph for optimisation and keyframe selection (e.g. traversible by the function that uses RGBD/ProximityMaxGraphDepth). Can I please have your opinion on the planned approach (below)? Do you think that if I implement these changes as described I will be able to achieve the aforementioned goal?

RGBD/ProximityMaxGraphDepth is used in proximity detection. The landmark links should be used (to be verified) to select nodes to compare too. For loop closure detection, if you want to limit the radius like with the GPS, is it done here before computing the likelihood. There could be a filtering of the signatures to keep only those inside a radius around a wifi hotspot detected in the current signature.

cheers, Mathieu