Add serialization of the sparse pose graph. (#97)

cartographer/mapping/CMakeLists.txt

@@ -116,13 +116,16 @@ google_library(mapping_sparse_pose_graph
     sparse_pose_graph.h
   DEPENDS
     common_lua_parameter_dictionary
+    kalman_filter_pose_tracker
     mapping_proto_scan_matching_progress
+    mapping_proto_sparse_pose_graph
     mapping_proto_sparse_pose_graph_options
     mapping_sparse_pose_graph_constraint_builder
     mapping_sparse_pose_graph_optimization_problem_options
     mapping_submaps
     mapping_trajectory_node
     transform_rigid_transform
+    transform_transform
 )
 
 google_library(mapping_submaps

cartographer/mapping/proto/CMakeLists.txt

@@ -26,6 +26,14 @@ google_proto_library(mapping_proto_scan_matching_progress
     scan_matching_progress.proto
 )
 
+google_proto_library(mapping_proto_sparse_pose_graph
+  SRCS
+    sparse_pose_graph.proto
+  DEPENDS
+    proto_trajectory
+    transform_proto_transform
+)
+
 google_proto_library(mapping_proto_sparse_pose_graph_options
   SRCS
     sparse_pose_graph_options.proto

cartographer/mapping/proto/sparse_pose_graph.proto

@@ -0,0 +1,54 @@
+// 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.mapping.proto;
+
+import "cartographer/transform/proto/transform.proto";
+import "cartographer/proto/trajectory.proto";
+
+message SparsePoseGraph {
+  message Constraint {
+    message SubmapId {
+      optional int32 trajectory_id = 1;
+      optional int32 submap_index = 2;  // Submap index in the given trajectory.
+    }
+
+    message ScanId {
+      optional int32 trajectory_id = 1;
+      optional int32 scan_index = 2;  // Scan index in the given trajectory.
+    }
+
+    // Differentiates between intra-submap (where the scan was inserted into the
+    // submap) and inter-submap constraints (where the scan was not inserted
+    // into the submap).
+    enum Tag {
+      INTRA_SUBMAP = 0;
+      INTER_SUBMAP = 1;
+    }
+
+    optional SubmapId submap_id = 1;  // Submap ID.
+    optional ScanId scan_id = 2;  // Scan ID.
+    // Pose of the scan relative to submap, i.e. taking data from the scan frame
+    // into the submap frame.
+    optional transform.proto.Rigid3d relative_pose = 3;
+    // 6x6 square root inverse of the covariance matrix.
+    repeated double sqrt_Lambda = 4;  // NOLINT
+    optional Tag tag = 5;
+  }
+
+  repeated Constraint constraint = 2;
+  repeated .cartographer.proto.Trajectory trajectory = 4;
+}

cartographer/mapping/sparse_pose_graph.cc

@@ -18,17 +18,41 @@
 
 #include <unordered_map>
 
+#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/sparse_pose_graph/constraint_builder.h"
 #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h"
+#include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
 namespace mapping {
 
+proto::SparsePoseGraph::Constraint::Tag ToProto(
+    const SparsePoseGraph::Constraint2D::Tag& tag) {
+  switch (tag) {
+    case SparsePoseGraph::Constraint2D::Tag::INTRA_SUBMAP:
+      return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
+    case SparsePoseGraph::Constraint2D::Tag::INTER_SUBMAP:
+      return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
+  }
+  LOG(FATAL) << "Unsupported tag.";
+}
+
+proto::SparsePoseGraph::Constraint::Tag ToProto(
+    const SparsePoseGraph::Constraint3D::Tag& tag) {
+  switch (tag) {
+    case SparsePoseGraph::Constraint3D::Tag::INTRA_SUBMAP:
+      return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP;
+    case SparsePoseGraph::Constraint3D::Tag::INTER_SUBMAP:
+      return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
+  }
+  LOG(FATAL) << "Unsupported tag.";
+}
+
 std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
     const std::vector<TrajectoryNode>& trajectory_nodes) {
   std::vector<std::vector<TrajectoryNode>> trajectories;
-  std::unordered_map<const mapping::Submaps*, int> trajectory_ids;
+  std::unordered_map<const Submaps*, int> trajectory_ids;
   for (const auto& node : trajectory_nodes) {
     const auto* trajectory = node.constant_data->trajectory;
     if (trajectory_ids.emplace(trajectory, trajectories.size()).second) {
@@ -49,7 +73,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
       sparse_pose_graph::CreateConstraintBuilderOptions(
           parameter_dictionary->GetDictionary("constraint_builder").get());
   *options.mutable_optimization_problem_options() =
-      mapping::sparse_pose_graph::CreateOptimizationProblemOptions(
+      sparse_pose_graph::CreateOptimizationProblemOptions(
           parameter_dictionary->GetDictionary("optimization_problem").get());
   options.set_max_num_final_iterations(
       parameter_dictionary->GetNonNegativeInt("max_num_final_iterations"));
@@ -59,5 +83,57 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
   return options;
 }
 
+proto::SparsePoseGraph SparsePoseGraph::ToProto() {
+  proto::SparsePoseGraph proto;
+  for (const auto& constraint : constraints_2d()) {
+    auto* const constraint_proto = proto.add_constraint();
+    *constraint_proto->mutable_relative_pose() =
+        transform::ToProto(transform::Embed3D(constraint.pose.zbar_ij));
+    for (int i = 0; i != 36; ++i) {
+      constraint_proto->mutable_sqrt_lambda()->Add(0.);
+    }
+    constexpr double kFakePositionCovariance = 1.;
+    constexpr double kFakeOrientationCovariance = 1.;
+    Eigen::Map<Eigen::Matrix<double, 6, 6>>(
+        constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
+        kalman_filter::Embed3D(constraint.pose.sqrt_Lambda_ij,
+                               kFakePositionCovariance,
+                               kFakeOrientationCovariance);
+    // TODO(whess): Support multi-trajectory.
+    constraint_proto->mutable_submap_id()->set_submap_index(constraint.i);
+    constraint_proto->mutable_scan_id()->set_scan_index(constraint.j);
+    constraint_proto->set_tag(mapping::ToProto(constraint.tag));
+  }
+  for (const auto& constraint : constraints_3d()) {
+    auto* const constraint_proto = proto.add_constraint();
+    *constraint_proto->mutable_relative_pose() =
+        transform::ToProto(constraint.pose.zbar_ij);
+    for (int i = 0; i != 36; ++i) {
+      constraint_proto->mutable_sqrt_lambda()->Add(0.);
+    }
+    Eigen::Map<Eigen::Matrix<double, 6, 6>>(
+        constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
+        constraint.pose.sqrt_Lambda_ij;
+    // TODO(whess): Support multi-trajectory.
+    constraint_proto->mutable_submap_id()->set_submap_index(constraint.i);
+    constraint_proto->mutable_scan_id()->set_scan_index(constraint.j);
+    constraint_proto->set_tag(mapping::ToProto(constraint.tag));
+  }
+
+  // TODO(whess): Support multi-trajectory.
+  ::cartographer::proto::Trajectory* const trajectory = proto.add_trajectory();
+  *trajectory = mapping::ToProto(GetTrajectoryNodes());
+  const std::vector<std::vector<const Submaps*>> components =
+      GetConnectedTrajectories();
+  CHECK_EQ(components.size(), 1);
+  CHECK_EQ(components[0].size(), 1);
+  const Submaps* const submaps = components[0][0];
+  for (const auto& transform : GetSubmapTransforms(*submaps)) {
+    *trajectory->add_submap()->mutable_pose() = transform::ToProto(transform);
+  }
+
+  return proto;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/sparse_pose_graph.h

@@ -21,6 +21,7 @@
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping/proto/scan_matching_progress.pb.h"
+#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
@@ -110,7 +111,7 @@ class SparsePoseGraph {
   // continuous, non-loop-closed frame) into the global map frame (i.e. the
   // discontinuous, loop-closed frame).
   virtual transform::Rigid3d GetLocalToGlobalTransform(
-      const mapping::Submaps& submaps) = 0;
+      const Submaps& submaps) = 0;
 
   // Returns the current optimized trajectory.
   virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
@@ -120,6 +121,11 @@ class SparsePoseGraph {
 
   // Returns the collection of 3D constraints.
   virtual std::vector<Constraint3D> constraints_3d() = 0;
+
+  // Serializes the constraints and the computed trajectory.
+  //
+  // TODO(whess): Support multiple trajectories.
+  proto::SparsePoseGraph ToProto();
 };
 
 }  // namespace mapping

cartographer/proto/trajectory.proto

@@ -28,6 +28,13 @@ message Trajectory {
     optional transform.proto.Rigid3d pose = 5;
   }
 
+  message Submap {
+    optional transform.proto.Rigid3d pose = 1;
+  }
+
   // Time-ordered sequence of Nodes.
   repeated Node node = 1;
+
+  // Submaps associated with the trajectory.
+  repeated Submap submap = 2;
 }

cartographer/sensor/ordered_multi_queue.h

@@ -167,7 +167,8 @@ class OrderedMultiQueue {
   void CannotMakeProgress() {
     for (auto& entry : queues_) {
       if (entry.second.queue.Size() > kMaxQueueSize) {
-        LOG_EVERY_N(WARNING, 60) << "Queues waiting for data: " << EmptyQueuesDebugString();
+        LOG_EVERY_N(WARNING, 60) << "Queues waiting for data: "
+                                 << EmptyQueuesDebugString();
         return;
       }
     }