Add CSV export to the evaluation tool (#1159)

- adds `covered_distance` to the ground truth relations
- the flags `write_relation_metrics` and `relation_metrics_filename` control whether and where to write the relations metrics as comma-separated values

cartographer/ground_truth/autogenerate_ground_truth_main.cc

@@ -121,9 +121,10 @@ proto::GroundTruth GenerateGroundTruth(
         submap_to_node_index.at(constraint.submap_id().submap_index());
 
     // Covered distance between the two should not be too small.
-    if (std::abs(covered_distance.at(matched_node) -
+    double covered_distance_in_constraint =
-                 covered_distance.at(representative_node)) <
+        std::abs(covered_distance.at(matched_node) -
-        min_covered_distance) {
+                 covered_distance.at(representative_node));
+    if (covered_distance_in_constraint < min_covered_distance) {
       continue;
     }
 
@@ -157,6 +158,7 @@ proto::GroundTruth GenerateGroundTruth(
         trajectory.node(representative_node).timestamp());
     new_relation->set_timestamp2(trajectory.node(matched_node).timestamp());
     *new_relation->mutable_expected() = transform::ToProto(expected);
+    new_relation->set_covered_distance(covered_distance_in_constraint);
   }
   LOG(INFO) << "Generated " << ground_truth.relation_size()
             << " relations and ignored " << num_outliers << " outliers.";

cartographer/ground_truth/compute_relations_metrics_main.cc

@@ -43,6 +43,9 @@ DEFINE_string(relations_filename, "",
 DEFINE_bool(read_text_file_with_unix_timestamps, false,
             "Enable support for the relations text files as in the paper. "
             "Default is to read from a GroundTruth proto file.");
+DEFINE_bool(write_relation_metrics, false,
+            "Enable exporting relation metrics as comma-separated values to "
+            "[pose_graph_filename].relation_metrics.csv");
 
 namespace cartographer {
 namespace ground_truth {
@@ -109,6 +112,46 @@ std::string StatisticsString(const std::vector<Error>& errors) {
          MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
 }
 
+void WriteRelationMetricsToFile(const std::vector<Error>& errors,
+                                const proto::GroundTruth& ground_truth,
+                                const std::string& relation_metrics_filename) {
+  std::ofstream relation_errors_file;
+  std::string log_file_path;
+  LOG(INFO) << "Writing relation metrics to '" + relation_metrics_filename +
+                   "'...";
+  relation_errors_file.open(relation_metrics_filename);
+  relation_errors_file
+      << "translational_error,squared_translational_error,rotational_"
+         "errors_degree,squared_rotational_errors_degree,"
+         "expected_translation_x,expected_translation_y,expected_"
+         "translation_z,expected_rotation_w,expected_rotation_x,"
+         "expected_rotation_y,expected_rotation_z,covered_distance\n";
+  for (size_t relation_index = 0; relation_index < ground_truth.relation_size();
+       ++relation_index) {
+    const Error& error = errors[relation_index];
+    const proto::Relation& relation = ground_truth.relation(relation_index);
+    double translational_error = std::sqrt(error.translational_squared);
+    double squared_translational_error = error.translational_squared;
+    double rotational_errors_degree =
+        common::RadToDeg(std::sqrt(error.rotational_squared));
+    double squared_rotational_errors_degree =
+        common::Pow2(rotational_errors_degree);
+    relation_errors_file << translational_error << ","
+                         << squared_translational_error << ","
+                         << rotational_errors_degree << ","
+                         << squared_rotational_errors_degree << ","
+                         << relation.expected().translation().x() << ","
+                         << relation.expected().translation().y() << ","
+                         << relation.expected().translation().z() << ","
+                         << relation.expected().rotation().w() << ","
+                         << relation.expected().rotation().x() << ","
+                         << relation.expected().rotation().y() << ","
+                         << relation.expected().rotation().z() << ","
+                         << relation.covered_distance() << "\n";
+  }
+  relation_errors_file.close();
+}
+
 transform::Rigid3d LookupTransform(
     const transform::TransformInterpolationBuffer&
         transform_interpolation_buffer,
@@ -126,7 +169,8 @@ transform::Rigid3d LookupTransform(
 
 void Run(const std::string& pose_graph_filename,
          const std::string& relations_filename,
-         const bool read_text_file_with_unix_timestamps) {
+         const bool read_text_file_with_unix_timestamps,
+         const bool write_relation_metrics) {
   LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
   mapping::proto::PoseGraph pose_graph;
   {
@@ -162,6 +206,12 @@ void Run(const std::string& pose_graph_filename,
     errors.push_back(ComputeError(pose1, pose2, expected));
   }
 
+  const std::string relation_metrics_filename =
+      pose_graph_filename + ".relation_metrics.csv";
+  if (write_relation_metrics) {
+    WriteRelationMetricsToFile(errors, ground_truth, relation_metrics_filename);
+  }
+
   LOG(INFO) << "Result:\n" << StatisticsString(errors);
 }
 
@@ -184,7 +234,8 @@ int main(int argc, char** argv) {
     google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics");
     return EXIT_FAILURE;
   }
-  ::cartographer::ground_truth::Run(FLAGS_pose_graph_filename,
+
-                                    FLAGS_relations_filename,
+  ::cartographer::ground_truth::Run(
-                                    FLAGS_read_text_file_with_unix_timestamps);
+      FLAGS_pose_graph_filename, FLAGS_relations_filename,
+      FLAGS_read_text_file_with_unix_timestamps, FLAGS_write_relation_metrics);
 }

cartographer/ground_truth/proto/relations.proto

@@ -25,6 +25,7 @@ message Relation {
   // The 'expected' relative transform of the tracking frame from 'timestamp2'
   // to 'timestamp1'.
   transform.proto.Rigid3d expected = 3;
+  double covered_distance = 4;
 }
 
 message GroundTruth {