Fixed - RPE trajectory segment selection if there is large gaps betwe…

…en groundtruth poses, also warn the user if this is the case. Before if there a dropped message in the groundtruth or large pos delta, it would break early and wouldn't get all valid trajectory segments past the point where a segment failed to be found.

ov_eval/launch/record.launch

@@ -2,7 +2,7 @@
 
 
     <!-- what ros bag to play -->
-    <arg name="bag_name"    default="outdoor_forward_5_snapdragon_with_gt" />
+    <arg name="bag_name"    default="indoor_forward_5_snapdragon_with_gt" />
     <arg name="bag_path"    default="/media/patrick/RPNG\ FLASH\ 2/uzhfpv_newer" />
 
 

ov_eval/src/calc/ResultTrajectory.cpp

@@ -145,10 +145,22 @@ void ResultTrajectory::calculate_ate_2d(Statistics &error_ori, Statistics &error
 void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths, std::map<double,std::pair<Statistics,Statistics>> &error_rpe) {
 
     // Distance at each point along the trajectory
+    double average_pos_difference = 0;
     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].block(0,0,3,1) - gt_poses[i - 1].block(0,0,3,1)).norm();
+        double pos_diff = (gt_poses[i].block(0,0,3,1) - gt_poses[i - 1].block(0,0,3,1)).norm();
+        accum_distances[i] = accum_distances[i - 1] + pos_diff;
+        average_pos_difference += pos_diff;
+    }
+    average_pos_difference /= gt_poses.size();
+
+    // Warn if large pos difference
+    double max_dist_diff = 0.5;
+    if(average_pos_difference > max_dist_diff) {
+        printf(YELLOW "[COMP]: average groundtruth position difference %.2f > %.2f is too large\n" RESET, average_pos_difference, max_dist_diff);
+        printf(YELLOW "[COMP]: this will prevent the RPE from finding valid trajectory segments!!!\n" RESET);
+        printf(YELLOW "[COMP]: the recommendation is to use a higher frequency groundtruth, or relax this trajectory segment logic...\n" RESET);
     }
 
     // Loop through each segment length
@@ -158,7 +170,10 @@ void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths,
         Statistics error_ori, error_pos;
 
         // Get end of subtrajectories for each possible starting point
-        std::vector<size_t> comparisons = compute_comparison_indices_length(accum_distances, distance, 0.4*distance);
+        // NOTE: is there a better way to select which end pos is a valid segments that are of the correct lenght?
+        // NOTE: right now this allows for longer segments to have bigger error in their start-end distance vs the desired segment length
+        //std::vector<size_t> comparisons = compute_comparison_indices_length(accum_distances, distance, 0.1*distance);
+        std::vector<size_t> comparisons = compute_comparison_indices_length(accum_distances, distance, max_dist_diff);
 
         // Loop through each relative comparison
         for (size_t id_start = 0; id_start < comparisons.size(); id_start++) {
@@ -178,7 +193,7 @@ void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths,
             T_c2.block(0, 3, 3, 1) = est_poses_aignedtoGT.at(id_end).block(0,0,3,1);
 
             // Get T I2 to I1 EST
-            Eigen::Matrix4d T_c1_c2 = T_c1.inverse() * T_c2;
+            Eigen::Matrix4d T_c1_c2 = Math::Inv_se3(T_c1) * T_c2;
 
             //===============================================================================
             // Get T I1 to world GT at beginning of subtrajectory (at state idx)
@@ -192,11 +207,11 @@ void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths,
             T_m2.block(0, 3, 3, 1) = gt_poses.at(id_end).block(0,0,3,1);
 
             // Get T I2 to I1 GT
-            Eigen::Matrix4d T_m1_m2 = T_m1.inverse() * T_m2;
+            Eigen::Matrix4d T_m1_m2 = Math::Inv_se3(T_m1) * T_m2;
 
             //===============================================================================
             // Compute error transform between EST and GT start-end transform
-            Eigen::Matrix4d T_error_in_c2 = T_m1_m2.inverse() * T_c1_c2;
+            Eigen::Matrix4d T_error_in_c2 = Math::Inv_se3(T_m1_m2) * T_c1_c2;
 
             Eigen::Matrix4d T_c2_rot = Eigen::Matrix4d::Identity();
             T_c2_rot.block(0, 0, 3, 3) = T_c2.block(0, 0, 3, 3);

ov_eval/src/calc/ResultTrajectory.h

@@ -174,31 +174,30 @@ namespace ov_eval {
          */
         std::vector<size_t> compute_comparison_indices_length(std::vector<double> &distances, double distance, double max_dist_diff) {
 
-            // Our max id and the vector of end ids for our pose indexes
-            int max_idx = distances.size() - 1;
+            // Vector of end ids for our pose indexes
             std::vector<size_t> comparisons;
 
             // Loop through each pose in our trajectory (i.e. our distance vector generated from the trajectory).
             for (size_t idx = 0; idx < distances.size(); idx++) {
 
                 // Loop through and find the pose that minimized the difference between
                 // The desired trajectory distance and our current trajectory distance
-                double distance_startpose = distances[idx];
+                double distance_startpose = distances.at(idx);
                 int best_idx = -1;
                 double best_error = max_dist_diff;
-                for (int i = idx; i < max_idx; i++) {
-                    if (std::abs(distances[i] - (distance_startpose + distance)) < best_error) {
+                for (size_t i = idx; i < distances.size(); i++) {
+                    if (std::abs(distances.at(i) - (distance_startpose + distance)) < best_error) {
                         best_idx = i;
-                        best_error = std::abs(distances[i] - (distance_startpose + distance));
+                        best_error = std::abs(distances.at(i) - (distance_startpose + distance));
                     }
                 }
 
                 // If we have an end id that reached this trajectory distance then add it!
-                // Else we can break since we are at the end of the trajectory and thus won't find any more segments of this length
+                // Else this isn't a valid segment, thus we shouldn't add it (we will try again at the next pose)
+                // NOTE: just because we searched through all poses and didn't find a close one doesn't mean we have ended
+                // NOTE: this could happen if there is a gap in the groundtruth poses and we just couldn't find a pose with low error
                 if (best_idx != -1) {
                     comparisons.push_back(best_idx);
-                } else {
-                    break;
                 }
 
             }

ov_eval/src/error_comparison.cpp

@@ -97,10 +97,11 @@ int main(int argc, char **argv) {
     }
 
     // Relative pose error segment lengths
-    std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0, 48.0};
+    //std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0, 48.0};
     //std::vector<double> segments = {7.0, 14.0, 21.0, 28.0, 35.0};
     //std::vector<double> segments = {10.0, 25.0, 50.0, 75.0, 120.0};
     //std::vector<double> segments = {5.0, 15.0, 30.0, 45.0, 60.0};
+    std::vector<double> segments = {40.0, 60.0, 80.0, 100.0, 120.0};
 
     // The overall RPE error calculation for each algorithm type
     std::map<std::string,std::map<double,std::pair<ov_eval::Statistics,ov_eval::Statistics>>> algo_rpe;

ov_eval/src/utils/Math.h

@@ -409,6 +409,26 @@ namespace ov_eval {
             return mat;
         }
 
+        /**
+         * @brief SE(3) matrix analytical inverse
+         *
+         * It seems that using the .inverse() function is not a good way.
+         * This should be used in all cases we need the inverse instead of numerical inverse.
+         * https://github.com/rpng/open_vins/issues/12
+         * \f{align*}{
+         * \mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^\top & -\mathbf{R}^\top\mathbf{p} \\ \mathbf{0} & 1 \end{bmatrix}
+         * \f}
+         *
+         * @param[in] T SE(3) matrix
+         * @return inversed SE(3) matrix
+         */
+        static inline Eigen::Matrix4d Inv_se3(const Eigen::Matrix4d &T) {
+            Eigen::Matrix4d Tinv = Eigen::Matrix4d::Identity();
+            Tinv.block(0,0,3,3) = T.block(0,0,3,3).transpose();
+            Tinv.block(0,3,3,1) = -Tinv.block(0,0,3,3)*T.block(0,3,3,1);
+            return Tinv;
+        }
+
         /**
          * @brief JPL Quaternion inverse
          *