Refactoring: Do not pass 'constant_data' in pieces. (#488)

Also removes the no longer used range data from trajectory nodes
in 2D.

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -38,11 +38,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
     return;
   }
   sparse_pose_graph_->AddScan(
-      insertion_result->time, insertion_result->tracking_to_tracking_2d,
+      insertion_result->constant_data, insertion_result->pose_estimate_2d,
-      insertion_result->range_data_in_tracking_2d,
+      trajectory_id_, insertion_result->insertion_submaps);
-      insertion_result->filtered_point_cloud_in_tracking_2d,
-      insertion_result->pose_estimate_2d, trajectory_id_,
-      insertion_result->insertion_submaps);
 }
 
 void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -17,6 +17,7 @@
 #include "cartographer/mapping_2d/local_trajectory_builder.h"
 
 #include <limits>
+#include <memory>
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/sensor/range_data.h"
@@ -186,10 +187,16 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   const sensor::PointCloud filtered_point_cloud_in_tracking_2d =
       adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns);
 
-  return common::make_unique<InsertionResult>(
+  return common::make_unique<InsertionResult>(InsertionResult{
-      InsertionResult{time, std::move(insertion_submaps),
+      std::make_shared<const mapping::TrajectoryNode::Data>(
-                      tracking_to_tracking_2d, range_data_in_tracking_2d,
+          mapping::TrajectoryNode::Data{
-                      pose_estimate_2d, filtered_point_cloud_in_tracking_2d});
+              time,
+              {},  // 'range_data' is only used in 3D.
+              filtered_point_cloud_in_tracking_2d,
+              {},  // 'high_resolution_point_cloud' is only used in 3D.
+              {},  // 'low_resolution_point_cloud' is only used in 3D.
+              tracking_to_tracking_2d}),
+      pose_estimate_2d, std::move(insertion_submaps)});
 }
 
 const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

cartographer/mapping_2d/local_trajectory_builder.h

@@ -41,12 +41,9 @@ namespace mapping_2d {
 class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
-    common::Time time;
+    std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-    transform::Rigid3d tracking_to_tracking_2d;
-    sensor::RangeData range_data_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;
-    sensor::PointCloud filtered_point_cloud_in_tracking_2d;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 
   explicit LocalTrajectoryBuilder(

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -94,9 +94,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 }
 
 void SparsePoseGraph::AddScan(
-    common::Time time, const transform::Rigid3d& tracking_to_pose,
+    std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const sensor::RangeData& range_data_in_pose,
-    const sensor::PointCloud& filtered_point_cloud,
     const transform::Rigid2d& pose, const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
@@ -104,16 +102,7 @@ void SparsePoseGraph::AddScan(
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
-      trajectory_id,
+      trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
-      mapping::TrajectoryNode{
-          std::make_shared<const mapping::TrajectoryNode::Data>(
-              mapping::TrajectoryNode::Data{time,
-                                            Compress(range_data_in_pose),
-                                            filtered_point_cloud,
-                                            {},
-                                            {},
-                                            tracking_to_pose}),
-          optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -64,16 +64,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   SparsePoseGraph(const SparsePoseGraph&) = delete;
   SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
 
-  // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' that
+  // Adds a new node with 'constant_data' and a 'pose' that will later be
-  // will later be optimized. The 'tracking_to_pose' is remembered so that the
+  // optimized. The 'pose' was determined by scan matching against
-  // optimized pose can be embedded into 3D. The 'pose' was determined by scan
+  // 'insertion_submaps.front()' and the scan was inserted into the
-  // matching against the 'insertion_submaps.front()' and the scan was inserted
+  // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
-  // into the 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
   // 'true', this submap was inserted into for the last time.
   void AddScan(
-      common::Time time, const transform::Rigid3d& tracking_to_pose,
+      std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const sensor::RangeData& range_data_in_pose,
-      const sensor::PointCloud& filtered_point_cloud,
       const transform::Rigid2d& pose, int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -158,8 +158,14 @@ class SparsePoseGraphTest : public ::testing::Test {
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
 
     sparse_pose_graph_->AddScan(
-        common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
+        std::make_shared<const mapping::TrajectoryNode::Data>(
-        range_data.returns, pose_estimate, kTrajectoryId, insertion_submaps);
+            mapping::TrajectoryNode::Data{common::FromUniversal(0),
+                                          Compress(range_data),
+                                          range_data.returns,
+                                          {},
+                                          {},
+                                          transform::Rigid3d::Identity()}),
+        pose_estimate, kTrajectoryId, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -31,17 +31,14 @@ GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 void GlobalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
     const sensor::PointCloud& ranges) {
-  auto insertion_result =
+  std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
       local_trajectory_builder_.AddRangefinderData(time, origin, ranges);
   if (insertion_result == nullptr) {
     return;
   }
   sparse_pose_graph_->AddScan(
-      insertion_result->time, insertion_result->range_data_in_tracking,
+      insertion_result->constant_data, insertion_result->pose_observation,
-      insertion_result->high_resolution_point_cloud,
+      trajectory_id_, insertion_result->insertion_submaps);
-      insertion_result->low_resolution_point_cloud,
-      insertion_result->pose_observation, trajectory_id_,
-      insertion_result->insertion_submaps);
 }
 
 void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -16,6 +16,8 @@
 
 #include "cartographer/mapping_3d/local_trajectory_builder.h"
 
+#include <memory>
+
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
@@ -201,9 +203,16 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
                                  pose_observation.cast<float>()),
       extrapolator_->gravity_orientation());
   return std::unique_ptr<InsertionResult>(new InsertionResult{
-      time, range_data_in_tracking, high_resolution_point_cloud,
+
-      low_resolution_point_cloud, pose_observation,
+      std::make_shared<const mapping::TrajectoryNode::Data>(
-      std::move(insertion_submaps)});
+          mapping::TrajectoryNode::Data{
+              time,
+              sensor::Compress(range_data_in_tracking),
+              {},  // 'filtered_point_cloud' is only used in 2D.
+              high_resolution_point_cloud,
+              low_resolution_point_cloud,
+              transform::Rigid3d::Identity()}),
+      pose_observation, std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/local_trajectory_builder.h

@@ -41,13 +41,7 @@ namespace mapping_3d {
 class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
-    common::Time time;
+    std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    sensor::RangeData range_data_in_tracking;
-
-    // Used for loop closure in 3D.
-    sensor::PointCloud high_resolution_point_cloud;
-    sensor::PointCloud low_resolution_point_cloud;
-
     transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -93,25 +93,14 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 }
 
 void SparsePoseGraph::AddScan(
-    common::Time time, const sensor::RangeData& range_data_in_tracking,
+    std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const sensor::PointCloud& high_resolution_point_cloud,
-    const sensor::PointCloud& low_resolution_point_cloud,
     const transform::Rigid3d& pose, const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * pose);
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
-      trajectory_id, mapping::TrajectoryNode{
+      trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
-                         std::make_shared<const mapping::TrajectoryNode::Data>(
-                             mapping::TrajectoryNode::Data{
-                                 time,
-                                 sensor::Compress(range_data_in_tracking),
-                                 {},
-                                 high_resolution_point_cloud,
-                                 low_resolution_point_cloud,
-                                 transform::Rigid3d::Identity()}),
-                         optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -64,15 +64,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   SparsePoseGraph(const SparsePoseGraph&) = delete;
   SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
 
-  // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
+  // Adds a new node with 'constant_data' and a 'pose' that will later be
-  // that will later be optimized. The 'pose' was determined by scan matching
+  // optimized. The 'pose' was determined by scan matching against
-  // against 'insertion_submaps.front()' and the scan was inserted into the
+  // 'insertion_submaps.front()' and the scan was inserted into the
-  // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is 'true',
+  // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
-  // this submap was inserted into for the last time.
+  // 'true', this submap was inserted into for the last time.
   void AddScan(
-      common::Time time, const sensor::RangeData& range_data_in_tracking,
+      std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const sensor::PointCloud& high_resolution_point_cloud,
-      const sensor::PointCloud& low_resolution_point_cloud,
       const transform::Rigid3d& pose, int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);