gps with heading as global_pose with GTSAM

[soldierofhell]

Hi @matlabbe,

I want to incorporate gps measurments into optimization with GTSAM optimizer (from what I've read here for g2o is not working properly?). My gps has heading (from moving baseline RTK), so I want to use /global_pose to utilize position.x, position.y and heading (yaw). I've translated sensor_msgs/NavSatFix to geometry_msgs/PoseWithCovariance with robot_localization's navsat_transform_node. My source of "features" is velodyne lidar, so I'm using icp_odometry node for odometry and poincloud input to rtabmap node (I'm using test_velodyne.launch parameters)

I think I might misunderstand the /global_pose usage. My input to /global_pose is a robot_localization' navsat_transform_node /odometry/gps with added yaw (so the frame_id=map and child_frame_id=gps=base_link). My doubts are connected with this part of code (I'm using ROS2 branch which by the way may bring additional source of "error", because it's probably not intensively tested yet) https://github.com/introlab/rtabmap_ros/blob/8ef6cff8777aa43818afe4744fe5942916f6fe40/src/CoreWrapper.cpp#L1714-L1723 What I don't understand is the sensorToBase correction.

1. I guess this makes the globaPose a frameId_=base_link offset realive to current map->base_link tf? (not a global pose in "map" frame). On the other hand according to your comment

// transform global pose from sensor frame to robot base frame

globalPose_.header.frame_id should be sensor frame, so in my case "gps"?

2. I don't understand what should be the source of globalPose_.header.frame_id -> frameId_ tf (in my case map->base_link). Should it be from rtabmap node? If so I couldn't set up this like this and I'm using map->base_link tf from navsat (I turned off map->base_link tf from rtab).

Nevertheless this the results I currently obtain (from /localization_pose) are incorrect. are kind of "random walk" not aligned with either gps or icp_odometry

BTW My end goal is to have:

1. map aligned with gps heading
2. smooth translation from gps to non-gps environment

Regards,

[soldierofhell]

In the meantime I'm digging through test_prior.launch example and from what I understand my globalPose frame_id should be base_link=gps (like "kinnect" in test_prior.launch) and map->base_link tf should come from rtabmap node (so I have to switch off robot_localization tf).

Nevertheless, any comments are appreciated

[soldierofhell]

Hurray, so far I've changed only the globalPose frame_id and the results are finally in line with gps, now I must clean up the tf tree

[soldierofhell]

@matlabbe, just one more question about "proximity detection". Am I understand correctly that pure lidar doesn't have loop closure, but only proximity detection, so the scenario that the robot will localize itself globally correct (accoriding to gps) starting in gps-denied is not possible, but scenario when the robot is loosing gps (enter non-gps zone) is available, because robot can "switch" to map localization with proximity detection?

[matlabbe]

Oh sorry, I just stumbled on this issue that I missed (I think it seems during the time I got covid the first time). I'll answer now if someone else has the same question. For the original question, I confirm that the frame_id in global_pose should be the frame of the sensor, e.g., gps frame in your case. The sensorToBase transform is to convert the pose from the sensor frame in the base frame. The global_pose is meant for ground truth pose, but can be used with RTK like you did. The map frame will then match the same frame than the RTK is referring to.

Note sure to understand the sense of this phrase "but scenario when the robot is loosing gps (enter non-gps zone) is available", but without a camera, lidar-only setup will only do proximity detection. If robot lose GPS, the proximity detection is still done based on the current estimate of the map.