Fixed #36 - Don't norm the quaterion for calculating trajectory length

rpng · Feb 28, 2020 · 235bb88 · 235bb88
1 parent 7940dd8
commit 235bb88
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp
@@ -148,7 +148,7 @@ void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths,
     std::vector<double> accum_distances(gt_poses.size());
     accum_distances[0] = 0;
     for (size_t i = 1; i < gt_poses.size(); i++) {
-        accum_distances[i] = accum_distances[i - 1] + (gt_poses[i] - gt_poses[i - 1]).norm();
+        accum_distances[i] = accum_distances[i - 1] + (gt_poses[i].block(0,0,3,1) - gt_poses[i - 1].block(0,0,3,1)).norm();
     }
 
     // Loop through each segment length

diff --git a/ov_eval/src/utils/Loader.cpp b/ov_eval/src/utils/Loader.cpp
@@ -258,7 +258,7 @@ double Loader::get_total_length(const std::vector<Eigen::Matrix<double,7,1>> &po
     // Loop through every pose and append its segment
     double distance = 0.0;
     for (size_t i=1; i<poses.size(); i++) {
-        distance += (poses[i] - poses[i-1]).norm();
+        distance += (poses[i].block(0,0,3,1) - poses[i-1].block(0,0,3,1)).norm();
     }
 
     // return the distance