jungr-ait
commented
2 years ago Dear Patrick,
Thank you very much for the detailed description in the raised issue, which helped a lot! Second, we are sorry for the late response, we just recently saw the issue and I needed some time to get into the topic again.
In general, it would be ideal if both toolboxes, ov_eval and our trajectory_evaluation, would lead to the same ATE and NEES values.
I will quickly answer your questions:
Q1: “your paper and I believe it is because you are using the "global" orientation error state as compared to the outputted "local" error-state covariance.”
A1: The trajectory evaluation assumes a “local” orientation error state (https://github.com/aau-cns/cnspy_trajectory_evaluation#definitions-and-metrics) and a globally defined position error state. In our notation we assume: R_G_B_true = R_G_B_est * (I + skew(theta_Best_B)); with the rotation notation: p_in_G = R_G_N * p_in_N; meaning R_G_N maps a vector from the reference frame N to G;
The small angle rotation error in OV is alsotheta_Best_B (orientation from the estimated body to the true body), right?
Concluding, our toolbox assumes the same error representation as OV.
Q2: Additionally, it seems that the estimated trajectory is transformed as compared to the groundtruth. Ideally, this means that a covariance propagation should be performed since this is changing the underlying error state location. I think this will be an issue for the position and global orientation error state methods. The way to get around this is to just align the groundtruth to the estimate instead (this might not be that big of an effect at the end of the day).
A2: Thank you very much for raising that issue, and indeed the covariance of the estimated trajectory needs to be transformed as well. Since both our toolboxes treat the position and orientation uncertainty separately, and as we assume local rotation errors, only the covariance of the global position error needs to be mapped.
(Hint: In our next release we plan to support different error definitions and the uncertainty of the estimated trajectory will be accordingly transformed.)
I would still suggest aligning the estimated trajectory with the true trajectory as this would support an overlaid plot of multiple estimates to directly compare different approaches in e.g., a 3D plot.
Despite having that issue in the implementation, it should lead only to minor errors in the NEES computation, since the true trajectory in VINSEval starts at the identity with an z-offset of 1m and all estimators are initialized at identity. Meaning the posyaw alignment in the first frame, would only result in a translational shift and thus not affecting the estimated covariance.
Here are the generated (VINSEval) and estimated trajectory (OpenVINS) (please note the different color assignments in the legend):
Since I was also curious why the NEES values calculated in our toolbox are - in comparison to other approaches - so high, I tried to compute NEES values using your ov_eval toolbox on the EuRoC machine hall MH01 sequence to directly compare the NEES results.
I ran the freshly downloaded OpenVINS with catkin_ws/test_openvins$ roslaunch ov_msckf subscribe.launch config:=euroc_mav dobag:=true bag:=<my home>/workspace/datasets/machine_hall/MH_01_easy.bag dosave:=true dataset:=MH_01_easy dolivetraj:=true
And executed rosrun ov_eval error_dataset sim3 eval_results/truth/MH_01_easy.txt eval_results/algorithms/ after creating the corresponding folder tree:
eval_results
├── algorithms
│ └── open_vins
│ └── MH_01_easy
│ ├── MH01_ov_est.csv
│ ├── MH01_ov_est.txt
│ └── run1.txt
└── truth
├── MH_01_easy.txt
├── MH01_gt.csv
└── MH01_gt.txt
The NEES result using posyaw aligment using the first frame:
And using se3 using the first frame:
I have converted the true and estimated txt-files of the trajectories into CSV-files (note that the JPL quaternions were not converted to Hamiltonian ones, thus the rotation is inverted) to process them in our trajectory evaluation toolbox and obtained for posyaw single frame:
And from sim3 over the entire trajectory:
Both toolboxes indicated, despite differences in the implementation, a similar trend and resulted in the end in high average single run NEES values. Potentially, these values can be improved by manual tuning of the estimator, but I am lacking in experience using OpenVINS to achieve the best results.
As it would be best that both toolboxes are leading to the same results our next release will support the proper transformation of estimated trajectories. Comparing the NEES computation in OV: https://github.com/rpng/open_vins/blob/bf0ada9f4f6d671f472616ab00e5df3bd6d6ce83/ov_eval/src/calc/ResultTrajectory.cpp#L254-L265, our toolbox computes the rotation error as: https://github.com/aau-cns/cnspy_trajectory_evaluation/blob/13857709a3cd38ed24e8a8f1b2c9af7ae7a9a4d1/cnspy_trajectory_evaluation/AbsoluteTrajectoryError.py#L151 and converts it to a tangent space vector (theta in so(3)) in: https://github.com/aau-cns/cnspy_trajectory_evaluation/blob/13857709a3cd38ed24e8a8f1b2c9af7ae7a9a4d1/cnspy_trajectory_evaluation/TrajectoryNEES.py#L131 And computes the NEES according to: https://github.com/aau-cns/cnspy_trajectory_evaluation/blob/13857709a3cd38ed24e8a8f1b2c9af7ae7a9a4d1/cnspy_trajectory_evaluation/TrajectoryNEES.py#L156
The only difference is the negative sign in OV, which can be neglected in the NEES computations, since it will be squared, right? Are high NEES values in general a known issue of OpenVINS? The estimation accuracy is in general pretty astonishing, impressive, and was super robust on the machine hall sequence.
In future we will support different error representations (theta as tangent space element in so(3), theta from first order Taylor expansion of rotation matrices, and theta from a small quaternion approximation) and error definitions (local/global pose, global/local position + global/local orientation). Also the next release will allow the visualization of the N-sigma bounds on top of the estimation error (as shown above), support a full SE(3) covariance, a relative pose error evaluation, and much more. Those topics and improvements should be covered in a short paper and we would be honored to have a discussion with you.
Bests, Roland
goldbattle
I was taking a look at the weird OpenVINS NEES results, in your paper and I believe it is because you are using the "global" orientation error state as compared to the outputed "local" error-state covariance. See the ATE error and the NEES error computation in the two files below which compute the global error (to my understanding): https://github.com/aau-cns/cnspy_trajectory_evaluation/blob/main/cnspy_trajectory_evaluation/AbsoluteTrajectoryError.py#L152 https://github.com/aau-cns/cnspy_trajectory_evaluation/blob/main/cnspy_trajectory_evaluation/TrajectoryNEES.py#L156
Additionally, it seems that the estimated trajectory is transformed as compared to the groundtruth. Ideally, this means that a covariance propagation should be performed since this is changing the underlying error state location. I think this will be an issue for the position and global orientation error state methods. The way to get around this is to just align the groundtruth to the estimate instead (this might not be that big of an effect at the end of the day). https://github.com/aau-cns/cnspy_trajectory_evaluation/blob/main/cnspy_trajectory_evaluation/AlignedTrajectories.py#L42
OpenVINS outputs the covariance in the local error state for the q_GtoI JPL quaternion orientation: https://github.com/rpng/open_vins/blob/master/ov_msckf/src/ros/ROS1Visualizer.cpp#L524-L534
or in SO(3) terms it is:
(I-skew(theta)) * R_GtoI_hat = R_GtoI_trueor in your notation:(I-skew(theta)) * R_B_G_est = R_B_G_trueAs an example, one can inspect the codebase to see how we performed evaluation for NEES: https://github.com/rpng/open_vins/blob/master/ov_eval/src/calc/ResultTrajectory.cpp#L254-L265
Hope this helps.