rosbag topics about '/navsat/odom' in CA dataset

[robotstar]

I assumed that the topic '/navsat/odom' corresponds to ground truth poses T_{W,Gk}, where W means the world frame and Gk means the frame of SPAN-CPT in time tk. However, the test results contradict this assumption. It seems that '/navsat/odom' is close to poses T{W,Lk}, where L_k is the LiDAR frame in time t_k. But I am not sure about it.

Thanks for your help!

[robotstar]

In my test, I assumed that the topic '/navsat/odom' corresponds to ground truth poses T_{W,Gk}, where W means the world frame and Gk means the frame of SPAN-CPT in time tk and calculate the relative poses T{G0,Gk} = T{W,G0}^{-1} * T{W,Gk}. Then, I calculate T{L0,Lk} = T{L,G} T_{G0,Gk} T{L,G}^{-1} for ground truth poses of LiDAR odometry, where T{L,G} is the extrinsic of SPAN-CPT to LiDAR in the calibration_CA.txt:

%% SPAN-CPT % Auto-calibrated after triggering % Extrinsic [to LIDAR] [0., -1., 0., -5.245e-01 -1., 0., 0., 1.06045 0., 0., -1., 7.98576e-01]

However, I find that above poses T{L0,Lk} differ from the pose estimation results of common LiDAR odometry algorithm a lot. The former goes left and the latter goes forward. It looks like above obtained T{L0,Lk} is not aligned to the LiDAR frame. But I do not know what went wrong.

[robotstar]

This is experiment results of CA-20190828184706_bluralign.bag Red trajectory is generated by a LiDAR odometry algorithm and blue trajectory is the ground truth T{L0,Lk} calculated by above method. It can be seen that they are not aligned as expected.

[weisongwen]

Hi @robotstar Thanks for your interest. We did not mention the "/navsat/odom" topic in the readme of this dataset as we are not quite sure about the coordinates of this topic (possibly the ENU). However, the topic "/novatel_data/inspvax" provides the ground truth geodetic pose. We suggest using this data for ground truth evaluation.

Regarding your third question, it is clear that the coordinate system of LiDAR odometry with the "/navsat/odom" are not aligned. To align this, you can use the orientation information from "/novatel_data/inspvax".

[robotstar]

Thanks for your reply. I also converted "/novatel_data/inspvax" to ros message navmsgs/Odometry and treat it as T{W,Gk}, where W means the world frame and Gk means the frame of SPAN-CPT in time tk. And then calculate the relative poses T{G0,Gk} = T{W,G0}^{-1} * T{W,Gk}. Finally, T{L0,Lk} = T{L,G} T_{G0,Gk} T{L,G}^{-1} is calculated for ground truth poses of LiDAR odometry, where T{L,G} is the extrinsic of SPAN-CPT to LiDAR in the calibration_CA.txt:

%% SPAN-CPT % Auto-calibrated after triggering % Extrinsic [to LIDAR] [0., -1., 0., -5.245e-01 -1., 0., 0., 1.06045 0., 0., -1., 7.98576e-01]

However, there exists about 90-degree yaw difference between above ground truth T_{L0,Lk} and the trajectory generated by LiDAR odometry algorithm in the beginning.

I think there are two possible reasons. One is that the "/novatel_data/inspvax" is not fixed in the frame SPAN-CPT. Another is the extrinsic of SPAN-CPT to LiDAR, especially the rotation matrix is not right.

[weisongwen]

Hi @robotstar , thanks for the update. Your formulation is perfect if you are using the Relative positioning sensor. However, the extrinsic between the LiDAR and SPAN-CPT provided in this dataset is only the Lever arm. The "/novatel_data/inspvax" is in the geodetic frame but the LiDAR odometry is originated at the first point where the vehicle start. Therefore, the extrinsic between your L0 and the world frame should be calculated from the first point provided by the "/novatel_data/inspvax" .

To understand this, you may need be clear about the ECEF, ENU. Local frame (your L0). For a detailed formulation, you may like this work which used the CA dataset.

Liu, Jinxu, Wei Gao, and Zhanyi Hu. "Optimization-Based Visual-Inertial SLAM Tightly Coupled with Raw GNSS Measurements." arXiv preprint arXiv:2010.11675 (2020).paper

[robotstar]

Thanks for your reply. I read provided reference paper and the definition of geodetic frame, ECEF, ENU and Local frame. But I still don't know how to modify my formulation to get correct ground truth for LiDAR Odometry.

According to my understanding, I have transfered the absolute pose T{W,Gk} to the relative pose T{G0,Gk} = T{W,G0}^{-1} * T{W,Gk}, where W is the geodetic frame, and Gk is the frame of SPAN-CPT in time t_k.

Besides, I don't understand the meaning of "Lever arm". Please explain it detailly.

[weisongwen]

Hi @robotstar !

To clear answer your question, we draw a figure as follows:

Please let us know if you still get an questions!

[robotstar]

Hi @robotstar !

To clear answer your question, we draw a figure as follows:

Please let us know if you still get an questions!

Thanks for your reply! I read your reply detailly and respect your sincere answer. And I have a quick question about the figure. The extrinsic parameter T_{ENU_i}^{SPAN_i} does not include the first SPAN rotation. Generally, the orientation of SPAN_i is not necessarily the same orientation as the ENU_i frame. Is it ok?

[weisongwen]

@robotstar you need the orientation which also provided by the SPAN. To be more specific, the T_{ENU_i}^{SPAN_i} mainly involves the rotation if you choose the first point as the reference point of ENU.