aau-cns / cnspy_trajectory_evaluation Goto Github PK

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_true
or in your notation:
(I-skew(theta)) * R_B_G_est = R_B_G_true

As 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.