Added - Ouptput of 2d ATE/RMSE error for dataset error script

ov_eval/src/calc/ResultTrajectory.cpp

@@ -110,6 +110,36 @@ void ResultTrajectory::calculate_ate(Statistics &error_ori, Statistics &error_po
 }
 
 
+void ResultTrajectory::calculate_ate_2d(Statistics &error_ori, Statistics &error_pos) {
+
+    // Clear any old data
+    error_ori.clear();
+    error_pos.clear();
+
+    // Calculate the position and orientation error at every timestep
+    for(size_t i=0; i<est_poses_aignedtoGT.size(); i++) {
+
+        // Calculate orientation error
+        Eigen::Matrix3d e_R = Math::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3,0,4,1)).transpose() * Math::quat_2_Rot(gt_poses.at(i).block(3,0,4,1));
+        double ori_err = 180.0/M_PI*Math::log_so3(e_R)(2);
+
+        // Calculate position error
+        double pos_err = (gt_poses.at(i).block(0,0,2,1)-est_poses_aignedtoGT.at(i).block(0,0,2,1)).norm();
+
+        // Append this error!
+        error_ori.timestamps.push_back(est_times.at(i));
+        error_ori.values.push_back(ori_err);
+        error_pos.timestamps.push_back(est_times.at(i));
+        error_pos.values.push_back(pos_err);
+
+    }
+
+    // Update stat information
+    error_ori.calculate();
+    error_pos.calculate();
+
+}
+
 
 
 void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths, std::map<double,std::pair<Statistics,Statistics>> &error_rpe) {

ov_eval/src/calc/ResultTrajectory.h

@@ -82,6 +82,20 @@ namespace ov_eval {
          */
         void calculate_ate(Statistics &error_ori, Statistics &error_pos);
 
+        /**
+         * @brief Computes the Absolute Trajectory Error (ATE) for this trajectory in the 2d x-y plane.
+         *
+         * This will first do our alignment of the two trajectories.
+         * We just grab the yaw component of the orientation and the xy plane error.
+         * Then at each point the error will be calculated and normed as follows:
+         * \f{align*}{
+         * e_{ATE} &= \sqrt{ \frac{1}{K} \sum_{k=1}^{K} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}^+_{k,i}||^2_{2} }
+         * \f}
+         *
+         * @param error_ori Error values for the orientation (yaw error)
+         * @param error_pos Error values for the position (xy error)
+         */
+        void calculate_ate_2d(Statistics &error_ori, Statistics &error_pos);
 
         /**
          * @brief Computes the Relative Pose Error (RPE) for this trajectory

ov_eval/src/error_dataset.cpp

@@ -117,11 +117,13 @@ int main(int argc, char **argv) {
 
         // Errors for this specific dataset (i.e. our averages over the total runs)
         ov_eval::Statistics ate_dataset_ori, ate_dataset_pos;
+        ov_eval::Statistics ate_2d_dataset_ori, ate_2d_dataset_pos;
         std::map<double,std::pair<ov_eval::Statistics,ov_eval::Statistics>> rpe_dataset;
         for(const auto& len : segments) {
             rpe_dataset.insert({len,{ov_eval::Statistics(),ov_eval::Statistics()}});
         }
         std::map<double,std::pair<ov_eval::Statistics,ov_eval::Statistics>> rmse_dataset;
+        std::map<double,std::pair<ov_eval::Statistics,ov_eval::Statistics>> rmse_2d_dataset;
         std::map<double,std::pair<ov_eval::Statistics,ov_eval::Statistics>> nees_dataset;
 
         // Loop though the different runs for this dataset
@@ -147,6 +149,17 @@ int main(int argc, char **argv) {
                     assert(error_ori.timestamps.at(j)==error_pos.timestamps.at(j));
                 }
 
+                // Calculate ATE 2D error for this dataset
+                ov_eval::Statistics error_ori_2d, error_pos_2d;
+                traj.calculate_ate_2d(error_ori_2d, error_pos_2d);
+                ate_2d_dataset_ori.values.push_back(error_ori_2d.rmse);
+                ate_2d_dataset_pos.values.push_back(error_pos_2d.rmse);
+                for(size_t j=0; j<error_ori_2d.values.size(); j++) {
+                    rmse_2d_dataset[error_ori_2d.timestamps.at(j)].first.values.push_back(error_ori_2d.values.at(j));
+                    rmse_2d_dataset[error_pos_2d.timestamps.at(j)].second.values.push_back(error_pos_2d.values.at(j));
+                    assert(error_ori_2d.timestamps.at(j)==error_pos_2d.timestamps.at(j));
+                }
+
                 // NEES error for this dataset
                 ov_eval::Statistics nees_ori, nees_pos;
                 traj.calculate_nees(nees_ori, nees_pos);
@@ -182,10 +195,14 @@ int main(int argc, char **argv) {
         // Compute our mean ATE score
         ate_dataset_ori.calculate();
         ate_dataset_pos.calculate();
+        ate_2d_dataset_ori.calculate();
+        ate_2d_dataset_pos.calculate();
 
         // Print stats for this specific dataset
         ROS_INFO("\tATE: mean_ori = %.3f | mean_pos = %.3f",ate_dataset_ori.mean,ate_dataset_pos.mean);
         ROS_INFO("\tATE: std_ori  = %.5f | std_pos  = %.5f",ate_dataset_ori.std,ate_dataset_pos.std);
+        ROS_INFO("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f",ate_2d_dataset_ori.mean,ate_2d_dataset_pos.mean);
+        ROS_INFO("\tATE 2D: std_ori  = %.5f | std_pos  = %.5f",ate_2d_dataset_ori.std,ate_2d_dataset_pos.std);
         for(auto &seg : rpe_dataset) {
             seg.second.first.calculate();
             seg.second.second.calculate();
@@ -209,6 +226,22 @@ int main(int argc, char **argv) {
         rmse_pos.calculate();
         ROS_INFO("\tRMSE: mean_ori = %.3f | mean_pos = %.3f",rmse_ori.mean,rmse_pos.mean);
 
+        // RMSE: Convert into the right format (only use times where all runs have an error)
+        ov_eval::Statistics rmse_2d_ori, rmse_2d_pos;
+        for(auto &elm : rmse_2d_dataset) {
+            if(elm.second.first.values.size()==total_runs) {
+                elm.second.first.calculate();
+                elm.second.second.calculate();
+                rmse_2d_ori.timestamps.push_back(elm.first);
+                rmse_2d_ori.values.push_back(elm.second.first.rmse);
+                rmse_2d_pos.timestamps.push_back(elm.first);
+                rmse_2d_pos.values.push_back(elm.second.second.rmse);
+            }
+        }
+        rmse_2d_ori.calculate();
+        rmse_2d_pos.calculate();
+        ROS_INFO("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f",rmse_2d_ori.mean,rmse_2d_pos.mean);
+
         // NEES: Convert into the right format (only use times where all runs have an error)
         ov_eval::Statistics nees_ori, nees_pos;
         for(auto &elm : nees_dataset) {

ov_msckf/launch/pgeneva_sim.launch

@@ -160,4 +160,4 @@
     </group>
 
 
-</launch>
+</launch>