Change fixed_frame_pose_data_ to MapByTime. (#662)

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -47,6 +47,32 @@ namespace cartographer {
 namespace mapping_3d {
 namespace sparse_pose_graph {
 
+namespace {
+
+// For odometry and fixed frame pose.
+template <typename MapByTimeType>
+std::unique_ptr<transform::Rigid3d> Interpolate(
+    const MapByTimeType& map_by_time, const int trajectory_id,
+    const common::Time time) {
+  const auto it = map_by_time.lower_bound(trajectory_id, time);
+  if (it == map_by_time.EndOfTrajectory(trajectory_id)) {
+    return nullptr;
+  }
+  if (it == map_by_time.BeginOfTrajectory(trajectory_id)) {
+    if (it->time == time) {
+      return common::make_unique<transform::Rigid3d>(it->pose);
+    }
+    return nullptr;
+  }
+  const auto prev_it = std::prev(it);
+  return common::make_unique<transform::Rigid3d>(
+      Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
+                  transform::TimestampedTransform{it->time, it->pose}, time)
+          .transform);
+}
+
+}  // namespace
+
 OptimizationProblem::OptimizationProblem(
     const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
         options,
@@ -68,11 +94,7 @@ void OptimizationProblem::AddOdometerData(
 void OptimizationProblem::AddFixedFramePoseData(
     const int trajectory_id,
     const sensor::FixedFramePoseData& fixed_frame_pose_data) {
-  CHECK_GE(trajectory_id, 0);
-  fixed_frame_pose_data_.resize(std::max(
-      fixed_frame_pose_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  fixed_frame_pose_data_[trajectory_id].Push(fixed_frame_pose_data.time,
-                                             fixed_frame_pose_data.pose);
+  fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
 }
 
 void OptimizationProblem::AddTrajectoryNode(
@@ -98,6 +120,7 @@ void OptimizationProblem::InsertTrajectoryNode(
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   imu_data_.Trim(node_data_, node_id);
   odometry_data_.Trim(node_data_, node_id);
+  fixed_frame_pose_data_.Trim(node_data_, node_id);
   node_data_.Trim(node_id);
 }
 
@@ -346,23 +369,25 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   std::deque<CeresPose> C_fixed_frames;
   for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
     const int trajectory_id = node_it->id.trajectory_id;
-    if (trajectory_id >= static_cast<int>(fixed_frame_pose_data_.size())) {
-      break;
-    }
-    bool fixed_frame_pose_initialized = false;
-
     const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
+    if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
+      node_it = trajectory_end;
+      continue;
+    }
+
+    bool fixed_frame_pose_initialized = false;
     for (; node_it != trajectory_end; ++node_it) {
       const mapping::NodeId node_id = node_it->id;
       const NodeData& node_data = node_it->data;
 
-      if (!fixed_frame_pose_data_.at(trajectory_id).Has(node_data.time)) {
+      const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
+          Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
+      if (fixed_frame_pose == nullptr) {
         continue;
       }
 
       const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{
-          fixed_frame_pose_data_.at(trajectory_id).Lookup(node_data.time),
-          options_.fixed_frame_pose_translation_weight(),
+          *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
           options_.fixed_frame_pose_rotation_weight()};
 
       if (!fixed_frame_pose_initialized) {
@@ -440,33 +465,14 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
   return imu_data_;
 }
 
-std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
-    const int trajectory_id, const common::Time time) const {
-  const auto it = odometry_data_.lower_bound(trajectory_id, time);
-  if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
-    return nullptr;
-  }
-  if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) {
-    if (it->time == time) {
-      return common::make_unique<transform::Rigid3d>(it->pose);
-    }
-    return nullptr;
-  }
-  const auto prev_it = std::prev(it);
-  return common::make_unique<transform::Rigid3d>(
-      Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
-                  transform::TimestampedTransform{it->time, it->pose}, time)
-          .transform);
-}
-
 transform::Rigid3d OptimizationProblem::ComputeRelativePose(
     const int trajectory_id, const NodeData& first_node_data,
     const NodeData& second_node_data) const {
   if (odometry_data_.HasTrajectory(trajectory_id)) {
     const std::unique_ptr<transform::Rigid3d> first_node_odometry =
-        InterpolateOdometry(trajectory_id, first_node_data.time);
+        Interpolate(odometry_data_, trajectory_id, first_node_data.time);
     const std::unique_ptr<transform::Rigid3d> second_node_odometry =
-        InterpolateOdometry(trajectory_id, second_node_data.time);
+        Interpolate(odometry_data_, trajectory_id, second_node_data.time);
     if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
       return first_node_odometry->inverse() * (*second_node_odometry);
     }

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -96,8 +96,6 @@ class OptimizationProblem {
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
 
  private:
-  std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
-      int trajectory_id, common::Time time) const;
   // Uses odometry if available, otherwise the local SLAM results.
   transform::Rigid3d ComputeRelativePose(
       int trajectory_id, const NodeData& first_node_data,
@@ -114,8 +112,8 @@ class OptimizationProblem {
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
+  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
   std::vector<TrajectoryData> trajectory_data_;
-  std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
 };
 
 }  // namespace sparse_pose_graph

cartographer/sensor/map_by_time.h

@@ -52,6 +52,9 @@ class MapByTime {
             const mapping::NodeId& node_id) {
     const int trajectory_id = node_id.trajectory_id;
     CHECK_GE(trajectory_id, 0);
+    if (data_.count(trajectory_id) == 0) {
+      return;
+    }
 
     // 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
@@ -68,7 +71,7 @@ class MapByTime {
                                      : common::Time::max();
     CHECK_LT(gap_start, gap_end);
 
-    auto& trajectory = data_[trajectory_id];
+    auto& trajectory = data_.at(trajectory_id);
     auto data_it = trajectory.lower_bound(gap_start);
     auto data_end = trajectory.upper_bound(gap_end);
     if (data_it == data_end) {

cartographer/sensor/map_by_time_test.cc

@@ -88,6 +88,15 @@ TEST(MapByTimeTest, Trimming) {
   EXPECT_FALSE(map_by_time.HasTrajectory(42));
 }
 
+TEST(MapByTimeTest, TrimmingDoesNotCreateTrajectory) {
+  MapByTime<Data> map_by_time;
+  EXPECT_FALSE(map_by_time.HasTrajectory(42));
+  mapping::MapById<mapping::NodeId, NodeData> map_by_id;
+  map_by_id.Append(42, NodeData{CreateTime(42)});
+  map_by_time.Trim(map_by_id, mapping::NodeId{42, 0});
+  EXPECT_FALSE(map_by_time.HasTrajectory(42));
+}
+
 }  // namespace
 }  // namespace sensor
 }  // namespace cartographer