Add ground truth proto support for trajectory evaluation. (#349)

This adds support of GroundTruth protos in addition to
text files.

Fixes the ground truth generation code to skip outliers.

Improves the ground truth generation to use expected
relative poses instead of the solution.

Writes constraints including the roll and pitch from the
node for 2D SLAM, so that constraints correctly capture
the expected transform between the 3D poses of scan and
submap in 2D SLAM.

Related to #239.
Related to #310.

cartographer/ground_truth/autogenerate_ground_truth_main.cc

@@ -32,9 +32,12 @@ 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,
+DEFINE_double(outlier_threshold_meters, 0.15,
               "Distance in meters beyond which constraints are considered "
               "outliers.");
+DEFINE_double(outlier_threshold_radians, 0.02,
+              "Distance in radians beyond which constraints are considered "
+              "outliers.");
 
 namespace cartographer {
 namespace ground_truth {
@@ -59,7 +62,7 @@ std::vector<double> ComputeCoveredDistance(
 // 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?
+// exclude, 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;
@@ -85,7 +88,8 @@ std::vector<int> ComputeSubmapRepresentativeScan(
 
 proto::GroundTruth GenerateGroundTruth(
     const mapping::proto::SparsePoseGraph& pose_graph,
-    const double min_covered_distance, const double outlier_threshold) {
+    const double min_covered_distance, const double outlier_threshold_meters,
+    const double outlier_threshold_radians) {
   const mapping::proto::Trajectory& trajectory = pose_graph.trajectory(0);
   const std::vector<double> covered_distance =
       ComputeCoveredDistance(trajectory);
@@ -123,27 +127,34 @@ proto::GroundTruth GenerateGroundTruth(
 
     // Compute the transform between the scans according to the solution and
     // the constraint.
-    const auto solution_pose1 =
+    const transform::Rigid3d solution_pose1 =
         transform::ToRigid3(trajectory.node(representative_scan).pose());
-    const auto solution_pose2 =
+    const transform::Rigid3d solution_pose2 =
         transform::ToRigid3(trajectory.node(matched_scan).pose());
-    const auto solution = solution_pose1.inverse() * solution_pose2;
+    const transform::Rigid3d solution =
+        solution_pose1.inverse() * solution_pose2;
 
-    const auto scan_to_submap_constraint =
+    const transform::Rigid3d submap_solution = transform::ToRigid3(
-        transform::ToRigid3(constraint.relative_pose());
-    const auto submap_solution = transform::ToRigid3(
         trajectory.submap(constraint.submap_id().submap_index()).pose());
-    const auto error =
+    const transform::Rigid3d submap_solution_to_scan_solution =
-        submap_solution * scan_to_submap_constraint * solution_pose2.inverse();
+        solution_pose1.inverse() * submap_solution;
+    const transform::Rigid3d scan_to_submap_constraint =
+        transform::ToRigid3(constraint.relative_pose());
+    const transform::Rigid3d expected =
+        submap_solution_to_scan_solution * scan_to_submap_constraint;
 
-    if (error.translation().norm() > outlier_threshold) {
+    const transform::Rigid3d error = solution * expected.inverse();
+
+    if (error.translation().norm() > outlier_threshold_meters ||
+        transform::GetAngle(error) > outlier_threshold_radians) {
       ++num_outliers;
+      continue;
     }
     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);
+    *new_relation->mutable_expected() = transform::ToProto(expected);
   }
   LOG(INFO) << "Generated " << ground_truth.relation_size()
             << " relations and ignored " << num_outliers << " outliers.";
@@ -151,7 +162,9 @@ proto::GroundTruth GenerateGroundTruth(
 }
 
 void Run(const string& pose_graph_filename, const string& output_filename,
-         const double min_covered_distance, const double outlier_threshold) {
+         const double min_covered_distance,
+         const double outlier_threshold_meters,
+         const double outlier_threshold_radians) {
   LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
   mapping::proto::SparsePoseGraph pose_graph;
   {
@@ -162,7 +175,8 @@ void Run(const string& pose_graph_filename, const string& output_filename,
   }
   LOG(INFO) << "Autogenerating ground truth relations...";
   const proto::GroundTruth ground_truth =
-      GenerateGroundTruth(pose_graph, min_covered_distance, outlier_threshold);
+      GenerateGroundTruth(pose_graph, min_covered_distance,
+                          outlier_threshold_meters, outlier_threshold_radians);
   LOG(INFO) << "Writing " << ground_truth.relation_size() << " relations to '"
             << output_filename << "'.";
   {
@@ -202,5 +216,6 @@ int main(int argc, char** argv) {
   }
   ::cartographer::ground_truth::Run(
       FLAGS_pose_graph_filename, FLAGS_output_filename,
-      FLAGS_min_covered_distance, FLAGS_outlier_threshold);
+      FLAGS_min_covered_distance, FLAGS_outlier_threshold_meters,
+      FLAGS_outlier_threshold_radians);
 }

cartographer/ground_truth/compute_relations_metrics_main.cc

@@ -25,7 +25,9 @@
 
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
-#include "cartographer/mapping/proto/trajectory.pb.h"
+#include "cartographer/ground_truth/proto/relations.pb.h"
+#include "cartographer/ground_truth/relations_text_file.h"
+#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
@@ -33,31 +35,28 @@
 #include "glog/logging.h"
 
 DEFINE_string(
-    trajectory_filename, "",
+    pose_graph_filename, "",
-    "Proto containing the trajectory of which to assess the quality.");
+    "File with the pose graph proto from which to assess the quality.");
 DEFINE_string(relations_filename, "",
               "Relations file containing the ground truth.");
+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.");
 
 namespace cartographer {
 namespace ground_truth {
 namespace {
 
-transform::Rigid3d LookupPose(const transform::TransformInterpolationBuffer&
-                                  transform_interpolation_buffer,
-                              const double time) {
-  constexpr int64 kUtsTicksPerSecond = 10000000;
-  common::Time common_time =
-      common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds *
-                            kUtsTicksPerSecond) +
-      common::FromSeconds(time);
-  return transform_interpolation_buffer.Lookup(common_time);
-}
-
 struct Error {
   double translational_squared;
   double rotational_squared;
 };
 
+// TODO(whess): This gives different results for the translational error if
+// 'pose1' and 'pose2' are swapped and 'expected' is inverted. Consider a
+// different way to compute translational error. Maybe just look at the
+// absolute difference in translation norms of each relative transform as a
+// lower bound of the translational error.
 Error ComputeError(const transform::Rigid3d& pose1,
                    const transform::Rigid3d& pose2,
                    const transform::Rigid3d& expected) {
@@ -109,33 +108,40 @@ string StatisticsString(const std::vector<Error>& errors) {
          MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
 }
 
-void Run(const string& trajectory_filename, const string& relations_filename) {
+void Run(const string& pose_graph_filename, const string& relations_filename,
-  LOG(INFO) << "Reading trajectory from '" << trajectory_filename << "'...";
+         const bool read_text_file_with_unix_timestamps) {
-  mapping::proto::Trajectory trajectory_proto;
+  LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
+  mapping::proto::SparsePoseGraph pose_graph;
   {
-    std::ifstream trajectory_stream(trajectory_filename.c_str(),
+    std::ifstream stream(pose_graph_filename.c_str());
-                                    std::ios::binary);
+    CHECK(pose_graph.ParseFromIstream(&stream));
-    CHECK(trajectory_proto.ParseFromIstream(&trajectory_stream));
+    CHECK_EQ(pose_graph.trajectory_size(), 1)
+        << "Only pose graphs containing a single trajectory are supported.";
+  }
+  const auto transform_interpolation_buffer =
+      transform::TransformInterpolationBuffer::FromTrajectory(
+          pose_graph.trajectory(0));
+
+  proto::GroundTruth ground_truth;
+  if (read_text_file_with_unix_timestamps) {
+    LOG(INFO) << "Reading relations from '" << relations_filename << "'...";
+    ground_truth = ReadRelationsTextFile(relations_filename);
+  } else {
+    LOG(INFO) << "Reading ground truth from '" << relations_filename << "'...";
+    std::ifstream ground_truth_stream(relations_filename.c_str(),
+                                      std::ios::binary);
+    CHECK(ground_truth.ParseFromIstream(&ground_truth_stream));
   }
 
-  const auto transform_interpolation_buffer =
-      transform::TransformInterpolationBuffer::FromTrajectory(trajectory_proto);
-
-  LOG(INFO) << "Reading relations from '" << relations_filename << "'...";
   std::vector<Error> errors;
-  {
+  for (const auto& relation : ground_truth.relation()) {
-    std::ifstream relations_stream(relations_filename.c_str());
+    const auto pose1 = transform_interpolation_buffer->Lookup(
-    double time1, time2, x, y, z, roll, pitch, yaw;
+        common::FromUniversal(relation.timestamp1()));
-    while (relations_stream >> time1 >> time2 >> x >> y >> z >> roll >> pitch >>
+    const auto pose2 = transform_interpolation_buffer->Lookup(
-           yaw) {
+        common::FromUniversal(relation.timestamp2()));
-      const auto pose1 = LookupPose(*transform_interpolation_buffer, time1);
+    const transform::Rigid3d expected =
-      const auto pose2 = LookupPose(*transform_interpolation_buffer, time2);
+        transform::ToRigid3(relation.expected());
-      const transform::Rigid3d expected =
+    errors.push_back(ComputeError(pose1, pose2, expected));
-          transform::Rigid3d(transform::Rigid3d::Vector(x, y, z),
-                             transform::RollPitchYaw(roll, pitch, yaw));
-      errors.push_back(ComputeError(pose1, pose2, expected));
-    }
-    CHECK(relations_stream.eof());
   }
 
   LOG(INFO) << "Result:\n" << StatisticsString(errors);
@@ -153,16 +159,14 @@ int main(int argc, char** argv) {
       "This program computes the relation based metric described in:\n"
       "R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti,\n"
       "C. Stachniss, and A. Kleiner, \"On measuring the accuracy of SLAM\n"
-      "algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.\n"
+      "algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.");
-      "\n"
-      "Note: Timestamps in the relations_file are interpreted relative to\n"
-      "      the Unix epoch.");
   google::ParseCommandLineFlags(&argc, &argv, true);
 
-  if (FLAGS_trajectory_filename.empty() || FLAGS_relations_filename.empty()) {
+  if (FLAGS_pose_graph_filename.empty() || FLAGS_relations_filename.empty()) {
     google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics");
     return EXIT_FAILURE;
   }
-  ::cartographer::ground_truth::Run(FLAGS_trajectory_filename,
+  ::cartographer::ground_truth::Run(FLAGS_pose_graph_filename,
-                                    FLAGS_relations_filename);
+                                    FLAGS_relations_filename,
+                                    FLAGS_read_text_file_with_unix_timestamps);
 }

cartographer/ground_truth/relations_text_file.cc

@@ -0,0 +1,61 @@
+/*
+ * 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 "cartographer/ground_truth/relations_text_file.h"
+
+#include <fstream>
+
+#include "cartographer/common/time.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace ground_truth {
+
+namespace {
+
+common::Time UnixToCommonTime(double unix_time) {
+  constexpr int64 kUtsTicksPerSecond = 10000000;
+  return common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds *
+                               kUtsTicksPerSecond) +
+         common::FromSeconds(unix_time);
+}
+
+}  // namespace
+
+proto::GroundTruth ReadRelationsTextFile(const string& relations_filename) {
+  proto::GroundTruth ground_truth;
+  std::ifstream relations_stream(relations_filename.c_str());
+  double unix_time_1, unix_time_2, x, y, z, roll, pitch, yaw;
+  while (relations_stream >> unix_time_1 >> unix_time_2 >> x >> y >> z >>
+         roll >> pitch >> yaw) {
+    const common::Time common_time_1 = UnixToCommonTime(unix_time_1);
+    const common::Time common_time_2 = UnixToCommonTime(unix_time_2);
+    const transform::Rigid3d expected =
+        transform::Rigid3d(transform::Rigid3d::Vector(x, y, z),
+                           transform::RollPitchYaw(roll, pitch, yaw));
+    auto* const new_relation = ground_truth.add_relation();
+    new_relation->set_timestamp1(common::ToUniversal(common_time_1));
+    new_relation->set_timestamp2(common::ToUniversal(common_time_2));
+    *new_relation->mutable_expected() = transform::ToProto(expected);
+  }
+  CHECK(relations_stream.eof());
+  return ground_truth;
+}
+
+}  // namespace ground_truth
+}  // namespace cartographer

cartographer/ground_truth/relations_text_file.h

@@ -0,0 +1,41 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_
+#define CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_
+
+#include <string>
+
+#include "cartographer/common/port.h"
+#include "cartographer/ground_truth/proto/relations.pb.h"
+
+namespace cartographer {
+namespace ground_truth {
+
+// Reads a text file and converts it to a GroundTruth proto. Each line contains:
+// time1 time2 x y z roll pitch yaw
+// using Unix epoch timestamps.
+//
+// This is the format used in the relations files provided for:
+// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
+// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
+// Robots, vol. 27, no. 4, pp. 387–407, 2009.
+proto::GroundTruth ReadRelationsTextFile(const string& relations_filename);
+
+}  // namespace ground_truth
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_

cartographer/mapping/sparse_pose_graph.cc

@@ -98,8 +98,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
 
   for (const auto& constraint : constraints()) {
     auto* const constraint_proto = proto.add_constraint();
-    *constraint_proto->mutable_relative_pose() =
+    const auto& node = all_trajectory_nodes.at(constraint.node_id.trajectory_id)
-        transform::ToProto(constraint.pose.zbar_ij);
+                           .at(constraint.node_id.node_index);
+    *constraint_proto->mutable_relative_pose() = transform::ToProto(
+        constraint.pose.zbar_ij * node.constant_data->tracking_to_pose);
     constraint_proto->set_translation_weight(
         constraint.pose.translation_weight);
     constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);