Introduces mapping::MapById in the 2D optimization problem. (#577)

PAIR=cschuet

cartographer/mapping/id.h

@@ -18,10 +18,14 @@
 #define CARTOGRAPHER_MAPPING_ID_H_
 
 #include <algorithm>
+#include <iterator>
+#include <map>
 #include <ostream>
 #include <tuple>
 #include <vector>
 
+#include "glog/logging.h"
+
 namespace cartographer {
 namespace mapping {
 
@@ -105,6 +109,139 @@ class NestedVectorsById {
   std::vector<std::vector<ValueType>> data_;
 };
 
+// Like std::map, but indexed by 'IdType' which can be 'NodeId' or 'SubmapId'.
+template <typename IdType, typename DataType>
+class MapById {
+ private:
+  class MapByIndex;
+
+ public:
+  struct IdDataReference {
+    IdType id;
+    const DataType& data;
+  };
+
+  class ConstIterator
+      : public std::iterator<std::forward_iterator_tag, IdDataReference> {
+   public:
+    explicit ConstIterator(const MapById& map_by_id)
+        : current_trajectory_(map_by_id.trajectories_.begin()),
+          end_trajectory_(map_by_id.trajectories_.end()) {
+      if (current_trajectory_ != end_trajectory_) {
+        current_data_ = current_trajectory_->second.data_.begin();
+        end_data_ = current_trajectory_->second.data_.end();
+        AdvanceToValidDataIterator();
+      }
+    }
+
+    static ConstIterator EndIterator(const MapById& map_by_id) {
+      auto it = ConstIterator(map_by_id);
+      it.current_trajectory_ = it.end_trajectory_;
+      return it;
+    }
+
+    IdDataReference operator*() const {
+      CHECK(current_trajectory_ != end_trajectory_);
+      return IdDataReference{
+          IdType{current_trajectory_->first, current_data_->first},
+          current_data_->second};
+    }
+
+    ConstIterator& operator++() {
+      CHECK(current_trajectory_ != end_trajectory_);
+      ++current_data_;
+      AdvanceToValidDataIterator();
+      return *this;
+    }
+
+    bool operator==(const ConstIterator& it) const {
+      if (current_trajectory_ == end_trajectory_ ||
+          it.current_trajectory_ == it.end_trajectory_) {
+        return current_trajectory_ == it.current_trajectory_;
+      }
+      return current_trajectory_ == it.current_trajectory_ &&
+             current_data_ == it.current_data_;
+    }
+
+    bool operator!=(const ConstIterator& it) const { return !operator==(it); }
+
+   private:
+    void AdvanceToValidDataIterator() {
+      CHECK(current_trajectory_ != end_trajectory_);
+      while (current_data_ == end_data_) {
+        ++current_trajectory_;
+        if (current_trajectory_ == end_trajectory_) {
+          return;
+        }
+        current_data_ = current_trajectory_->second.data_.begin();
+        end_data_ = current_trajectory_->second.data_.end();
+      }
+    }
+
+    typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
+    typename std::map<int, MapByIndex>::const_iterator end_trajectory_;
+    typename std::map<int, DataType>::const_iterator current_data_;
+    typename std::map<int, DataType>::const_iterator end_data_;
+  };
+
+  // Appends data to a 'trajectory_id', creating trajectories as needed.
+  IdType Append(const int trajectory_id, const DataType& data) {
+    CHECK_GE(trajectory_id, 0);
+    auto& trajectory = trajectories_[trajectory_id];
+    CHECK(trajectory.can_append_);
+    const int index =
+        trajectory.data_.empty() ? 0 : trajectory.data_.rbegin()->first + 1;
+    trajectory.data_.emplace(index, data);
+    return IdType{trajectory_id, index};
+  }
+
+  // Inserts data (which must not exist already) into a trajectory.
+  void Insert(const IdType& id, const DataType& data) {
+    auto& trajectory = trajectories_[id.trajectory_id];
+    trajectory.can_append_ = false;
+    CHECK(trajectory.data_.emplace(GetIndex(id), data).second);
+  }
+
+  // Removes the data for 'id' which must exist.
+  void Trim(const IdType& id) {
+    auto& trajectory = trajectories_.at(id.trajectory_id);
+    const auto it = trajectory.data_.find(GetIndex(id));
+    CHECK(it != trajectory.data_.end());
+    if (std::next(it) == trajectory.data_.end()) {
+      // We are removing the data with the highest index from this trajectory.
+      // We assume that we will never append to it anymore. If we did, we would
+      // have to make sure that gaps in indices are properly chosen to maintain
+      // correct connectivity.
+      trajectory.can_append_ = false;
+    }
+    trajectory.data_.erase(it);
+  }
+
+  const DataType& at(const IdType& id) const {
+    return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
+  }
+
+  DataType& at(const IdType& id) {
+    return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id));
+  }
+
+  ConstIterator begin() const { return ConstIterator(*this); }
+  ConstIterator end() const { return ConstIterator::EndIterator(*this); }
+
+  bool empty() const { return begin() == end(); }
+
+ private:
+  struct MapByIndex {
+    bool can_append_ = true;
+    std::map<int, DataType> data_;
+  };
+
+  static int GetIndex(const NodeId& id) { return id.node_index; }
+  static int GetIndex(const SubmapId& id) { return id.submap_index; }
+
+  std::map<int, MapByIndex> trajectories_;
+};
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping/id_test.cc

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2017 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/id.h"
+
+#include <deque>
+#include <utility>
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+TEST(IdTest, EmptyMapById) {
+  MapById<NodeId, int> map_by_id;
+  EXPECT_TRUE(map_by_id.empty());
+  const NodeId id = map_by_id.Append(42, 42);
+  EXPECT_FALSE(map_by_id.empty());
+  map_by_id.Trim(id);
+  EXPECT_TRUE(map_by_id.empty());
+}
+
+TEST(IdTest, MapByIdIterator) {
+  MapById<NodeId, int> map_by_id;
+  map_by_id.Append(7, 2);
+  map_by_id.Append(42, 3);
+  map_by_id.Append(0, 0);
+  map_by_id.Append(0, 1);
+  std::deque<std::pair<NodeId, int>> expected_id_data = {
+      {NodeId{0, 0}, 0},
+      {NodeId{0, 1}, 1},
+      {NodeId{7, 0}, 2},
+      {NodeId{42, 0}, 3},
+  };
+  for (const auto& id_data : map_by_id) {
+    EXPECT_EQ(expected_id_data.front().first, id_data.id);
+    EXPECT_EQ(expected_id_data.front().second, id_data.data);
+    ASSERT_FALSE(expected_id_data.empty());
+    expected_id_data.pop_front();
+  }
+  EXPECT_TRUE(expected_id_data.empty());
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -55,7 +55,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   CHECK(!insertion_submaps.empty());
-  const auto& submap_data = optimization_problem_.submap_data();
+  auto submap_data = optimization_problem_.submap_data();
   if (insertion_submaps.size() == 1) {
     // If we don't already have an entry for the first submap, add one.
     if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
@@ -63,6 +63,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
       optimization_problem_.AddSubmap(
           trajectory_id,
           sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
+      submap_data = optimization_problem_.submap_data();
     }
     CHECK_EQ(submap_data[trajectory_id].size(), 1);
     const mapping::SubmapId submap_id{trajectory_id, 0};
@@ -499,7 +500,7 @@ void SparsePoseGraph::RunOptimization() {
   optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 
-  const auto& submap_data = optimization_problem_.submap_data();
+  const auto submap_data = optimization_problem_.submap_data();
   const auto& node_data = optimization_problem_.node_data();
   for (int trajectory_id = 0;
        trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -109,20 +109,11 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
 
 void OptimizationProblem::AddSubmap(const int trajectory_id,
                                     const transform::Rigid2d& submap_pose) {
-  CHECK_GE(trajectory_id, 0);
+  submap_data_.Append(trajectory_id, SubmapData{submap_pose});
-  submap_data_.resize(
-      std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  trajectory_data_.resize(
-      std::max(trajectory_data_.size(), submap_data_.size()));
-  auto& trajectory_data = trajectory_data_[trajectory_id];
-  submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index,
-                                      SubmapData{submap_pose});
-  ++trajectory_data.next_submap_index;
 }
 
 void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
-  auto& submap_data = submap_data_.at(submap_id.trajectory_id);
+  submap_data_.Trim(submap_id);
-  CHECK(submap_data.erase(submap_id.submap_index));
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@@ -142,33 +133,24 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
 
   // Set the starting point.
   // TODO(hrapp): Move ceres data into SubmapData.
-  std::vector<std::map<int, std::array<double, 3>>> C_submaps(
+  mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
-      submap_data_.size());
   std::vector<std::map<int, std::array<double, 3>>> C_nodes(node_data_.size());
   bool first_submap = true;
-  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
+  for (const auto& submap_id_data : submap_data_) {
-       ++trajectory_id) {
+    const bool frozen =
-    const bool frozen = frozen_trajectories.count(trajectory_id);
+        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
-    for (const auto& index_submap_data : submap_data_[trajectory_id]) {
+    C_submaps.Insert(submap_id_data.id, FromPose(submap_id_data.data.pose));
-      const int submap_index = index_submap_data.first;
+    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
-      const SubmapData& submap_data = index_submap_data.second;
+    if (first_submap || frozen) {
-
+      first_submap = false;
-      C_submaps[trajectory_id].emplace(submap_index,
+      // Fix the pose of the first submap or all submaps of a frozen
-                                       FromPose(submap_data.pose));
+      // trajectory.
-      problem.AddParameterBlock(
+      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
-          C_submaps[trajectory_id].at(submap_index).data(), 3);
-      if (first_submap || frozen) {
-        first_submap = false;
-        // Fix the pose of the first submap or all submaps of a frozen
-        // trajectory.
-        problem.SetParameterBlockConstant(
-            C_submaps[trajectory_id].at(submap_index).data());
-      }
     }
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    const bool frozen = frozen_trajectories.count(trajectory_id);
+    const bool frozen = frozen_trajectories.count(trajectory_id) != 0;
     for (const auto& index_node_data : node_data_[trajectory_id]) {
       const int node_index = index_node_data.first;
       const NodeData& node_data = index_node_data.second;
@@ -190,9 +172,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
             : nullptr,
-        C_submaps.at(constraint.submap_id.trajectory_id)
+        C_submaps.at(constraint.submap_id).data(),
-            .at(constraint.submap_id.submap_index)
-            .data(),
         C_nodes.at(constraint.node_id.trajectory_id)
             .at(constraint.node_id.node_index)
             .data());
@@ -258,12 +238,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   }
 
   // Store the result.
-  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
+  for (const auto& C_submap_id_data : C_submaps) {
-       ++trajectory_id) {
+    submap_data_.at(C_submap_id_data.id).pose = ToPose(C_submap_id_data.data);
-    for (auto& index_submap_data : submap_data_[trajectory_id]) {
-      index_submap_data.second.pose =
-          ToPose(C_submaps[trajectory_id].at(index_submap_data.first));
-    }
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
@@ -279,9 +255,15 @@ const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
   return node_data_;
 }
 
-const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data()
+std::vector<std::map<int, SubmapData>> OptimizationProblem::submap_data()
     const {
-  return submap_data_;
+  std::vector<std::map<int, SubmapData>> result;
+  for (const auto& submap_id_data : submap_data_) {
+    result.resize(submap_id_data.id.trajectory_id + 1);
+    result[submap_id_data.id.trajectory_id].emplace(
+        submap_id_data.id.submap_index, submap_id_data.data);
+  }
+  return result;
 }
 
 }  // namespace sparse_pose_graph

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -27,6 +27,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/sensor/imu_data.h"
@@ -79,19 +80,18 @@ class OptimizationProblem {
              const std::set<int>& frozen_trajectories);
 
   const std::vector<std::map<int, NodeData>>& node_data() const;
-  const std::vector<std::map<int, SubmapData>>& submap_data() const;
+  std::vector<std::map<int, SubmapData>> submap_data() const;
 
  private:
   struct TrajectoryData {
     // TODO(hrapp): Remove, once we can relabel constraints.
-    int next_submap_index = 0;
     int next_node_index = 0;
   };
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::vector<std::deque<sensor::ImuData>> imu_data_;
   std::vector<std::map<int, NodeData>> node_data_;
   std::vector<transform::TransformInterpolationBuffer> odometry_data_;
-  std::vector<std::map<int, SubmapData>> submap_data_;
+  mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;
 };
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -157,7 +157,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   bool first_submap = true;
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
-    const bool frozen = frozen_trajectories.count(trajectory_id);
+    const bool frozen = frozen_trajectories.count(trajectory_id) != 0;
     for (const auto& index_submap_data : submap_data_[trajectory_id]) {
       const int submap_index = index_submap_data.first;
       if (first_submap) {
@@ -191,7 +191,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    const bool frozen = frozen_trajectories.count(trajectory_id);
+    const bool frozen = frozen_trajectories.count(trajectory_id) != 0;
     for (const auto& index_node_data : node_data_[trajectory_id]) {
       const int node_index = index_node_data.first;
       C_nodes[trajectory_id].emplace(