Introduce sensor::MapByTime<>. (#631)

This data structure is used for keeping IMU data. This
allows trimming IMU data in the middle which is needed for
life-long mapping.

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -64,10 +64,7 @@ OptimizationProblem::~OptimizationProblem() {}
 
 void OptimizationProblem::AddImuData(const int trajectory_id,
                                      const sensor::ImuData& imu_data) {
-  CHECK_GE(trajectory_id, 0);
+  imu_data_.Append(trajectory_id, imu_data);
-  imu_data_.resize(
-      std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  imu_data_[trajectory_id].push_back(imu_data);
 }
 
 void OptimizationProblem::AddOdometerData(
@@ -95,18 +92,8 @@ void OptimizationProblem::InsertTrajectoryNode(
 }
 
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
+  imu_data_.Trim(node_data_, node_id);
   node_data_.Trim(node_id);
-
-  const int trajectory_id = node_id.trajectory_id;
-  if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 &&
-      trajectory_id < static_cast<int>(imu_data_.size())) {
-    const common::Time node_time =
-        node_data_.BeginOfTrajectory(trajectory_id)->data.time;
-    auto& imu_data = imu_data_.at(trajectory_id);
-    while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
-      imu_data.pop_front();
-    }
-  }
 }
 
 void OptimizationProblem::AddSubmap(const int trajectory_id,

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -31,6 +31,7 @@
 #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"
+#include "cartographer/sensor/map_by_time.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
 
@@ -90,7 +91,7 @@ class OptimizationProblem {
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
-  std::vector<std::deque<sensor::ImuData>> imu_data_;
+  sensor::MapByTime<sensor::ImuData> imu_data_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
   std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -56,10 +56,7 @@ OptimizationProblem::~OptimizationProblem() {}
 
 void OptimizationProblem::AddImuData(const int trajectory_id,
                                      const sensor::ImuData& imu_data) {
-  CHECK_GE(trajectory_id, 0);
+  imu_data_.Append(trajectory_id, imu_data);
-  imu_data_.resize(
-      std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  imu_data_[trajectory_id].push_back(imu_data);
 }
 
 void OptimizationProblem::AddOdometerData(
@@ -101,18 +98,8 @@ void OptimizationProblem::InsertTrajectoryNode(
 }
 
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
+  imu_data_.Trim(node_data_, node_id);
   node_data_.Trim(node_id);
-
-  const int trajectory_id = node_id.trajectory_id;
-  if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 &&
-      trajectory_id < static_cast<int>(imu_data_.size())) {
-    const common::Time node_time =
-        node_data_.BeginOfTrajectory(trajectory_id)->data.time;
-    auto& imu_data = imu_data_.at(trajectory_id);
-    while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
-      imu_data.pop_front();
-    }
-  }
 }
 
 void OptimizationProblem::AddSubmap(const int trajectory_id,
@@ -233,8 +220,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
 
       problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                                 new ceres::QuaternionParameterization());
-      const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
+      CHECK(imu_data_.HasTrajectory(trajectory_id));
-      CHECK(!imu_data.empty());
+      const auto imu_data = imu_data_.trajectory(trajectory_id);
+      CHECK(imu_data.begin() != imu_data.end());
 
       auto imu_it = imu_data.begin();
       auto prev_node_it = node_it;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -32,6 +32,7 @@
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/map_by_time.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
 
@@ -100,7 +101,7 @@ class OptimizationProblem {
 
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
-  std::vector<std::deque<sensor::ImuData>> imu_data_;
+  sensor::MapByTime<sensor::ImuData> imu_data_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
   std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;

cartographer/sensor/map_by_time.h

@@ -0,0 +1,156 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
+#define CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
+
+#include <algorithm>
+#include <iterator>
+#include <map>
+#include <memory>
+#include <vector>
+
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/id.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace sensor {
+
+// 'DataType' must contain a 'time' member of type common::Time.
+template <typename DataType>
+class MapByTime {
+ public:
+  // Appends data to a 'trajectory_id', creating trajectories as needed.
+  void Append(const int trajectory_id, const DataType& data) {
+    CHECK_GE(trajectory_id, 0);
+    auto& trajectory = data_[trajectory_id];
+    if (!trajectory.empty()) {
+      CHECK_GT(data.time, std::prev(trajectory.end())->first);
+    }
+    trajectory.emplace(data.time, data);
+  }
+
+  // Removes data no longer needed once 'node_id' gets removed from 'nodes'.
+  // 'NodeType' must contain a 'time' member of type common::Time.
+  template <typename NodeType>
+  void Trim(const mapping::MapById<mapping::NodeId, NodeType>& nodes,
+            const mapping::NodeId& node_id) {
+    const int trajectory_id = node_id.trajectory_id;
+    CHECK_GE(trajectory_id, 0);
+
+    // Data only important between 'gap_start' and 'gap_end' is no longer
+    // needed. We retain the first and last data of the gap so that
+    // interpolation with the adjacent data outside the gap is still possible.
+    const auto node_it = nodes.find(node_id);
+    CHECK(node_it != nodes.end());
+    const common::Time gap_start =
+        node_it != nodes.BeginOfTrajectory(trajectory_id)
+            ? std::prev(node_it)->data.time
+            : common::Time::min();
+    const auto next_it = std::next(node_it);
+    const common::Time gap_end = next_it != nodes.EndOfTrajectory(trajectory_id)
+                                     ? next_it->data.time
+                                     : common::Time::max();
+    CHECK_LT(gap_start, gap_end);
+
+    auto& trajectory = data_[trajectory_id];
+    auto data_it = trajectory.lower_bound(gap_start);
+    auto data_end = trajectory.upper_bound(gap_end);
+    if (data_it == data_end) {
+      return;
+    }
+    if (gap_end != common::Time::max()) {
+      // Retain the last data inside the gap.
+      data_end = std::prev(data_end);
+      if (data_it == data_end) {
+        return;
+      }
+    }
+    if (gap_start != common::Time::min()) {
+      // Retain the first data inside the gap.
+      data_it = std::next(data_it);
+    }
+    while (data_it != data_end) {
+      data_it = trajectory.erase(data_it);
+    }
+    if (trajectory.empty()) {
+      data_.erase(trajectory_id);
+    }
+  }
+
+  bool HasTrajectory(const int trajectory_id) const {
+    return data_.count(trajectory_id) != 0;
+  }
+
+  class ConstIterator {
+   public:
+    using iterator_category = std::bidirectional_iterator_tag;
+    using value_type = DataType;
+    using difference_type = int64;
+    using pointer = const DataType*;
+    using reference = const DataType&;
+
+    explicit ConstIterator(
+        typename std::map<common::Time, DataType>::const_iterator iterator)
+        : iterator_(iterator) {}
+
+    const DataType& operator*() const { return iterator_->second; }
+
+    const DataType* operator->() const { return &iterator_->second; }
+
+    ConstIterator& operator++() {
+      ++iterator_;
+      return *this;
+    }
+
+    ConstIterator& operator--() {
+      --iterator_;
+      return *this;
+    }
+
+    bool operator==(const ConstIterator& it) const {
+      return iterator_ == it.iterator_;
+    }
+
+    bool operator!=(const ConstIterator& it) const { return !operator==(it); }
+
+   private:
+    typename std::map<common::Time, DataType>::const_iterator iterator_;
+  };
+
+  ConstIterator BeginOfTrajectory(const int trajectory_id) const {
+    return ConstIterator(data_.at(trajectory_id).begin());
+  }
+
+  ConstIterator EndOfTrajectory(const int trajectory_id) const {
+    return ConstIterator(data_.at(trajectory_id).end());
+  }
+
+  mapping::Range<ConstIterator> trajectory(const int trajectory_id) const {
+    return mapping::Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
+                                         EndOfTrajectory(trajectory_id));
+  }
+
+ private:
+  std::map<int, std::map<common::Time, DataType>> data_;
+};
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_

cartographer/sensor/map_by_time_test.cc

@@ -0,0 +1,93 @@
+/*
+ * 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/sensor/map_by_time.h"
+
+#include <deque>
+
+#include "cartographer/common/time.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace sensor {
+namespace {
+
+common::Time CreateTime(const int milliseconds) {
+  return common::Time(common::FromMilliseconds(milliseconds));
+}
+
+struct Data {
+  common::Time time;
+};
+
+struct NodeData {
+  common::Time time;
+};
+
+TEST(MapByTimeTest, AppendAndViewTrajectory) {
+  MapByTime<Data> map_by_time;
+  map_by_time.Append(0, Data{CreateTime(10)});
+  map_by_time.Append(42, Data{CreateTime(42)});
+  map_by_time.Append(42, Data{CreateTime(43)});
+  std::deque<Data> expected_data = {Data{CreateTime(42)}, Data{CreateTime(43)}};
+  for (const Data& data : map_by_time.trajectory(42)) {
+    ASSERT_FALSE(expected_data.empty());
+    EXPECT_EQ(expected_data.front().time, data.time);
+    expected_data.pop_front();
+  }
+  EXPECT_TRUE(expected_data.empty());
+}
+
+TEST(MapByTimeTest, Trimming) {
+  MapByTime<Data> map_by_time;
+  EXPECT_FALSE(map_by_time.HasTrajectory(42));
+  map_by_time.Append(42, Data{CreateTime(1)});
+  map_by_time.Append(42, Data{CreateTime(41)});
+  map_by_time.Append(42, Data{CreateTime(42)});
+  map_by_time.Append(42, Data{CreateTime(43)});
+  map_by_time.Append(42, Data{CreateTime(47)});
+  map_by_time.Append(42, Data{CreateTime(48)});
+  map_by_time.Append(42, Data{CreateTime(49)});
+  map_by_time.Append(42, Data{CreateTime(5000)});
+  EXPECT_TRUE(map_by_time.HasTrajectory(42));
+  // Trim one node.
+  mapping::MapById<mapping::NodeId, NodeData> map_by_id;
+  map_by_id.Append(42, NodeData{CreateTime(42)});
+  map_by_id.Append(42, NodeData{CreateTime(46)});
+  map_by_id.Append(42, NodeData{CreateTime(48)});
+  map_by_time.Trim(map_by_id, mapping::NodeId{42, 1});
+  map_by_id.Trim(mapping::NodeId{42, 1});
+  ASSERT_TRUE(map_by_time.HasTrajectory(42));
+  std::deque<Data> expected_data = {
+      Data{CreateTime(1)},  Data{CreateTime(41)}, Data{CreateTime(42)},
+      Data{CreateTime(48)}, Data{CreateTime(49)}, Data{CreateTime(5000)}};
+  for (const Data& data : map_by_time.trajectory(42)) {
+    ASSERT_FALSE(expected_data.empty());
+    EXPECT_EQ(expected_data.front().time, data.time);
+    expected_data.pop_front();
+  }
+  EXPECT_TRUE(expected_data.empty());
+  // Trim everything.
+  map_by_time.Trim(map_by_id, mapping::NodeId{42, 2});
+  map_by_id.Trim(mapping::NodeId{42, 2});
+  map_by_time.Trim(map_by_id, mapping::NodeId{42, 0});
+  map_by_id.Trim(mapping::NodeId{42, 0});
+  EXPECT_FALSE(map_by_time.HasTrajectory(42));
+}
+
+}  // namespace
+}  // namespace sensor
+}  // namespace cartographer