Remove flat indices from SparsePoseGraph::GetTrajectoryNodes(). (#288)

Related to #256.

Also removes ComputeMapLimits with is moved to Cartographer ROS.

PAIR=SirVer

cartographer/mapping/sparse_pose_graph.cc

@@ -36,23 +36,6 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
   LOG(FATAL) << "Unsupported tag.";
 }
 
-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 int id = trajectory_ids.at(node.constant_data->trajectory);
-    new_indices->emplace_back(id, (*grouped_nodes)[id].size());
-    (*grouped_nodes)[id].push_back(node);
-  }
-}
-
 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::SparsePoseGraphOptions options;
@@ -75,11 +58,6 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
 
-  std::vector<std::vector<TrajectoryNode>> grouped_nodes;
-  std::vector<std::pair<int, int>> grouped_node_indices;
-  GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes,
-                       &grouped_node_indices);
-
   for (const auto& constraint : constraints()) {
     auto* const constraint_proto = proto.add_constraint();
     *constraint_proto->mutable_relative_pose() =
@@ -105,21 +83,24 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     constraint_proto->set_tag(mapping::ToProto(constraint.tag));
   }
 
-  for (const auto& group : grouped_nodes) {
+  for (const auto& trajectory_nodes : GetTrajectoryNodes()) {
     auto* trajectory_proto = proto.add_trajectory();
-    for (const auto& node : group) {
+    for (const auto& node : trajectory_nodes) {
       auto* node_proto = trajectory_proto->add_node();
       node_proto->set_timestamp(common::ToUniversal(node.constant_data->time));
       *node_proto->mutable_pose() =
           transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
     }
 
-    const Submaps* const trajectory = group[0].constant_data->trajectory;
+    if (!trajectory_nodes.empty()) {
+      const Submaps* const trajectory =
+          trajectory_nodes[0].constant_data->trajectory;
       for (const auto& transform : GetSubmapTransforms(trajectory)) {
         *trajectory_proto->add_submap()->mutable_pose() =
             transform::ToProto(transform);
       }
     }
+  }
 
   return proto;
 }

cartographer/mapping/sparse_pose_graph.h

@@ -36,18 +36,6 @@ namespace mapping {
 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary);
 
-// 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:
   // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
@@ -93,18 +81,14 @@ class SparsePoseGraph {
   virtual transform::Rigid3d GetLocalToGlobalTransform(
       const Submaps* submaps) = 0;
 
-  // Returns the current optimized trajectory.
+  // Returns the current optimized trajectories.
-  virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
+  virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;
 
   // Serializes the constraints and trajectories.
   proto::SparsePoseGraph ToProto();
 
   // Returns the collection of constraints.
   virtual std::vector<Constraint> constraints() = 0;
-
- protected:
-  // Returns the mapping from Submaps* to trajectory IDs.
-  virtual const std::unordered_map<const Submaps*, int>& trajectory_ids() = 0;
 };
 
 }  // namespace mapping

cartographer/mapping/sparse_pose_graph_test.cc

@@ -1,87 +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/sparse_pose_graph.h"
-
-#include <algorithm>
-#include <deque>
-#include <vector>
-
-#include "glog/logging.h"
-#include "gmock/gmock.h"
-
-namespace cartographer {
-namespace mapping {
-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."; }
-
-  void SubmapToProto(int, const transform::Rigid3d&,
-                     proto::SubmapQuery::Response*) const override {
-    LOG(FATAL) << "Not implemented.";
-  }
-};
-
-TEST(SparsePoseGraphTest, TrajectoryFunctions) {
-  std::vector<TrajectoryNode> trajectory_nodes;
-  const std::vector<FakeSubmaps> submaps(5);
-  std::deque<TrajectoryNode::ConstantData> constant_data;
-  constexpr int kNumTrajectoryNodes = 10;
-  for (int j = 0; j < kNumTrajectoryNodes; ++j) {
-    for (size_t i = 0; i < submaps.size(); ++i) {
-      constant_data.push_back({});
-      constant_data.back().trajectory = &submaps[i];
-      TrajectoryNode node;
-      node.constant_data = &constant_data.back();
-      trajectory_nodes.push_back(node);
-    }
-  }
-
-  const std::unordered_map<const Submaps*, int> index_map = {{&submaps[0], 0},
-                                                             {&submaps[1], 1},
-                                                             {&submaps[2], 2},
-                                                             {&submaps[3], 3},
-                                                             {&submaps[4], 4}};
-  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(grouped_nodes[i].size(), kNumTrajectoryNodes);
-    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
-}  // namespace mapping
-}  // namespace cartographer

cartographer/mapping_2d/map_limits.h

@@ -84,51 +84,6 @@ class MapLimits {
                .all();
   }
 
-  // Computes MapLimits that contain the origin, and all rays (both returns and
-  // misses) in the 'trajectory'.
-  static MapLimits ComputeMapLimits(
-      const double resolution,
-      const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
-    Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes);
-    // Add some padding to ensure all rays are still contained in the map after
-    // discretization.
-    const float kPadding = 3.f * resolution;
-    bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
-    bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
-    const Eigen::Vector2d pixel_sizes =
-        bounding_box.sizes().cast<double>() / resolution;
-    return MapLimits(resolution, bounding_box.max().cast<double>(),
-                     CellLimits(common::RoundToInt(pixel_sizes.y()),
-                                common::RoundToInt(pixel_sizes.x())));
-  }
-
-  static Eigen::AlignedBox2f ComputeMapBoundingBox(
-      const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
-    Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
-    for (const auto& node : trajectory_nodes) {
-      const auto& data = *node.constant_data;
-      if (!data.range_data_3d.returns.empty()) {
-        const sensor::RangeData range_data = sensor::TransformRangeData(
-            Decompress(data.range_data_3d), node.pose.cast<float>());
-        bounding_box.extend(range_data.origin.head<2>());
-        for (const Eigen::Vector3f& hit : range_data.returns) {
-          bounding_box.extend(hit.head<2>());
-        }
-      } else {
-        const sensor::RangeData range_data = sensor::TransformRangeData(
-            data.range_data_2d, node.pose.cast<float>());
-        bounding_box.extend(range_data.origin.head<2>());
-        for (const Eigen::Vector3f& hit : range_data.returns) {
-          bounding_box.extend(hit.head<2>());
-        }
-        for (const Eigen::Vector3f& miss : range_data.misses) {
-          bounding_box.extend(miss.head<2>());
-        }
-      }
-    }
-    return bounding_box;
-  }
-
  private:
   double resolution_;
   Eigen::Vector2d max_;

cartographer/mapping_2d/map_limits_test.cc

@@ -62,29 +62,6 @@ TEST(MapLimitsTest, ConstructAndGet) {
   EXPECT_EQ(42., limits.resolution());
 }
 
-TEST(MapLimitsTest, ComputeMapLimits) {
-  const mapping::TrajectoryNode::ConstantData constant_data{
-      common::FromUniversal(52),
-      sensor::RangeData{
-          Eigen::Vector3f::Zero(),
-          {Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)},
-          {}},
-      Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), nullptr,
-      transform::Rigid3d::Identity()};
-  const mapping::TrajectoryNode trajectory_node{&constant_data,
-                                                transform::Rigid3d::Identity()};
-  constexpr double kResolution = 0.05;
-  const MapLimits limits =
-      MapLimits::ComputeMapLimits(kResolution, {trajectory_node});
-  constexpr float kPaddingAwareTolerance = 5 * kResolution;
-  EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance);
-  EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);
-  EXPECT_LT(200, limits.cell_limits().num_x_cells);
-  EXPECT_LT(1600, limits.cell_limits().num_y_cells);
-  EXPECT_GT(400, limits.cell_limits().num_x_cells);
-  EXPECT_GT(2000, limits.cell_limits().num_y_cells);
-}
-
 }  // namespace
 }  // namespace mapping_2d
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -424,9 +424,16 @@ void SparsePoseGraph::RunOptimization() {
   }
 }
 
-std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
+std::vector<std::vector<mapping::TrajectoryNode>>
+SparsePoseGraph::GetTrajectoryNodes() {
   common::MutexLocker locker(&mutex_);
-  return trajectory_nodes_;
+  std::vector<std::vector<mapping::TrajectoryNode>> result(
+      trajectory_ids_.size());
+  for (const auto& node : trajectory_nodes_) {
+    result.at(trajectory_ids_.at(node.constant_data->trajectory))
+        .push_back(node);
+  }
+  return result;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
@@ -434,12 +441,6 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   return constraints_;
 }
 
-const std::unordered_map<const mapping::Submaps*, int>&
-SparsePoseGraph::trajectory_ids() {
-  common::MutexLocker locker(&mutex_);
-  return trajectory_ids_;
-}
-
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
     const mapping::Submaps* const submaps) {
   const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -86,13 +86,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
       EXCLUDES(mutex_) override;
-  std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
+  std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
-      EXCLUDES(mutex_);
-  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
-
- protected:
-  const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
       override EXCLUDES(mutex_);
+  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private:
   struct SubmapState {

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -189,12 +189,13 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
   MoveRelative(transform::Rigid2d::Identity());
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
-  EXPECT_THAT(nodes.size(), ::testing::Eq(3));
+  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
-  EXPECT_THAT(nodes[0].pose,
+  EXPECT_THAT(nodes[0].size(), ::testing::Eq(3));
+  EXPECT_THAT(nodes[0][0].pose,
               transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
-  EXPECT_THAT(nodes[1].pose,
+  EXPECT_THAT(nodes[0][1].pose,
               transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
-  EXPECT_THAT(nodes[2].pose,
+  EXPECT_THAT(nodes[0][2].pose,
               transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
 }
 
@@ -208,8 +209,10 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
   }
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
+  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
   for (int i = 0; i != 4; ++i) {
-    EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2))
+    EXPECT_THAT(poses[i],
+                IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2))
         << i;
   }
 }
@@ -224,8 +227,10 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
   }
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
+  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
   for (int i = 0; i != 5; ++i) {
-    EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2))
+    EXPECT_THAT(poses[i],
+                IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2))
         << i;
   }
 }
@@ -247,12 +252,13 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) {
   }
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
+  ASSERT_THAT(nodes.size(), ::testing::Eq(1));
   transform::Rigid2d true_movement =
       ground_truth.front().inverse() * ground_truth.back();
   transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
   transform::Rigid2d error_before = movement_before.inverse() * true_movement;
   transform::Rigid3d optimized_movement =
-      nodes.front().pose.inverse() * nodes.back().pose;
+      nodes[0].front().pose.inverse() * nodes[0].back().pose;
   transform::Rigid2d optimized_error =
       transform::Project2D(optimized_movement).inverse() * true_movement;
   EXPECT_THAT(std::abs(optimized_error.normalized_angle()),

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -415,9 +415,16 @@ void SparsePoseGraph::RunOptimization() {
   }
 }
 
-std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
+std::vector<std::vector<mapping::TrajectoryNode>>
+SparsePoseGraph::GetTrajectoryNodes() {
   common::MutexLocker locker(&mutex_);
-  return trajectory_nodes_;
+  std::vector<std::vector<mapping::TrajectoryNode>> result(
+      trajectory_ids_.size());
+  for (const auto& node : trajectory_nodes_) {
+    result.at(trajectory_ids_.at(node.constant_data->trajectory))
+        .push_back(node);
+  }
+  return result;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
@@ -425,12 +432,6 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   return constraints_;
 }
 
-const std::unordered_map<const mapping::Submaps*, int>&
-SparsePoseGraph::trajectory_ids() {
-  common::MutexLocker locker(&mutex_);
-  return trajectory_ids_;
-}
-
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
     const mapping::Submaps* const submaps) {
   const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -73,8 +73,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const std::vector<const Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
-  // The index into the vector of trajectory nodes as used with
+  // The index into the flat vector of trajectory nodes used internally for the
-  // GetTrajectoryNodes() for the next node added with AddScan() is returned.
+  // next node added with AddScan() is returned.
   int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
@@ -88,13 +88,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
       EXCLUDES(mutex_) override;
-  std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
+  std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
-      EXCLUDES(mutex_);
-  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
-
- protected:
-  const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
       override EXCLUDES(mutex_);
+  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private:
   struct SubmapState {