How to align ground truth coordinate and trajectory/odometry coordinate?

[WANGYINGYU]

Hi Victor Reijgwart @victorreijgwart ,

I have a question about the ground truth coordinate and robot trajectory (or odometry inputs) alignment.

In the demo dataset arche_flight1_2ms_indoor-outdoor-figure-8.bag, I found topics /penguin/piksi_node/enu_pose_best_fix and /penguin/piksi_node/enu_pose_fix, I think the two topics are obtained by RTK-GNSS which can be used as ground truth to evaluate the full trajectory from Voxgraph or the odometry inputs. However, I found the coordinate of the two topics and the map coordinate are different, and I didn't found the transformation information between the two coordinates. Due to the ground truth doesn't include rotation information, I can't align the two coordinates even though the timestamps of these topics are given.

Could you provide some guidance? Thank you.

Best regards,
Yingyu

[victorreijgwart]

Hi @WANGYINGYU,

For the evaluations in the voxgraph paper, we used the rpg_trajectory_evaluation toolbox to compute the Absolute Trajectory Error (ATE) w.r.t. the RTK ground truth. During preprocessing, we interpolate the trajectory of voxgraph (and the other frameworks) to have the same timestamps as the RTK ground truth. To handle the fact that they're in different frames, we enabled gravity-aligned rigid alignment 'align_type: posyaw' when computing the ATE.
The ground truth trajectory topic we used is /penguin/piksi_node/enu_pose_best_fix.
I hope this helps. Let me know in case you have any follow up questions.

Best wishes,
Victor

[WANGYINGYU]

Hi @victorreijgwart ,

Thank you for your reply.

I read the descriptions of evaluation tools, and the inputs are the timestamps and corresponding poses. But the ground truth obtained from /penguin/piksi_node/enu_pose_best_fix only contains timestamps and corresponding positions (i.e., only x,y and z). Do I need to process the ground truth positions by some methods to assign rotation information?

BTW, I noticed the loop closure information needs to be added manually in the rosbag, and then the code can use this information to add loop closure constraints in pose graph optimization. For the trajectory of demo dataset arche_flight1_2ms_indoor-outdoor-figure-8.bagshown in your paper (i.e., the first subfigure of Fig. 7), did you use loop closure information?

Best regards,
Yingyu

[WANGYINGYU]

Hi @victorreijgwart ,

Thank you for your reply.

I read the descriptions of evaluation tools, and the inputs are the timestamps and corresponding poses. But the ground truth obtained from /penguin/piksi_node/enu_pose_best_fix only contains timestamps and corresponding positions (i.e., only x,y and z). Do I need to process the ground truth positions by some methods to assign rotation information?

BTW, I noticed the loop closure information needs to be added manually in the rosbag, and then the code can use this information to add loop closure constraints in pose graph optimization. For the trajectory of demo dataset arche_flight1_2ms_indoor-outdoor-figure-8.bagshown in your paper (i.e., the first subfigure of Fig. 7), did you use loop closure information?

Best regards, Yingyu

I checked /penguin/piksi_node/enu_pose_best_fix again, and all rotations of this topic are 0,0,0,1 (in quaternion form, x,y,z,w).

[victorreijgwart]

Hi @WANGYINGYU,

Since the ground truth comes purely from GPS RTK, it does not contain rotation information. The position ATE we report in the paper is obtained by computing the RMSE over position only, so there is no need to estimate or assign rotation information to the trajectories.

For the ARCHE datasets, voxgraph did not need any explicit/external loop closures to be added. Thanks to the incremental optimization, the error at the end of the trajectory is small enough for the pose graph optimization to converge by itself using only the SDF-registration constraints. In the paper, we only used external loop closure constraints for the RGB-D machine hall dataset (obtained from dbow2).

Best wishes,
Victor

[WANGYINGYU]

Hi @victorreijgwart ,

Thank you very much for your patience and detailed response:). I will try it.

Best Regards,
Yingyu

[WANGYINGYU]

Hi @victorreijgwart ,

Sorry to bother you again. Now I have aligned the Voxgraph's trajectory with the ground truth well using rpg alignment toolbox, but the ATE is large than the results shown in paper. I think it may caused by the parameters setting, so I start from the default setting from the code and try to tune them, but the results do not change a lot. Below shows the results drawn by rpg toolbox. Could you provide me some guidance about how to tune these parameters?

Best Regards,
Yingyu

[WANGYINGYU]

Hi @victorreijgwart ,

This problem may caused by the different version of Ceres when solving the final pose graph. Using the default code of backend/pose_graph.cpp with eigen 3.3.7 and lapack-suitesparse 4.2.1, the final pose graph will not converge. I modified this code by adding ceres_options.max_num_consecutive_invalid_steps = 30; and then the final pose graph converge. The new result shown below is similar to the paper's result.

[victorreijgwart]

Hi @WANGYINGYU, sorry for the slow reply. I tried to replicate your error but didn't manage so far, so I'm glad to hear you found a solution! It's good to know that a change in Ceres caused it and that it can be fixed by setting max_num_consecutive_invalid_steps :)