Package: cnspy-trajectory-evaluation
cnspy_trajectory_evaluationThe classTrajectoryEvaluationevaluates two trajectories given by a CSV-file (estimated and ground-truth), associates their timestamps, aligns them according to a specified scheme, and computes the absolute trajectory error (ATE) and the normalized estimation error square (NEES). The NEES is based on the uncertainty of the estimated trajectory and the ATE.
The results can be plotted and will be saved asEvaluationReport.The CSV-file of the estimated trajectory must contain the pose uncertainty (CSVFormatPose.PoseWithCov). For the file format please refer to thecnspy_spatial_csv_formatspackage and theCSVFormatPose.pyfile.InstallationInstall the current code base from GitHub and pip install a link to that cloned copygit clone https://github.com/aau-cns/cnspy_trajectory_evaluation.git
cd cnspy_trajectory_evaluation
pip install -e .DependenciesIt is part of thecnspy eco-systemof thecns-githubgroup.Main dependencies are:numpymatplotlibpandasscipycnspy_timestamp_associationcnspy_csv2dataframecnspy_spatial_csv_formatscnspy_trajectoryDefinitions and MetricsAs mentioned in the introduction, the aim to evaluate an estimated trajectory of a body reference frame with respect to a global/world reference frame against the true/actual trajectory (the so called groundtruth). Compared to [1], we removed the relative trajectory error (RTE) evaluation, as we agree with the authors that it is less straightforward to compare/judge estimation accuracy. In addition to [1], we added the normalized estimation error square (NEES) evaluation as measure for the estimator's credibility as defined in [2]. The NEES, also known as the Mahalonobis distance squared, is a unit-less metric that relates the absolute estimation error to the estimated uncertainty.The estimated quantities can be modeled in various ways, which directly influences the definition of the uncertainty. E.g. assuming we have two coordinate reference framesG(GLOBAL) andB(BODY). The estimated states are the position and orientation ofBwith respect toG. Now regarding the error definition for the position and velocity, one has two options: (i) the position error with respect to the global frame (common case) or (ii) with respect to the body frame.
As we compute the ATE and the NEES with respect to the global/world reference frameG, the uncertainty the position must be expressed in this frame as well. If the uncertainty of the estimator is defined in the body reference frame, it has to be transformed in advance back to the global frame, before the trajectory evaluation is performed.
In case of the orientation, again various possibilities to define the uncertainty exists.
First, different representations of orientations exists: rotation matrices in SO(3), unit quaternions in H, or euler angles in radians or degrees. For indirect (error-state) EKF formulations, the use of quaternions has become a gold standard (OpenVINS, LARVIO, VinsMono), while the trend goes towards representing the error in the tangent space of the corresponding manifold (ROVIO).Currently, the evaluation tool assumes the orientation uncertainty to refer to the small angle approximations of quaternionstheta.
Thus, the rotational error for quaternions is defined asq_err = [1; 0.5 * theta]or asR_err = eye(3) + skew(theta)for SO(3) matrices.
The rotational error is defined asR_G_B_err = R_G_B_true^T * R_G_B_est, leading to local perturbations (EQ. 190 in [3]). Note that rotation matrices and unit-quaternions can be mapped directlyR_A_B = R(q_A_B), reading as the orientation ofBwith respect toA. This means that the uncertainty of the orientation/attitude has to be defined in the local/body reference frameB.ExamplesPlease refer to the unit-test sections in the directorytest.position error plotPose error plotPose NEES plotCreditsThe classesAbsoluteTrajectoryErrorandSpatialAlignmentof the  packagetrajectory_evaluationare based on the preliminary work of theRobotics and Perception Group, ETH Zurich.From ETH Zurich:rpg_trajectory_evaluationreleased by Zichao Zhang and Davide Scaramuzza with [1].References[1] Z. Zhang and D. Scaramuzza, "A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018, pp. 7244-7251, doi: 10.1109/IROS.2018.8593941.[2] X. R. Li, Z. Zhao and X. Li, "Evaluation of Estimation Algorithms: Credibility Tests," in IEEE Transactions on Systems, Man, and Cybernetics - Part A: Systems and Humans, vol. 42, no. 1, pp. 147-163, Jan. 2012, doi: 10.1109/TSMCA.2011.2158095.[3] Joan Solà, "Quaternion kinematics for the error-state Kalman filter", 2017 arXiv, eprint: 1711.02508.LicenseSoftware License Agreement (GNU GPLv3  License), refer to the LICENSE file.Sharing is caring!-Roland Jung