Multi-trajectory SparsePoseGraph::ToProto. (#227)

Multi-trajectory SparsePoseGraph::ToProto.
Remove obsolete TrajectoryNode::ToProto.
Add GetSubmapStates.

cartographer/mapping/sparse_pose_graph.cc

@@ -16,8 +16,6 @@
 
 #include "cartographer/mapping/sparse_pose_graph.h"
 
-#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"
@@ -38,19 +36,46 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
   LOG(FATAL) << "Unsupported tag.";
 }
 
-std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
+std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
-    const std::vector<TrajectoryNode>& trajectory_nodes) {
+    const std::vector<const Submaps*>& trajectories) {
-  std::vector<std::vector<TrajectoryNode>> trajectories;
+  std::unordered_map<const Submaps*, int> result;
-  std::unordered_map<const Submaps*, int> trajectory_ids;
+  for (const auto& trajectory : trajectories) {
+    result.emplace(trajectory, result.size());
+  }
+  return result;
+}
+
+void GroupTrajectoryNodes(
+    const std::vector<TrajectoryNode>& trajectory_nodes,
+    const std::unordered_map<const Submaps*, int>& trajectory_ids,
+    std::vector<std::vector<TrajectoryNode>>* grouped_nodes,
+    std::vector<std::pair<int, int>>* new_indices) {
+  CHECK_NOTNULL(grouped_nodes)->clear();
+  CHECK_NOTNULL(new_indices)->clear();
+
+  grouped_nodes->resize(trajectory_ids.size());
+
   for (const auto& node : trajectory_nodes) {
-    const auto* trajectory = node.constant_data->trajectory;
+    const int id = trajectory_ids.at(node.constant_data->trajectory);
-    if (trajectory_ids.emplace(trajectory, trajectories.size()).second) {
+    new_indices->emplace_back(id, (*grouped_nodes)[id].size());
-      trajectories.push_back({node});
+    (*grouped_nodes)[id].push_back(node);
-    } else {
-      trajectories[trajectory_ids[trajectory]].push_back(node);
   }
 }
-  return trajectories;
+
+// TODO(macmason): This function is very nearly a copy of GroupTrajectoryNodes.
+// Consider refactoring them to share an implementation.
+void GroupSubmapStates(
+    const std::vector<SparsePoseGraph::SubmapState>& submap_states,
+    const std::unordered_map<const Submaps*, int>& trajectory_ids,
+    std::vector<std::pair<int, int>>* new_indices) {
+  CHECK_NOTNULL(new_indices)->clear();
+  std::vector<int> submap_group_sizes(trajectory_ids.size(), 0);
+
+  for (const auto& submap_state : submap_states) {
+    const int id = trajectory_ids.at(submap_state.trajectory);
+    new_indices->emplace_back(id, submap_group_sizes[id]);
+    submap_group_sizes[id]++;
+  }
 }
 
 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
@@ -74,32 +99,62 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
 
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
+
+  const auto trajectory_nodes = GetTrajectoryNodes();
+  std::vector<const Submaps*> submap_pointers;
+  for (const auto& trajectory_node : trajectory_nodes) {
+    submap_pointers.push_back(trajectory_node.constant_data->trajectory);
+  }
+
+  const auto trajectory_ids = ComputeTrajectoryIds(submap_pointers);
+
+  std::vector<std::vector<TrajectoryNode>> grouped_nodes;
+  std::vector<std::pair<int, int>> grouped_node_indices;
+  GroupTrajectoryNodes(trajectory_nodes, trajectory_ids, &grouped_nodes,
+                       &grouped_node_indices);
+
+  std::vector<std::pair<int, int>> grouped_submap_indices;
+  GroupSubmapStates(GetSubmapStates(), trajectory_ids, &grouped_submap_indices);
+
   for (const auto& constraint : constraints()) {
     auto* const constraint_proto = proto.add_constraint();
     *constraint_proto->mutable_relative_pose() =
         transform::ToProto(constraint.pose.zbar_ij);
+    constraint_proto->mutable_sqrt_lambda()->Reserve(36);
     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_submap_id()->set_trajectory_id(
-    constraint_proto->mutable_scan_id()->set_scan_index(constraint.j);
+        grouped_submap_indices[constraint.i].first);
+    constraint_proto->mutable_submap_id()->set_submap_index(
+        grouped_submap_indices[constraint.i].second);
+
+    constraint_proto->mutable_scan_id()->set_trajectory_id(
+        grouped_node_indices[constraint.j].first);
+    constraint_proto->mutable_scan_id()->set_scan_index(
+        grouped_node_indices[constraint.j].second);
+
     constraint_proto->set_tag(mapping::ToProto(constraint.tag));
   }
 
-  // TODO(whess): Support multi-trajectory.
+  for (const auto& group : grouped_nodes) {
-  proto::Trajectory* const trajectory = proto.add_trajectory();
+    auto* trajectory_proto = proto.add_trajectory();
-  *trajectory = mapping::ToProto(GetTrajectoryNodes());
+    for (const auto& node : group) {
-  const std::vector<std::vector<const Submaps*>> components =
+      auto* node_proto = trajectory_proto->add_node();
-      GetConnectedTrajectories();
+      node_proto->set_timestamp(common::ToUniversal(node.constant_data->time));
-  CHECK_EQ(components.size(), 1);
+      *node_proto->mutable_pose() =
-  CHECK_EQ(components[0].size(), 1);
+          transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
-  const Submaps* const submaps = components[0][0];
+    }
+
+    const Submaps* const submaps = group[0].constant_data->trajectory;
     for (const auto& transform : GetSubmapTransforms(*submaps)) {
-    *trajectory->add_submap()->mutable_pose() = transform::ToProto(transform);
+      *trajectory_proto->add_submap()->mutable_pose() =
+          transform::ToProto(transform);
+    }
   }
 
   return proto;

cartographer/mapping/sparse_pose_graph.h

@@ -17,6 +17,9 @@
 #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
 #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
 
+#include <set>
+#include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include "cartographer/common/lua_parameter_dictionary.h"
@@ -33,9 +36,22 @@ namespace mapping {
 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary);
 
-// Splits TrajectoryNodes by ID.
+// Construct a mapping from trajectory (ie Submaps*) to an integer. These
-std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
+// values are used to track trajectory identity between scans and submaps.
-    const std::vector<TrajectoryNode>& trajectory_nodes);
+std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
+    const std::vector<const Submaps*>& trajectories);
+
+// TrajectoryNodes are provided in a flat vector, but serialization requires
+// that we group them by trajectory. This groups the elements of
+// 'trajectory_nodes' into 'grouped_nodes' (so that (*grouped_nodes)[i]
+// contains a complete single trajectory). The re-indexing done is stored in
+// 'new_indices', such that 'trajectory_nodes[i]' landed in
+// '(*grouped_nodes)[new_indices[i].first][new_indices[i].second]'.
+void GroupTrajectoryNodes(
+    const std::vector<TrajectoryNode>& trajectory_nodes,
+    const std::unordered_map<const Submaps*, int>& trajectory_ids,
+    std::vector<std::vector<TrajectoryNode>>* grouped_nodes,
+    std::vector<std::pair<int, int>>* new_indices);
 
 class SparsePoseGraph {
  public:
@@ -63,6 +79,24 @@ class SparsePoseGraph {
     enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
   };
 
+  struct SubmapState {
+    const Submap* submap = nullptr;
+
+    // Indices of the scans that were inserted into this map together with
+    // constraints for them. They are not to be matched again when this submap
+    // becomes 'finished'.
+    std::set<int> scan_indices;
+
+    // Whether in the current state of the background thread this submap is
+    // finished. When this transitions to true, all scans are tried to match
+    // against this submap. Likewise, all new scans are matched against submaps
+    // which are finished.
+    bool finished = false;
+
+    // The trajectory to which this SubmapState belongs.
+    const Submaps* trajectory = nullptr;
+  };
+
   SparsePoseGraph() {}
   virtual ~SparsePoseGraph() {}
 
@@ -95,15 +129,24 @@ class SparsePoseGraph {
   // Returns the current optimized trajectory.
   virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
 
+  // TODO(macmason, wohe): Consider replacing this with a GroupSubmapStates,
+  // which would have better separation of concerns.
+  virtual std::vector<SubmapState> GetSubmapStates() = 0;
+
   // Returns the collection of constraints.
   virtual std::vector<Constraint> constraints() = 0;
 
-  // Serializes the constraints and the computed trajectory.
+  // Serializes the constraints and trajectories.
-  //
-  // TODO(whess): Support multiple trajectories.
   proto::SparsePoseGraph ToProto();
 };
 
+// Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be
+// grouped by trajectory. The arguments are just as in 'GroupTrajectoryNodes'.
+void GroupSubmapStates(
+    const std::vector<SparsePoseGraph::SubmapState>& submap_states,
+    const std::unordered_map<const Submaps*, int>& trajectory_ids,
+    std::vector<std::pair<int, int>>* new_indices);
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping/sparse_pose_graph_test.cc

@@ -16,7 +16,9 @@
 
 #include "cartographer/mapping/sparse_pose_graph.h"
 
+#include <algorithm>
 #include <deque>
+#include <vector>
 
 #include "glog/logging.h"
 #include "gmock/gmock.h"
@@ -27,6 +29,7 @@ namespace {
 
 class FakeSubmaps : public Submaps {
  public:
+  ~FakeSubmaps() override {}
   const Submap* Get(int) const override { LOG(FATAL) << "Not implemented."; }
 
   int size() const override { LOG(FATAL) << "Not implemented."; }
@@ -38,7 +41,7 @@ class FakeSubmaps : public Submaps {
   }
 };
 
-TEST(SparsePoseGraphTest, SplitTrajectoryNodes) {
+TEST(SparsePoseGraphTest, TrajectoryFunctions) {
   std::vector<TrajectoryNode> trajectory_nodes;
   const std::vector<FakeSubmaps> submaps(5);
   std::deque<TrajectoryNode::ConstantData> constant_data;
@@ -52,15 +55,42 @@ TEST(SparsePoseGraphTest, SplitTrajectoryNodes) {
       trajectory_nodes.push_back(node);
     }
   }
-  std::vector<std::vector<TrajectoryNode>> split_trajectory_nodes =
+
-      SplitTrajectoryNodes(trajectory_nodes);
+  std::vector<const Submaps*> submap_pointers;
-  EXPECT_EQ(split_trajectory_nodes.size(), submaps.size());
+  for (const auto& submap : submaps) {
+    submap_pointers.push_back(&submap);
+  }
+
+  const auto index_map = ComputeTrajectoryIds(submap_pointers);
+  ASSERT_EQ(submaps.size(), index_map.size());
+  for (const auto& kv : index_map) {
+    const auto pointer_iterator =
+        std::find(submap_pointers.begin(), submap_pointers.end(), kv.first);
+    ASSERT_TRUE(pointer_iterator != submap_pointers.end());
+    const auto pointer_index = pointer_iterator - submap_pointers.begin();
+    EXPECT_EQ(kv.second, pointer_index);
+  }
+
+  std::vector<std::vector<TrajectoryNode>> grouped_nodes;
+  std::vector<std::pair<int, int>> new_indices;
+  GroupTrajectoryNodes(trajectory_nodes, index_map, &grouped_nodes,
+                       &new_indices);
+
+  ASSERT_EQ(grouped_nodes.size(), submaps.size());
   for (size_t i = 0; i < submaps.size(); ++i) {
-    EXPECT_EQ(split_trajectory_nodes[i].size(), kNumTrajectoryNodes);
+    EXPECT_EQ(grouped_nodes[i].size(), kNumTrajectoryNodes);
-    for (const auto& node : split_trajectory_nodes[i]) {
+    for (const auto& node : grouped_nodes[i]) {
       EXPECT_EQ(node.constant_data->trajectory, &submaps[i]);
     }
   }
+
+  ASSERT_EQ(trajectory_nodes.size(), new_indices.size());
+  for (size_t i = 0; i < new_indices.size(); ++i) {
+    const auto index_pair = new_indices[i];
+    EXPECT_EQ(trajectory_nodes[i].constant_data->trajectory,
+              grouped_nodes[index_pair.first][index_pair.second]
+                  .constant_data->trajectory);
+  }
 }
 
 }  // namespace

cartographer/mapping/trajectory_node.cc

@@ -1,37 +0,0 @@
-/*
- * 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/mapping/trajectory_node.h"
-
-#include "cartographer/transform/transform.h"
-
-namespace cartographer {
-namespace mapping {
-
-proto::Trajectory ToProto(const std::vector<TrajectoryNode>& nodes) {
-  proto::Trajectory trajectory;
-  for (const auto& node : nodes) {
-    const auto& data = *node.constant_data;
-    auto* node_proto = trajectory.add_node();
-    node_proto->set_timestamp(common::ToUniversal(data.time));
-    *node_proto->mutable_pose() =
-        transform::ToProto(node.pose * data.tracking_to_pose);
-  }
-  return trajectory;
-}
-
-}  // namespace mapping
-}  // namespace cartographer

cartographer/mapping/trajectory_node.h

@@ -22,7 +22,6 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/proto/trajectory.pb.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/rigid_transform.h"
 
@@ -67,8 +66,6 @@ struct TrajectoryNodes {
   std::vector<mapping::TrajectoryNode> trajectory_nodes;
 };
 
-proto::Trajectory ToProto(const std::vector<TrajectoryNode>& nodes);
-
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -294,25 +294,20 @@ void SparsePoseGraph::WaitForAllComputations() {
   common::MutexLocker locker(&mutex_);
   const int num_finished_scans_at_start =
       constraint_builder_.GetNumFinishedScans();
-  while (!locker.AwaitWithTimeout(
+  while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
-      [this]() REQUIRES(mutex_) {
     return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
            trajectory_nodes_.size();
-      },
+  }, common::FromSeconds(1.))) {
-      common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. *
+                  << 100. * (constraint_builder_.GetNumFinishedScans() -
-                         (constraint_builder_.GetNumFinishedScans() -
                              num_finished_scans_at_start) /
                          (trajectory_nodes_.size() -
-                          num_finished_scans_at_start)
+                          num_finished_scans_at_start) << "%...";
-                  << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
-  constraint_builder_.WhenDone(
+  constraint_builder_.WhenDone([this, &notification](
-      [this, &notification](
       const sparse_pose_graph::ConstraintBuilder::Result& result) {
     constraints_.insert(constraints_.end(), result.begin(), result.end());
     common::MutexLocker locker(&mutex_);
@@ -351,11 +346,11 @@ void SparsePoseGraph::RunOptimization() {
       const mapping::Submaps* trajectory =
           trajectory_nodes_[i].constant_data->trajectory;
       if (extrapolation_transforms.count(trajectory) == 0) {
-        extrapolation_transforms[trajectory] = transform::Rigid3d(
+        extrapolation_transforms[trajectory] =
-            ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
+            transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
-                .back() *
+                                                           *trajectory).back() *
-            ExtrapolateSubmapTransforms(optimized_submap_transforms_,
+                               ExtrapolateSubmapTransforms(
-                                        *trajectory)
+                                   optimized_submap_transforms_, *trajectory)
                                    .back()
                                    .inverse());
       }
@@ -396,6 +391,11 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+std::vector<SparsePoseGraph::SubmapState> SparsePoseGraph::GetSubmapStates() {
+  common::MutexLocker locker(&mutex_);
+  return submap_states_;
+}
+
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   return constraints_;
 }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -96,27 +96,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
+  std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
+
   std::vector<Constraint> constraints() override;
 
  private:
-  struct SubmapState {
-    const mapping::Submap* submap = nullptr;
-
-    // Indices of the scans that were inserted into this map together with
-    // constraints for them. They are not to be matched again when this submap
-    // becomes 'finished'.
-    std::set<int> scan_indices;
-
-    // Whether in the current state of the background thread this submap is
-    // finished. When this transitions to true, all scans are tried to match
-    // against this submap. Likewise, all new scans are matched against submaps
-    // which are finished.
-    bool finished = false;
-
-    // The trajectory to which this SubmapState belongs.
-    const mapping::Submaps* trajectory = nullptr;
-  };
-
   // Handles a new work item.
   void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
 

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -285,25 +285,20 @@ void SparsePoseGraph::WaitForAllComputations() {
   common::MutexLocker locker(&mutex_);
   const int num_finished_scans_at_start =
       constraint_builder_.GetNumFinishedScans();
-  while (!locker.AwaitWithTimeout(
+  while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
-      [this]() REQUIRES(mutex_) {
     return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
            trajectory_nodes_.size();
-      },
+  }, common::FromSeconds(1.))) {
-      common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. *
+                  << 100. * (constraint_builder_.GetNumFinishedScans() -
-                         (constraint_builder_.GetNumFinishedScans() -
                              num_finished_scans_at_start) /
                          (trajectory_nodes_.size() -
-                          num_finished_scans_at_start)
+                          num_finished_scans_at_start) << "%...";
-                  << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
-  constraint_builder_.WhenDone(
+  constraint_builder_.WhenDone([this, &notification](
-      [this, &notification](
       const sparse_pose_graph::ConstraintBuilder::Result& result) {
     constraints_.insert(constraints_.end(), result.begin(), result.end());
     common::MutexLocker locker(&mutex_);
@@ -341,11 +336,11 @@ void SparsePoseGraph::RunOptimization() {
       const mapping::Submaps* trajectory =
           trajectory_nodes_[i].constant_data->trajectory;
       if (extrapolation_transforms.count(trajectory) == 0) {
-        extrapolation_transforms[trajectory] = transform::Rigid3d(
+        extrapolation_transforms[trajectory] =
-            ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
+            transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
-                .back() *
+                                                           *trajectory).back() *
-            ExtrapolateSubmapTransforms(optimized_submap_transforms_,
+                               ExtrapolateSubmapTransforms(
-                                        *trajectory)
+                                   optimized_submap_transforms_, *trajectory)
                                    .back()
                                    .inverse());
       }
@@ -386,6 +381,21 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+std::vector<mapping::SparsePoseGraph::SubmapState>
+SparsePoseGraph::GetSubmapStates() {
+  std::vector<mapping::SparsePoseGraph::SubmapState> result;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& submap_state : submap_states_) {
+    mapping::SparsePoseGraph::SubmapState entry;
+    entry.submap = submap_state.submap;
+    entry.scan_indices = submap_state.scan_indices;
+    entry.finished = submap_state.finished;
+    entry.trajectory = submap_state.trajectory;
+    result.push_back(entry);
+  }
+  return result;
+}
+
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   return constraints_;
 }

cartographer/mapping_3d/sparse_pose_graph.h

@@ -95,9 +95,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
+  std::vector<SubmapState> GetSubmapStates() override;
   std::vector<Constraint> constraints() override;
 
  private:
+  // This is 'mapping::SubmapState', but with the 3D versions of 'submap' and
+  // 'trajectory'.
   struct SubmapState {
     const Submap* submap = nullptr;