Adding RTK-GPS as a Ground Truth For Rtabmap.

[imran05]

Hello, I am using the RTK-GPS to connect with ntrip and i  followed "https://hm02123.tistory.com/8" this link to ssetup the rtk_gps. I remapped the gps data to

 <arg name="gps_topic"                default="/ublox_gps/fix" /> 

and i also set the :

<arg name="ground_truth_frame_id"   default="world"/> 
 <arg name="ground_truth_base_frame_id" default="base_link_gt"/>  .

From the example "rtabmap_examples/launch/rgbdslam_datasets.launch" I included the below transform:

  <!-- TF FRAMES -->
  <node pkg="tf" type="static_transform_publisher" name="world_to_map"
    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />

But i am not sure where should i add the base_link_gt frmae  taht is not link with my robot tf tree.
Can anyone please help me to setup the rtk-gps as a ground truth to the rtabmap?

Thank You

[matlabbe]

The RTK would give a relative position from where it started estimating position to the GPS receiver on the robot. If you know the offset between the GPS receiver and the base frame of the robot, you can express it as a static TF /gps_frame_gt -> /base_link_gt, with RTK pose giving /world -> /gps_frame_gt. These TF tree should be independent of the robot TF tree. You can then configure the ground truth of rtabmap like you did:

<arg name="ground_truth_frame_id"   default="world"/> 
<arg name="ground_truth_base_frame_id" default="base_link_gt"/> 

The world->map is not necessary, but useful to show at the same time the two TF trees so that we can debug/visualize that base_link_gt is following base_link (assuming mapping and RTK starts both at 0,0,0).

[imran05]

Thank You for the explanation, but i am still confused for the offset i set the gps_frame_gt to 1.0m high from my base_link ans i set a ststic tramnsform in my launch file like:

<launch>
  <!-- Static Transform for GPS -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_gps" args="0 0 1 0 0 0 /base_link_gt /gps_frame_gt 100" />
</launch>

but I didn't understand "

with RTK pose giving /world -> /gps_frame_gt

 how i implement this. As I am using the ntrip_ros package and ublox_ros package so that is only publishing the gps data on

/ublox_gps/fix

. So, how i do it to make sure tahts its also giving the  /world -> /gps_frame_gt.

[matlabbe]

When using /world -> /gps_frame_gt, I would do instead:

<launch>
  <!-- Static Transform for GPS -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_gps" args="0 0 -1 0 0 0 /gps_frame_gt /base_link_gt 100" />
</launch>

EDIT unless as shown in example below, you could get a TF tree like /world -> /base_link_gt -> gps_frame_gt, then you could keep what you did.

To provide /world->/gps_frame_gt, you may use robot_localization with navsat_transform_node http://docs.ros.org/en/melodic/api/robot_localization/html/integrating_gps.html

You may look at this page: https://msadowski.github.io/ardusimple-ros-integration/, it seems you may not need input odometry or imu to make it work. He has a demo repo: https://github.com/msadowski/ardusimple_rover

<launch>
  <rosparam command="load" file="$(find ardusimple_rover)/config/ekf_ardusimple.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <!-- <remap from="odometry/filtered" to="odometry/filtered_map"/> -->
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_gps" args="0 0 0 0 0 0 /base_link /gps 100"/>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
    <rosparam command="load" file="$(find ardusimple_rover)/config/navsat_config.yaml" />
    <remap from="/imu/data" to="/gps/navheading" />
    <remap from="/gps/fix" to="/gps/fix" />
    <param name="wait_for_datum" value="false" />
  </node>

</launch>

So he feeds the odometry output of the ekf node back to navsat node. The rosbag is missing from the repo to test this, but based on the blog, it looks like what you want. You may adjust the frame names of the ekf node so that right TF is published.

cheers,
Mathieu

[imran05]

Thank you for your explanation, but I am still having trouble understanding how to use this. The EKF node is usually set up to take odometry and IMU data, but I don't want to use that data. Instead, I am using GPS as the ground truth for RTAB-Map to compare the trajectory of visual odometry with the robot's actual trajectory and calculate the RMSE. I followed your instructions and created the launch file below:

<launch>

  <rosparam command="load" file="$(find hada_navigation)/params/hada_ekf_config.yaml" />
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <!-- <remap from="odometry/filtered" to="odometry/filtered_map"/> -->
  </node>


  <!--node pkg="tf" type="static_transform_publisher" name="base_gps" args="0 0 0 0 0 0 /base_link_gt /gps 100"/-->

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
    <rosparam command="load" file="$(find hada_navigation)/params/hada_navsat_config.yaml" />
    <remap from="/imu/data" to="/gps/navheading" />
    <remap from="/gps/fix" to="/ublox_gps/fix" />
    <param name="wait_for_datum" value="false" />
  </node>

</launch>

However, I’m still not getting the TF between the world frame and gps_frame_gt. Also, I’m confused about where to change the topic in the EKF node. I tried changing it in the YAML file, but it didn’t work for me.

Here is the transform tree:

frames.pdf

Here is the rqt_graph picture :

rosgraph.svg
I would really appreciate it if you could assist me.

Actually, I am currently analyzing the RTAB-Map results generated using stereo odometry and require ground truth values to compare the accuracy and evaluate the performance of the map. If you have any alternative methods or suggestions to compare mapping accuracy, I would appreciate your guidance.

Thank You

[matlabbe]

I think ideally you would need at least a 9-axis IMU to combine with the GPS to give 6DOF ground truth poses. With only the gps_fix values, we cannot know the pitch or roll of the robot, only the yaw (bearing). However, I never did this myself, so not sure why the current solution is not working or how to correctly use an IMU if you had one.

For convenience, I updated rtabmap-export tool to be able to export gps_fix values into local coordinate system. You can then use these poses to compare with RGB-D Dataset evaluation script and get a RMSE.

Example using that loop_3it_gps.db dataset:

$ rtabmap-export --poses_gps --poses loop_3it_gps.db
Opening database "loop_3it_gps.db"...
Opening database "loop_3it_gps.db"... done (0.000245s).
Optimizing the map (Full global optimization)...
Optimizing the map (Full global optimization)... done (0.036781s, poses=299, links=301).
299 poses exported to "./loop_3it_gps_poses.txt". (min=[-61.419334,-62.237259,-0.567582] max=[7.569976,9.257481,0.736105])
299 GPS poses exported to "./loop_3it_gps_gps_poses.txt".

$ python3 evaluate_ate.py loop_3it_gps_gps_poses.txt loop_3it_gps_poses.txt  --max_difference 2 --plot figure.pdf --verbose
compared_pose_pairs 286 pairs
absolute_translational_error.rmse 5.400886 m
absolute_translational_error.mean 4.636903 m
absolute_translational_error.median 3.924598 m
absolute_translational_error.std 2.769243 m
absolute_translational_error.min 1.071700 m
absolute_translational_error.max 16.964002 m

Note that this example uses a normal GPS value with large error (>2-5 meters). With a RTK GPS, the RMSE would be smaller. One caveat is that the GPS poses correspond to GPS frame, not base_link.

Regards,
Mathieu

[imran05]

Thank you for the explanation.
My IMU is a 9-axis IMU, but it is inbuilt into the camera I am using. I will follow your guidelines.

I also have one more question: If I want to compare the GPS data with the visual odometry trajectory, but not directly within RTAB-Map, I know it to export the poses from the RTAB-Map database and manually retrieve the GPS values from the Ublox software. But I am not sure what kind of data should be to compare and calculate the errors.

Could you please advise if this is possible and what things I need to be cautious about?

[matlabbe]

In the example above, when exporting GPS poses from rtabmap database, we convert them in local coordinates so that we can use RGB-D dataset evaluation script to compare the trajectories.

To do it externally, you would need to find a way to convert the ublox GPS values into a local coordinate system in meters (6DoF). You could try using robot_localization to generate that GPS "odometry" topic using ublox and the camera's imu data.

You can record nav_msgs/Odometry output topic from RTAB-Map's odometry node. Theses poses won't contain any loop closure correction though.