Ask Your Question
0

How to Compare the SLAM trajectory with Simulation Ground Truth

asked 2019-09-19 10:07:24 -0500

al389660 gravatar image

Hello, I am working with Gazebo simulator. I have successfully created a map of the environment using rtabmap_ros. Now I want to compare (plot) the trajectory of the robot as generated by the navigation set-up with the actual position of the robot in the Gazebo.

I don't know how to export each topic separately to plot them on the same scale later for comparison purposes. please, explain the steps and topics involved to plot the comparison.

  1. how to export and plot the actual position of the robot in the Gazebo.
  2. how to export and plot the trajectory of the robot as calculated by the navigation setup.

Thank in advance

edit retag flag offensive close merge delete

Comments

You want to get ground truth to compare it to the odometry you are getting using rtabmap?

Choco93 gravatar imageChoco93 ( 2019-09-20 01:36:00 -0500 )edit

yes, I want to plot them both. After getting a goal command, the robot navigates autonomously by generating the trajectory using rtabmap_ros. I want to plot that trajectory with the actual position of the robot in Gazebo (ground Truth) to compare the accuracy of the rtabmap_ros. I just do not know how to do that and which topics are involved.

al389660 gravatar imageal389660 ( 2019-09-20 16:30:53 -0500 )edit

You can take a look at this answer. And for plotting you can use rqt_plot I guess.

Choco93 gravatar imageChoco93 ( 2019-09-21 10:21:30 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-09-22 18:43:03 -0500

matlabbe gravatar image

updated 2019-09-29 18:47:12 -0500

As @Choco93 said, you could add the following to your urdf file:

<gazebo>
    <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>50.0</updateRate>
      <bodyName>base_footprint</bodyName>
      <topicName>ground_truth/state</topicName>
      <gaussianNoise>0.01</gaussianNoise>
      <frameName>world</frameName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin>
</gazebo>

To feed ground truth to rtabmap, we need to convert the /ground_truth/state topic to TF, and change the base_footprint frame to a fake one like base_footprint_gt. This can be done with odom_msg_to_tf node:

$ rosrun rtabmap_ros odom_msg_to_tf odom:=ground_truth/state _frame_id:=base_footprint_gt
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 world map 100

The TF world -> base_footprint_gt will be published. I added world -> map static transform just for convenience, to be able to see both base_footprint and base_footprint_gt in RVIZ under world global frame. Last thing to do is to setup ground truth parameters of rtabmap node:

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="-d">
  ...
  <param name="ground_truth_frame_id" value="world"/>
  <param name="ground_truth_base_frame_id" value="base_footprint_gt"/>
</node>

With rtabmapviz, you will see the ground truth path in gray, and the estimated path in blue. In View->Statistics->GT panel, you will see ATE (absolute trajectory error) statistics.

EDIT When opening the database in rtabmap-databaseViewer, you can show the Statistics panel to see the computed errors under Gt tab. You can plot the error over time by clicking the button next to the statistic (in screenshot below we can see that a loop closure occurred just before 170, causing a drop of error). Under the Graph view, you should see a gray path representing the ground truth along with the blue path representing the estimated trajectory. For convenience, you can also see the same RMSE computed under it for the current optimization iteration selected. image description

Another tool is rtabmap-report to export estimated and ground truth poses in RGB-D dataset format:

$ rtabmap-report --poses rtabmap.db 
Database: rtabmap.db
    (164, s=1.000): error lin=0.020m (max=0.035m, odom=0.036m) ang=1.1deg, slam: avg=59ms (max=197ms) loops=36, odom: avg=70ms (max=100ms), camera: avg=19ms, map=285MB

This will create rtabmap_odom.txt, rtabmap_slam.txt and rtabmap_gt.txt that can be fed to RGB-D dataset's evaluation tool:

$ python evaluate_ate.py rtabmap_slam.txt rtabmap_gt.txt --plot ate.pdf
$ python evaluate_rpe.py rtabmap_odom.txt rtabmap_gt.txt --plot rpe.pdf --fixed_delta

EDIT2 The databases you shared in the comments are very difficult for visual loop closure detection as the same texture is seen everywhere, thus we cannot distinguish a place from another. No loop closures can be detected. Also the biggest problem is that you never revisit the same area with the same camera orientation. image description

For the second map, you could however enable ICP refining to correct your odometry.

Before: image description

After: image description

Parameters (rtabmap should be built with libpointmatcher for those ICP parameters):

Icp/PM=true
Icp/MaxTranslation=2.00
Icp/MaxCorrespondenceDistance=0.75
Icp/PMOutlierRatio=0.9
Icp/PointToPlane=true
Reg/Strategy=1
RGBD/NeighborLinkRefining=true
RGBD/ProximityPathMaxNeighbors=5 ...
(more)
edit flag offensive delete link more

Comments

Thanks, I followed your instructions and I am getting ground-truth path in gray, and the estimated path in blue. But the problem is that I am cannot find "View->Statistics->GT panel, you will see ATE (absolute trajectory error) statistics."

Here is what I did,

  1. I changed my robot's urdf and added the recommended plugin.
  2. I then added the recommended parameters in my roadmap launch file under rtabmap node.
  3. I ran my simulations (gazebo and rtabmap_ros) and also executed the nodes above as given in step 2 of your response. I then moved my robot in an environment and built a map to see if everything is working fine.
  4. Finally, I stopped my simulations and opened the rtabmap database file in a standalone rtabmap application.

Here is what I got,

  1. I could see the ground-truth in grey and estimated path in blue under the "window->show view->map graph".

but I could not ...(more)

al389660 gravatar imageal389660 ( 2019-09-24 15:38:43 -0500 )edit

Please let me know how can I plot the error. thanks

al389660 gravatar imageal389660 ( 2019-09-24 15:44:38 -0500 )edit

In standalone, when reopening the database, the GT results woudl not appear in Statistics panel. Can you try opening the database in rtabmap-databaseViewer, and open the Statistics panel to see it results are there? Another way to export the poses (estimated and gt) is to use rtabmap-report tool.

matlabbe gravatar imagematlabbe ( 2019-09-24 17:26:43 -0500 )edit

I just tried rtabmap-databaeViewer. However, it doesnot give me the error. Another thing is that, I want to plot the error over time like for every pose (as a continuous graph). How can I do that?. because as I understand the GT panel Statistics will only give me a number to represent an obsolute error, not the whole set of errors over entire path of robot movement.

al389660 gravatar imageal389660 ( 2019-09-24 18:24:06 -0500 )edit

I updated the answer!

matlabbe gravatar imagematlabbe ( 2019-09-24 18:54:06 -0500 )edit

thank you very much for your prompt response and help that I am getting from you. I am able to edit the database files in rtabmap-databaseViewer. I would appreciate if you could either explain the GT tab its component in general or provide the link to the document. I mean what each of them represent, like you explained the drop in error is caused by loop closure. Thank you very much.

al389660 gravatar imageal389660 ( 2019-09-24 19:55:52 -0500 )edit

I can plot both paths to draw a comparison but the only problem is that when I plot the error using GT panel of rtabmap-databaseViewer, it shows that error increases with the time.

My question is that the error should drop to zero at some point whenever a loop closure is detected. in order to make sure that a loop closure situation should arise, i deliberately map the same area twice. But it does not affect the error plot. I think the loop closure detection is not working at all. Please, explain what I am missing here. here is the launch file for rtabmap https://gofile.io/?c=wmjV5K

al389660 gravatar imageal389660 ( 2019-09-25 16:43:16 -0500 )edit

See https://vision.in.tum.de/data/dataset... for explanation about ATE. The Gt panel show only ATE errors (in general only translational RMSE is used in papers for comparison), not RPE as in the reference. For interpretation of ATE over time, see figure 13 in this paper: https://introlab.3it.usherbrooke.ca/m...

Maybe no loop closures have been detected, can you share the database?

matlabbe gravatar imagematlabbe ( 2019-09-25 18:42:12 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-09-19 10:07:24 -0500

Seen: 67 times

Last updated: Sep 29