Adds a tool to generate ground truth data. (#340)

Fixes #310. Not yet tested.

CMakeLists.txt

@@ -89,6 +89,11 @@ configure_file(
   ${PROJECT_SOURCE_DIR}/cartographer/common/config.h.cmake
   ${PROJECT_BINARY_DIR}/cartographer/common/config.h)
 
+google_binary(cartographer_autogenerate_ground_truth
+  SRCS
+    cartographer/ground_truth/autogenerate_ground_truth_main.cc
+)
+
 google_binary(cartographer_compute_relations_metrics
   SRCS
     cartographer/ground_truth/compute_relations_metrics_main.cc

cartographer/ground_truth/autogenerate_ground_truth_main.cc

@@ -0,0 +1,206 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <cmath>
+#include <fstream>
+#include <string>
+
+#include "cartographer/common/port.h"
+#include "cartographer/ground_truth/proto/relations.pb.h"
+#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
+#include "cartographer/transform/transform.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(
+    pose_graph_filename, "",
+    "File with the pose graph proto from which to generate ground truth data.");
+DEFINE_string(output_filename, "", "File to write the ground truth proto to.");
+DEFINE_double(min_covered_distance, 100.,
+              "Minimum covered distance in meters before a loop closure is "
+              "considered a candidate for autogenerated ground truth.");
+DEFINE_double(outlier_threshold, 0.15,
+              "Distance in meters beyond which constraints are considered "
+              "outliers.");
+
+namespace cartographer {
+namespace ground_truth {
+namespace {
+
+std::vector<double> ComputeCoveredDistance(
+    const mapping::proto::Trajectory& trajectory) {
+  std::vector<double> covered_distance;
+  covered_distance.push_back(0.);
+  CHECK_GT(trajectory.node_size(), 0)
+      << "Trajectory does not contain any nodes.";
+  for (int i = 1; i < trajectory.node_size(); ++i) {
+    const auto last_pose = transform::ToRigid3(trajectory.node(i - 1).pose());
+    const auto this_pose = transform::ToRigid3(trajectory.node(i).pose());
+    covered_distance.push_back(
+        covered_distance.back() +
+        (last_pose.inverse() * this_pose).translation().norm());
+  }
+  return covered_distance;
+}
+
+// We pick the representative scan in the middle of the submap.
+//
+// TODO(whess): Should we consider all scans inserted into the submap and
+// excluded, e.g. based on large relative linear or angular distance?
+std::vector<int> ComputeSubmapRepresentativeScan(
+    const mapping::proto::SparsePoseGraph& pose_graph) {
+  std::vector<int> submap_to_scan_index;
+  for (const auto& constraint : pose_graph.constraint()) {
+    if (constraint.tag() !=
+        mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
+      continue;
+    }
+    CHECK_EQ(constraint.submap_id().trajectory_id(), 0);
+    CHECK_EQ(constraint.scan_id().trajectory_id(), 0);
+
+    const int next_submap_index = static_cast<int>(submap_to_scan_index.size());
+    const int submap_index = constraint.submap_id().submap_index();
+    if (submap_index <= next_submap_index) {
+      continue;
+    }
+
+    CHECK_EQ(submap_index, next_submap_index + 1);
+    submap_to_scan_index.push_back(constraint.scan_id().scan_index());
+  }
+  return submap_to_scan_index;
+}
+
+proto::GroundTruth GenerateGroundTruth(
+    const mapping::proto::SparsePoseGraph& pose_graph,
+    const double min_covered_distance, const double outlier_threshold) {
+  const mapping::proto::Trajectory& trajectory = pose_graph.trajectory(0);
+  const std::vector<double> covered_distance =
+      ComputeCoveredDistance(trajectory);
+
+  const std::vector<int> submap_to_scan_index =
+      ComputeSubmapRepresentativeScan(pose_graph);
+
+  int num_outliers = 0;
+  proto::GroundTruth ground_truth;
+  for (const auto& constraint : pose_graph.constraint()) {
+    // We're only interested in loop closure constraints.
+    if (constraint.tag() ==
+        mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
+      continue;
+    }
+
+    // For some submaps at the very end, we have not chosen a representative
+    // scan, but those should not be part of loop closure anyway.
+    CHECK_EQ(constraint.submap_id().trajectory_id(), 0);
+    CHECK_EQ(constraint.scan_id().trajectory_id(), 0);
+    if (constraint.submap_id().submap_index() >=
+        static_cast<int>(submap_to_scan_index.size())) {
+      continue;
+    }
+    const int matched_scan = constraint.scan_id().scan_index();
+    const int representative_scan =
+        submap_to_scan_index.at(constraint.submap_id().submap_index());
+
+    // Covered distance between the two should not be too small.
+    if (std::abs(covered_distance.at(matched_scan) -
+                 covered_distance.at(representative_scan)) <
+        min_covered_distance) {
+      continue;
+    }
+
+    // Compute the transform between the scans according to the solution and
+    // the constraint.
+    const auto solution_pose1 =
+        transform::ToRigid3(trajectory.node(representative_scan).pose());
+    const auto solution_pose2 =
+        transform::ToRigid3(trajectory.node(matched_scan).pose());
+    const auto solution = solution_pose1.inverse() * solution_pose2;
+
+    const auto scan_to_submap_constraint =
+        transform::ToRigid3(constraint.relative_pose());
+    const auto submap_solution = transform::ToRigid3(
+        trajectory.submap(constraint.submap_id().submap_index()).pose());
+    const auto error =
+        submap_solution * scan_to_submap_constraint * solution_pose2.inverse();
+
+    if (error.translation().norm() > outlier_threshold) {
+      ++num_outliers;
+    }
+    auto* const new_relation = ground_truth.add_relation();
+    new_relation->set_timestamp1(
+        trajectory.node(representative_scan).timestamp());
+    new_relation->set_timestamp2(trajectory.node(matched_scan).timestamp());
+    *new_relation->mutable_expected() = transform::ToProto(solution);
+  }
+  LOG(INFO) << "Generated " << ground_truth.relation_size()
+            << " relations and ignored " << num_outliers << " outliers.";
+  return ground_truth;
+}
+
+void Run(const string& pose_graph_filename, const string& output_filename,
+         const double min_covered_distance, const double outlier_threshold) {
+  LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
+  mapping::proto::SparsePoseGraph pose_graph;
+  {
+    std::ifstream stream(pose_graph_filename.c_str());
+    CHECK(pose_graph.ParseFromIstream(&stream));
+    CHECK_EQ(pose_graph.trajectory_size(), 1)
+        << "Only pose graphs containing a single trajectory are supported.";
+  }
+  LOG(INFO) << "Autogenerating ground truth relations...";
+  const proto::GroundTruth ground_truth =
+      GenerateGroundTruth(pose_graph, min_covered_distance, outlier_threshold);
+  LOG(INFO) << "Writing " << ground_truth.relation_size() << " relations to '"
+            << output_filename << "'.";
+  {
+    std::ofstream output_stream(output_filename,
+                                std::ios_base::out | std::ios_base::binary);
+    CHECK(ground_truth.SerializeToOstream(&output_stream))
+        << "Could not serialize ground truth data.";
+    output_stream.close();
+    CHECK(output_stream) << "Could not write ground truth data.";
+  }
+}
+
+}  // namespace
+}  // namespace ground_truth
+}  // namespace cartographer
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  FLAGS_logtostderr = true;
+  google::SetUsageMessage(
+      "\n\n"
+      "This program semi-automatically generates ground truth data from a\n"
+      "pose graph proto.\n"
+      "\n"
+      "The input should contain a single trajectory and should have been\n"
+      "manually assessed to be correctly loop closed. Small local distortions\n"
+      "are acceptable if they are tiny compared to the errors we want to\n"
+      "assess using the generated ground truth data.\n"
+      "\n"
+      "All loop closure constraints separated by long covered distance are\n"
+      "included in the output. Outliers are removed.\n");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  if (FLAGS_pose_graph_filename.empty() || FLAGS_output_filename.empty()) {
+    google::ShowUsageWithFlagsRestrict(argv[0], "autogenerate_ground_truth");
+    return EXIT_FAILURE;
+  }
+  ::cartographer::ground_truth::Run(
+      FLAGS_pose_graph_filename, FLAGS_output_filename,
+      FLAGS_min_covered_distance, FLAGS_outlier_threshold);
+}

cartographer/ground_truth/proto/relations.proto

@@ -0,0 +1,32 @@
+// Copyright 2016 The Cartographer Authors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+syntax = "proto2";
+
+package cartographer.ground_truth.proto;
+
+import "cartographer/transform/proto/transform.proto";
+
+message Relation {
+  optional int64 timestamp1 = 1;
+  optional int64 timestamp2 = 2;
+
+  // The 'expected' relative transform of the tracking frame from 'timestamp2'
+  // to 'timestamp1'.
+  optional transform.proto.Rigid3d expected = 3;
+}
+
+message GroundTruth {
+  repeated Relation relation = 1;
+}