Extracting trajectory/poses

[struncp]

Short version: I haven't found anything adressing this topic, so: Is there a way how to get robots trajectory from Cartographer with ROS?

Long version: Hello, I am comparing SLAM algorithms based on their poses throughout their trajectory in my thesis and I can't come up with a way to get a trajectory from cartographer. I use .bag data with ground truth from MIT Stata center (link below). Currently I'm obtaining trajectory via hector trajectory server and it worked fine with both Hector SLAM and gMapping but Cartographers trajectory is quite off

This is from run at 0.2 speed. When I run it at realtime and robot is coming back from that part "north" to the main long hall (that part that looks like two curved horns) back that long hall to go "east" it simply skips like this

I believe there is some hard computing when cartographer is closing the loop from that "north" part which is not compatible with hector trajectory server. I also hope that I have right settings for cartographer because map on the outcome is correct.

source code: https://www.dropbox.com/s/6mhipp0keaj1ee3/cartographer_source.rar?dl=0

used bag: 2012-01-28-11-12-01.bag.noimages (in ground truth) at http://projects.csail.mit.edu/stata/downloads.php

Thanks for any feedback in advance.

[SirVer]

I can't come up with a way to get a trajectory from cartographer.

You run Cartographer and once all your data has been replayed, you use rosservice call /finish_trajectory map. It will put files in ~/.ros/map*, one of them is the trajectory in a protobuf. Alternatively you can use the offline tool to run through your bag as quickly as possible and receive the trajectory next to the bag.

via hector trajectory server

To me this sounds like you are comparing apples with oranges then.

My understanding is that you are interested in the transform map -> base_link over time. But while the SLAM is running, earlier poses will be pushed around constantly, so you cannot listen to map -> base_link live and compare these later. For all systems, you have to let the whole SLAM run finish and compare the final trajectory. Cartographer gives you this data (as said), the other systems probably do too.

As of the ground truth data, I would suggest taking their ground truth with a grain of salt though - the procedure they describe does not sound like it would give 2cm accuracy. From http://projects.csail.mit.edu/stata/groundtruth.php:

Relaxing this posegraph (using iSAM, developed by Michael Kaess) produces the final groundtruth poses for that small batch. This process is repeated for each subsequent batch of scans. The scanmatching mentioned above uses the Fast and Robust Scan Matching (developed by Abe Bachrach) which produces very low drift rates in all situations (except long corridors).

I had a look at your bag and there is a pretty long hallway and since you only use the laser for your SLAM solution, Cartographer slides in there quite substantially. It only fixes it once you get back to the start - and maybe not completely (unsure, no time to investigate). You probably should mix in odometry to get a much better result.

My map is below. See the last few commits in my branch - I only did less work than your config. I ran your bag using roslaunch cartographer_ros cartographer.launch bag_filename:=$HOME/Downloads/2012-01-28-11-12-01.bag.noimages

[struncp]

Thank you for quick response and provided files.

I have a problem with opening the .pb file wich contains the trajectory. I tried to install protobuf today but errors like " The C compiler is not able to compile a simple test program." are popping up and I haven't found a way how I could open it with it. I found something about .proto files are making classes needed to work with .pb files. Could you please give me one more advice about this?

[SirVer]

In the root of your cartographer GitHub checkout, this should work (on Linux) to get a human readable version of the trajectory:

protoc --decode cartographer.mapping.proto.Trajectory -I. $(find . -name '*.proto') < ~/.ros/map.pb

[struncp]

Thank you and sorry for the delay. This trajectory is (big surprise) better than the method I used. It is still a little off though.

[SirVer]

I think we can do better than that. Probably relying more on the linear model prediction. I'll have another look at the tuning, but it will require a few days.

[SirVer]

Could you provide the ground truth and your plotting and comparing code somewhere?

[struncp]

Here is the matlab script I use. It's a bit messy and written in czech but I changed some descriptions so it could be understandable for english speaker.

comparing_slam_trajectory.zip(just run the main one)

The error I use for comparing trajectories is (4) in On Measuring the Accuracy of SLAM Algorithms.

Script generates plots for compared SLAM solutions and then prints RMSE of trajectories, mean of error from the article, its variance and standard deviation. All prints consists of 3 numbers, first for hectorslam, second for gmapping, third for cartographer.

[SirVer]

I took another look at your data. I wanted to use the IMU or odometry to improve the result a bit, but both data seems bogus in the bag (for example /base_odometry/odom has an empty child_frame_id which seems improbable to me).

I instead told the SLAM to penalize changing of speed which also improved the result - in fact I think the slipping is basically gone now. I was unable to get your scripts running to do the comparison against the provided ground truth. Could you try?

My changes are again in this branch. The important change is this commit but I also merged #342 into the branch to make offline processing faster.

[struncp]

After I changed the translation weight for scanmatcher as you did in your commit I ran the experiments again and results are much better. Cartographer now even has the lowest RMSE of all SLAM solving implementations. This is how the trajectory looks now

and matlab script with the new trajectory is here

Thank you very much for all your help.

[SirVer]

Cartographer now even has the lowest RMSE of all SLAM solving implementations.

Great! Of course giving the authors of the other system also a chance to tune for their implementation will yield the best comparison results.

Looking at the result, I cannot shake the feeling that the ground truth is funky. For example in the long corridor leading to the right you can see this black 'yarn ball'. If you look at the visualization in Cartographer you can see that the robot turns in place there (same for the corridor going left). I think Cartographer is configured to track the center of the robot (so there is very litte yarn, since the robot turns around its center), while the ground truth seems to track the position of the laser. You probably want to double check that and account for it in your comparisons.

Another thought: the method they use to establish their ground truth is essentially manual scan matching. This seems quite susceptible to angular errors - so I am skeptical if Cartographer or the ground truth is more correct compared to the physical world.

Let us know once your thesis is done!

[struncp]

Will look up that ground truth although I don't have much time left to do some super research. I have 12 hours to deadline.

And before we get any expectations about my thesis in here I should set something straight. It's Bachelor thesis and unfortunately for you it's not in english.

But if you would be interested in the results I would be more than happy to share them with you.

[SirVer]

The results would be very interesting. Even if it is just a paragraph of your conclusion.

[sunmk2006]

How do I get the data in this paperhttp://www2.informatik.uni-freiburg.de/~stachnis/pdf/kuemmerle09auro.pdf，i can't open its given link http://ais.informatik.uni-freiburg.de/slamevaluation ,do you help me?

[HolmarSeffelgruber]

@SirVer do you already have something programmed to load a trajectory of the cartographer 3d in rviz? I am interested in that. I also don't know yet how to extract the trajectory from the console. @sunmk2006 I don't think he can help you with that. You are probably better off asking someone at the university Freiburg or someone from the paper.

[sunmk2006]

They used the data in their 2D cartographer paper,I have sent an e-mail to the authors of the paper, but there is no reply.

[SirVer]

@HolmarSeffelgruber I do not think rViz has a plugin to visualize trajectories. We did not write one. I outlined how to get to the final trajectory in this issue further up.

@sunmk2006 The site used to be available when we wrote the paper. I am sorry, I cannot help there, you need to talk to the freiburg people.

[HolmarSeffelgruber]

Some recent update changed the protobuf output. Now it looks like this: before: rotation { x: 0.086251528618212767

after: 2 { 1: 0xbf59e9f2e45a3309

I don't know if it is needed now that we can get the trajectory from a ros message but I wanted to inform you.

[SirVer]

@HolmarSeffelgruber could you post the full commandline you are running? In which directory are you running it?

[HolmarSeffelgruber]

@SirVer Sure! This one is new and doesn't work. willing@a234:~/mapping/cartographer_ws/src/cartographer$ protoc --decode cartographer.mapping.proto.Trajectory -I. $(find . -name '*.proto') < ~/.ros/kreisel_front_rev533.pb It works, if I use an old one and I only change the file in the command. Here is the terminal output recorded with script: trajectory.txt

[SirVer]

@HolmarSeffelgruber Oh, I know what's wrong. Since #365, we serialize the full pose graph instead of just the trajectory. Try using cartographer.mapping.proto.SparsePoseGraph instead of Trajectory. It is now this schema.

[aabyshk]

Firstly, thanks for the great work with Cartographer! I am building an outdoor map with a 2D LiDAR and cartographer builds a very nice map with loop closure. I use finish_trajectory service to write the map and the corresponding .pb file. Is there a way I could stream the .pb file as a ROS topic while replaying the .bag file?

I want to assemble the scans from another tilted 2D LiDAR on the robot and consequently use PCL for registration. As mentioned above, we need to use the trajectory generated after the map is built. Any help would be much appreciated!

[sunmk2006]

You can add a little bit of code to your cartographer program to convert it into a nav_msgs/Path message after getting a new point of trajectory.

[SirVer]

There is also the Hector Trajectory Server that does essentially that. I do not know how well it deals with jumps through loop closure though, since imho Hector has no loop closure.

[sunmk2006]

I do not think hector will deal with jumps,since it only get the pose point of trajectory from tf transform(base_link ---> map)

[aabyshk]

Thanks for all your suggestions! I agree with sunmk2006 about Hector Trajectory Server.

I am curious about the contents of the .pb file. From previous discussions it seems like it contains pose over time for the final trajectory after all the loop closures. For my dataset, there are at least 3 loop closure instances which cause significant shift in the pose of the robot. Does the trajectory in .pb file include the sudden jumps in pose due to loop closure? If not, it would be useful to have a rosbag time synchronized trajectory player for processing other sensor data after the map is built using the "true" trajectory of the robot. Just curious if this already included in any of the ROS nodes.

[SirVer]

From previous discussions it seems like it contains pose over time for the final trajectory after all the loop closures.

That is correct. It used to be a binary Trajectory proto, but now it is a binary SparsePoseGraph proto - which also contains the Trajectory.

Does the trajectory in .pb file include the sudden jumps in pose due to loop closure?

No, the proto is entirely in map frame, i.e. it will contain the fully optimized Trajectory after all data is incorporated and optimization has run.

If not, it would be useful to have a rosbag time synchronized trajectory player for processing other sensor data after the map is built using the "true" trajectory of the robot. Just curious if this already included in any of the ROS nodes.

This is essentially what the assets_writer_main node does. It does not publish the sensor data though, so no other nodes can use the data - but it is easy to add another PointsProcessor to get more out of the binary.

[SirVer]

Btw, Cartographer now also publishes the trajectory in form of markers, so you can get to the information now.

[aabyshk]

Thanks a lot for your input SirVer! Yes I already tried visualizing the trajectory markers in RVIZ and they are very useful for visualization. However, I would really like to use the SparsePoseGraph proto (the .pb generated using the assets writer node) since it gives good structure to the mapping procedure and "load" the trajectory along with the corresponding map and play it along with the bag file. Do you have any recommendations on where I could start with implementing this? I am looking at the assets_writer_main node to see if I can refactor the code for the above purpose.

Thanks again for your quick responses and help!

[SirVer]

I am not exactly sure what you want to achieve, but this is my guess and how I would go about it: Yes, the assets_writer_main is exactly where you want to do this change. My suggestion to go about this is:

• Optional, but probably useful: Fix googlecartographer/cartographer#174, so that you get the trajectory on assets. Might not help if you are purely 2D (i.e. all your points are in the same plane and you have no tilt information).
• If you are purely 2D, you must fix googlecartographer/cartographer#355 first.
• Next, change assets_writer_main (see #395) to take in the proto (it does already) and a list of .bags that maps to the trajectories in the proto. The idea is that you run offline_node --bag_filenames bag1, bag2,bag3 and then assets_writer --bag_filenames bag1,bag2,bag3 --proto output.pb.
• Then loop over each bag in the asset writer and do essentially what we are doing right now, but keeping the points pipeline alive to get a combined result.

@abhinitd If you implement this or parts of it, you are my personal hero :)

[aabyshk]

@SirVer Thanks for all the help and inputs. I will definitely try to fix the above mentioned issues. However, I need to spend some time to understand the software architecture since a lot of it is new to me. I am referring to the online documentation and the ICRA paper to begin with. Let me know if you have any other suggestions.

[SirVer]

This and the code is all that we currently have. It is not too much, we understand and hope we can fix this eventually.

[litaws]

UPDATE - I see this was addressed in #460

Hello

I have been using an older version of cartographer_ros, where an offline run would generate a trajectory protobuf file (".pb"), along with xrays and a PLY file (This is similar to the discussion here). I was able to parse the trajectory file using trajectory.proto (on python)

After updating my code to the a newer revision (16-Aug-2017), I see the all latter files are no longer generated, and a get ".pbstream" instead. I cannot seem to be able to parse this protobuf even using sparse_pose_graph.proto

[damonkohler]

@litgm, I'll refer you to the new asset writer documentation. It discusses how to ingest the pbstream and turn it into a x-ray or PLY.

[Andalph]

I have a similar issue but the workaround suggested produces an error...

I'm trying to convert the resultant .pbstream to a trajectory in readable text format. I was able to get it with the .pb output but i'm having trouble with the *.pbstream.

this is the command i'm using and the resulting error : as@as-VirtualBox:~/new_catkin_ws$ protoc --decode cartographer.mapping.proto.SparsePoseGraph -I. $(find . -name '*.proto') < ~/Desktop/Local_Bags/0457.bag.pbstream

Edit I also tried running the same command from the /src/cartographer/ folder with no luck...

_cartographer/common/proto/ceres_solver_options.proto: File not found. src/cartographer/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto: Import "cartographer/common/proto/ceres_solver_options.proto" was not found or had errors. src/cartographer/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto:33:12: "cartographer.common" seems to be defined in "src/cartographer/cartographer/common/proto/ceres_solver_options.proto", which is not imported by "src/cartographer/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcheroptions.proto". To use it here, please add the necessary import. Any help would be greatly appreciated!

[SirVer]

@Andalph You are running it in the wrong directory. You have to run it so that the paths are resolvable from the current work directory. So probably cd src/cartographer && <rerun command> might work for you.

Alternatively you should be able to modify the -I path.

[Andalph]

@SirVer , Thanks for your reply! Glad to know I'm sort-of on the right track. I should have been more detailed with my edit - I did try changing my directory and rerunning already. I tried again this morning. Do I need to resource something? Here are the returns i'm getting from /src/cartographer:

_as@as-VirtualBox:~$ cd new_catkin_ws/ as@as-VirtualBox:~/new_catkin_ws$ source install_isolated/setup.bashas@as-VirtualBox:~/new_catkin_ws$ cd src as@as-VirtualBox:~/new_catkin_ws/src$ cd cartographer

as@as-VirtualBox:~/new_catkin_ws/src/cartographer$ protoc --decode cartographer.mapping.proto.SparsePoseGraph -I. $(find . -name '*.proto') < ~/Desktop/Local_Bags/0457.bag.pbstream Failed to parse input.

as@as-VirtualBox:~/new_catkin_ws/src/cartographer$ protoc --decode cartographer.mapping.proto.SparsePoseGraph -I. $(find . -name '*.proto') < '/home/as/Desktop/Local_Bags/0457.bag.pbstream' Failed to parse input.

as@as-VirtualBox:~/new_catkin_ws/src/cartographer$ protoc --decode cartographer.mapping.proto.SparsePoseGraph -I. $(find . -name '*.proto') < '/home/as/Desktop/Local_Bags/0457.bag.pbstream' > bash: syntax error near unexpected token `newline'

as@as-VirtualBox:~/new_catkin_ws/src/cartographer$ protoc --decode cartographer.mapping.proto.SparsePoseGraph -I. $(find . -name '*.proto') < '/home/as/Desktop/LocalBags/0457.bag.pbstream' > '/home/as/MyTrajectoryDecoded3.txt' Failed to parse input.

[SirVer]

Sorry, I misread. The .pbstream is not a flat proto anymore. You have to actually parse it using ProtoStream, like it is done in assets_writer_main. There is no longer a simple command-line way of getting to the trajectory poses, you have to code something.

[Andalph]

OK good to know; thanks for your quick replies!

[Flandoe]

@Andalph I use roscd cartographer, then I run the command: protoc --decode cartographer.mapping.proto.SparsePoseGraph -I. $(find . -name '*.proto') < '/home/zxkj/Downloads/bs.pb' > '/home/zxkj/Downloads/trajectory.txt' but the terminal show 'Missing input file.' Could you help me? Thanks in advance.

[MichaelGrupp]

@Andalph Unless you use an antique version of Cartographer, this will never work. As @SirVer pointed out, the format is not plain .pb anymore.

This might help you: https://github.com/googlecartographer/cartographer_ros/issues/1000#issuecomment-419068894

[Flandoe]

@MichaelGrupp I use the command: rosservice call /write_state ${HOME}/Downloads/bs.pb to generate a .pb file instead of .pbstream file you mean the .pb file is not the same format as it used to be?

[MichaelGrupp]

Unless you use an antique version of Cartographer

.pbstream has been the serialization format for more than a year, that's why I pointed that out. Your Cartographer version seems to be older then.

[Flandoe]

@MichaelGrupp I install my Cartographer by folllowing the tutorial of Cartographer, so I think my version is not so old. I just change the command rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream', include_unfinished_submaps: 'true'}" to generate a .pb file. I don't know the change is right or not. I have also read the commit you recommended, but I have no idea where to add the code. Thanks for your reply.

[ojura]

Here is a code sample which loops through all trajectory nodes: https://gist.github.com/ojura/85507cd6c8be419357305b30d9bb949d

[Flandoe]

@ojura I try to compile the cpp file you give, but I get wrong. Here is my CMakeLists: cmake_minimum_required(VERSION 2.8.3) project(read)

add_compile_options(-std=c++11)

find_package(PCL 1.7.2 REQUIRED) find_package( catkin REQUIRED COMPONENTS roscpp rospy std_msgs )

include_directories( ${PCL_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} /home/zxkj/Cartographer/install_isolated/include /usr/local/include/eigen3/ /usr/include/lua5.2 )

add_executable(read export_trajectories.cpp)

target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) Could you give some advice? Thanks.

[ojura]

Your CMakeLists is incorrect. You should not use include_directories. You need to have find_package(cartographer), and cartographer_ros_msgs and cartographer_ros as Catkin components.

I recommend you add export_trajectories.cpp to cartographer_ros so you don't have to set up a separate Catkin project.

[Flandoe]

@ojura I add add_executable(read export_trajectories.cpp) target_link_libraries(read ${catkin_LIBRARIES} cartographer ) at the end of the CMakeLists in the cartographer_ros, and then use catkin_make_isolated --install --use-ninja, but the compiler say fatal error: ros/types.h: no such file or directory. I think find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) in the CMakeLists will find the types.h, but indeed I cannot compile after adding my code. Could you point out what I should do? I‘m not good at CMakeLists. Thanks.

[ojura]

If you're adding it to cartographer_ros' CMakeLists, you should just link your executable with cartographer_ros.

[Flandoe]

Yes! target_link_libraries(read cartographer_ros ) is okay! Thank you sincerely!

[fuxingyin]

I have got a better result.