GPS with RTABMAP

[mahirc]

Hi there,

I recently acquired a GPS (ZED F9P) for my setup running d455+t265, on udoox86_64 board linux ubuntu 20.04 rtabmap ros melodic,

i was wondering how i can integrate the GPS into RTABMAP,
more specifically when i stream using
$ rosrun pcl_ros pointcloud_to_pcd, i would love for each .pcd file to have an accompanying gps coord (doesnt have to be in the same file)

I can't find any examples to get me started... Any pointers would be highly appreciated. I am very new to this. Thank you very much

[matlabbe]

To export each individual point cloud with GPS pose, I think you would need a custom node subscribed to clouds and GPS and save them in format you want.

If you want GPS pose saved in rtabmpa database (that can be further exported along point clouds, images of each node in the graph), you can remap your GPS topic to "gps/fix" input (of type sensor_msgs/NavSatFix) of rtabmap node like in this example.

EDIT: See also https://answers.ros.org/question/329165/using-rtabmap_ros-with-external-gps/?answer=329205#post-id-329205

cheers,
Mathieu

[flabrosse]

Hello,

I now have rtabmap use the GPS.  Is there a way to export the pointcloud as a pcd file that would be geo-referenced?  Or do I have to remember the origin (I guess when the map/pointcloud is started) and use this location in savePCDFile, somehow converting a GPS coordinate into a Eigen::Vector4f?

Cheers,
Fred

[iprob]

Hi,

Same question here, but I want to export to .obj or .ply.

[flabrosse]

I discovered the options ground_truth_frame_id and ground_truth_base_frame_id.  So using navsat_transform_node I publish a UTM frame relative to my odom frame and set the 2 options above to these.  My pose is now initialised to the current GPS coordinate (in UTM):

[ INFO] [1625644362.481017079, 289.483000000]: Initializing odometry pose to xyz=492817.406250,5527508.000000,4.477670 rpy=0.000000,0.000000,-0.001335 (from "utm" -> "odom")

However, I now get lots of:

[ WARN] (2021-07-07 08:52:43.015) OdometryF2M.cpp:529::computeTransform() Registration failed: "Too low inliers after bundle adjustment: 16<20" (guess=xyz=0.000000,0.000000,0.000046 rpy=-0.000010,0.000005,0.000002)
[ WARN] (2021-07-07 08:52:43.016) OdometryF2M.cpp:290::computeTransform() Failed to find a transformation with the provided guess (xyz=0.000000,0.000000,0.000046 rpy=-0.000010,0.000005,0.000002), trying again without a guess.

and the saved PCD file still does not have the UTM coordinates.

Any suggestions?

Cheers,
Fred

[matlabbe]

Hi,

If you export the poses in KML (assuming you are mapping with /gps/fix topic), you could get the poses in world coordinates. Exporting the point clouds are done in map coordinates. If you export in PCD, you could use "pcl_transform_point_cloud" tool to transform the point cloud using the first pose in KML.

For PLY or OBJ, you may apply a transform in CloudCompare or MeshLab.

Exporting PCD/PLY/OBJ files directly in GPS coordinates could be implemented, but I would need to have a dataset to test with.

cheers,
Mathieu

[flabrosse]

Hi Mathieu,

Thanks for the answer.  Yes, post processing the file is well possible, I was hoping that this step could be avoided.  Using the origin and orientation parameters of the PCD saving functions works, but still requires some post processing as most applications that use this won't use this info.  In fact, even PCL's conversion functions don't!  And saving as PLY directly does not save the orientation, only the origin (don't know if this is a limitation of PLY).

There are other things that are stopping me from using rtab-map for now (see other reported issues) in another thread, so for now using the ZED mapping facility but I'll come back to this later.

Cheers,
Fred