Remove the deque for constant node data. (#326)

This is needed in preparation of deleting nodes.
Related to #283.

cartographer/mapping/trajectory_node.h

@@ -17,7 +17,7 @@
 #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
 #define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
 
-#include <deque>
+#include <memory>
 #include <vector>
 
 #include "Eigen/Core"
@@ -31,7 +31,7 @@ namespace mapping {
 class Submaps;
 
 struct TrajectoryNode {
-  struct ConstantData {
+  struct Data {
     common::Time time;
 
     // Range data in 'pose' frame. Only used in the 2D case.
@@ -51,7 +51,9 @@ struct TrajectoryNode {
 
   common::Time time() const { return constant_data->time; }
 
-  const ConstantData* constant_data;
+  // This must be a shared_ptr. If the data is used for visualization while the
+  // node is being deleted, it must survive until all use finishes.
+  std::shared_ptr<const Data> constant_data;
 
   transform::Rigid3d pose;
 };

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -97,14 +97,15 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
 
   common::MutexLocker locker(&mutex_);
-  constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
+  trajectory_nodes_.Append(
+      trajectory_id,
+      mapping::TrajectoryNode{
+          std::make_shared<const mapping::TrajectoryNode::Data>(
+              mapping::TrajectoryNode::Data{
                   time, range_data_in_pose,
                   Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
-      trajectory_id, tracking_to_pose});
+                  trajectory_id, tracking_to_pose}),
-  trajectory_nodes_.Append(trajectory_id,
+          optimized_pose});
-                           mapping::TrajectoryNode{
-                               &constant_node_data_.back(), optimized_pose,
-                           });
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 
@@ -224,8 +225,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                                  .at(matching_id.trajectory_id)
                                  .size())
           : 0};
-  const mapping::TrajectoryNode::ConstantData* const scan_data =
+  const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
-      trajectory_nodes_.at(node_id).constant_data;
   CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(
       matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
@@ -466,8 +466,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     } else if (result.empty()) {
       result.push_back(transform::Rigid3d::Identity());
     } else {
-      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
+      // Extrapolate to the remaining submaps. Accessing 'local_pose' in
-      // is okay, since the member is const.
+      // Submaps is okay, since the member is const.
       const mapping::SubmapId previous_submap_id{
           trajectory_id, static_cast<int>(result.size()) - 1};
       result.push_back(
@@ -493,8 +493,8 @@ int SparsePoseGraph::TrimmingHandle::num_submaps(
 
 void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
     const mapping::SubmapId& submap_id) {
-  // TODO(hrapp): We have to make sure that the trajectory has been finished if
+  // TODO(hrapp): We have to make sure that the trajectory has been finished
-  // we want to delete the last submaps.
+  // if we want to delete the last submaps.
   CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
 
   // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
@@ -538,7 +538,6 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
   // Mark the submap with 'submap_id' as trimmed and remove its data.
   parent_->submap_data_.at(submap_id).state = SubmapState::kTrimmed;
   parent_->constraint_builder_.DeleteScanMatcher(submap_id);
-
   // TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
   // there.
 
@@ -548,8 +547,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
   // TODO(whess): The optimization problem should no longer include the submap
   // and the removed nodes.
 
-  // TODO(whess): If the first submap is gone, we want to tie the first not yet
+  // TODO(whess): If the first submap is gone, we want to tie the first not
-  // trimmed submap to be set fixed to its current pose.
+  // yet trimmed submap to be set fixed to its current pose.
 }
 
 }  // namespace mapping_2d

cartographer/mapping_2d/sparse_pose_graph.h

@@ -194,10 +194,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::map<int, size_t> reverse_connected_components_;
 
   // Data that are currently being shown.
-  //
-  // Deque to keep references valid for the background computation when adding
-  // new data.
-  std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
       trajectory_nodes_ GUARDED_BY(mutex_);
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -95,13 +95,15 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * pose);
 
   common::MutexLocker locker(&mutex_);
-  constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
-      time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
-      sensor::Compress(range_data_in_tracking), trajectory_id,
-      transform::Rigid3d::Identity()});
   trajectory_nodes_.Append(
       trajectory_id,
-      mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
+      mapping::TrajectoryNode{
+          std::make_shared<const mapping::TrajectoryNode::Data>(
+              mapping::TrajectoryNode::Data{
+                  time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
+                  sensor::Compress(range_data_in_tracking), trajectory_id,
+                  transform::Rigid3d::Identity()}),
+          optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 
@@ -240,8 +242,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                                  .at(matching_id.trajectory_id)
                                  .size())
           : 0};
-  const mapping::TrajectoryNode::ConstantData* const scan_data =
+  const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
-      trajectory_nodes_.at(node_id).constant_data;
   CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
                                           scan_data->time, optimized_pose);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -191,10 +191,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::map<int, size_t> reverse_connected_components_;
 
   // Data that are currently being shown.
-  //
-  // Deque to keep references valid for the background computation when adding
-  // new data.
-  std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
       trajectory_nodes_ GUARDED_BY(mutex_);
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;