How to extract trajectory when running pure localization mode?

[albertchang1989]

Thanks for this great work! I am running pure localization mode on cartographer with a map which is also built by cartographer.The result looks very good to me. I want to extract the pose of the trajectory to do some further analysis.I don't have any idea how to manage. It would be really helpful if you guys have any suggestion.

Thanks in advance!

[gaschler]

extract the pose of the trajectory to do some further analysis

Assuming that you want to analyze a trajectory that is saved in a pbstream file, you could take a look at cartographer_ros/dev/trajectory_comparison_main.cc and do something similar.

[albertchang1989]

extract the pose of the trajectory to do some further analysis

Assuming that you want to analyze a trajectory that is saved in a pbstream file, you could take a look at cartographer_ros/dev/trajectory_comparison_main.cc and do something similar.

Now my problem is that I don't know how to save the trajectory when I am using pure localization. Is it the same as running an online node? first : rosservice call /finish_trajectory 0 then: rosservice call /write_state

[MichaelGrupp]

/write_state won't be useful for saving a pure localization trajectory for evaluation purposes. In pure localization mode the pose graph is trimmed, so you won't be able to save the whole localization trajectory. The poses saved would also not be the live localization, but would have undergone some optimization.

It's usually better to evaluate the live performance of the localization, i.e. the trajectory of the published TF frame, to a fully optimized version generated from the same sensor data in mapping mode. (Assuming you don't have ground truth, otherwise use ground truth data.)

A script to do this is here for example (uses the code that @gaschler linked): https://github.com/googlecartographer/cartographer_ros/blob/master/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh

You can adapt this to use your configuration instead of backpack_2d.lua

[albertchang1989]

@MichaelGrupp Thanks for the answer! The purpose of extracting the pose of pure localization is to take it as a standard to evaluate the precision of the pose which is created by the visual slam (Lidar and camera is mounted on the same robot). So optimization is encouraged. Is there anyway to keep all the poses ? Thanks a lot!

[MichaelGrupp]

Ok, then you can just "disable" trimming set max_submaps_to_keep to a high value and use /write_state:

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 99999999,
}

I think if you remove the pure localization trimmer completely from the config, it will also not be used.

[albertchang1989]

@MichaelGrupp great! I've already change the set. When the process is finished (i.e. no more data), I want to save the trajectory of localization (i.e. trajectory 1, because trajectory 0 is the one with the map). I use :

rosservice call /finish_trajectory 1
rosservice call /write_state "{ filename: '${HOME}/Desktop/test.pbstream'}"

However, test.pbstream only contains trajectory 0. Am I doing right?

[MichaelGrupp]

I think rosservice call /finish_trajectory 1 is the problem... if you finish a pure localization trajectory, it's getting deleted. Try to directly call /write_state, should be fine...

[albertchang1989]

@MichaelGrupp Thanks a lot! I will try!

[MichaelGrupp]

You can also check rosservice call /get_trajectory_states after /finish_trajectory. If a trajectory is deleted, it will be in status "DELETED" (3) (defined here)

[albertchang1989]

@MichaelGrupp Thanks again for the answer! My problem is solved.