Store rotational histogram as part of the node data. (#503)

The compressed point cloud is no longer stored as part of the
node data to reduce memory consumption.

cartographer/mapping/map_builder.cc

@@ -181,9 +181,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
         // TODO(whess): Handle trimmed data.
         range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
         range_data_proto->mutable_node_id()->set_node_index(node_index);
-        const auto& data = *node_data[trajectory_id][node_index].constant_data;
+        range_data_proto->mutable_range_data();
-        *range_data_proto->mutable_range_data() =
-            sensor::ToProto(data.range_data);
         // TODO(whess): Only enable optionally? Resulting pbstream files will be
         // a lot larger now.
         writer->WriteProto(proto);

cartographer/mapping/trajectory_node.h

@@ -32,9 +32,6 @@ struct TrajectoryNode {
   struct Data {
     common::Time time;
 
-    // Range data in 'tracking' frame. Only used in 3D.
-    sensor::CompressedRangeData range_data;
-
     // Transform to approximately gravity align the tracking frame as
     // determined by local SLAM.
     Eigen::Quaterniond gravity_alignment;
@@ -46,6 +43,7 @@ struct TrajectoryNode {
     // Used for loop closure in 3D.
     sensor::PointCloud high_resolution_point_cloud;
     sensor::PointCloud low_resolution_point_cloud;
+    Eigen::VectorXf rotational_scan_matcher_histogram;
   };
 
   common::Time time() const { return constant_data->time; }

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -180,7 +180,6 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::TrajectoryNode::Data{
               time,
-              {},  // 'range_data' is only used in 3D.
               gravity_alignment.rotation(),
               filtered_gravity_aligned_point_cloud,
               {},  // 'high_resolution_point_cloud' is only used in 3D.

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -92,7 +92,6 @@ class SparsePoseGraphTest : public ::testing::Test {
               fast_correlative_scan_matcher_3d = {
                 branch_and_bound_depth = 3,
                 full_resolution_depth = 3,
-                rotational_histogram_size = 30,
                 min_rotational_score = 0.1,
                 min_low_resolution_score = 0.5,
                 linear_xy_search_window = 4.,
@@ -160,10 +159,10 @@ class SparsePoseGraphTest : public ::testing::Test {
     sparse_pose_graph_->AddScan(
         std::make_shared<const mapping::TrajectoryNode::Data>(
             mapping::TrajectoryNode::Data{common::FromUniversal(0),
-                                          Compress(range_data),
                                           Eigen::Quaterniond::Identity(),
                                           range_data.returns,
                                           {},
+                                          {},
                                           {}}),
         transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
   }

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -24,6 +24,7 @@
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/proto/submaps_options.pb.h"
 #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -158,6 +159,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   extrapolator_->AddPose(time, pose_estimate);
   const Eigen::Quaterniond gravity_alignment =
       extrapolator_->EstimateGravityOrientation(time);
+  const auto rotational_scan_matcher_histogram =
+      scan_matching::RotationalScanMatcher::ComputeHistogram(
+          sensor::TransformPointCloud(
+              filtered_range_data.returns,
+              transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
+          options_.rotational_histogram_size());
 
   last_pose_estimate_ = {
       time, pose_estimate,
@@ -167,7 +174,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   return InsertIntoSubmap(time, filtered_range_data, gravity_alignment,
                           filtered_point_cloud_in_tracking,
                           low_resolution_point_cloud_in_tracking,
-                          pose_estimate);
+                          rotational_scan_matcher_histogram, pose_estimate);
 }
 
 void LocalTrajectoryBuilder::AddOdometerData(
@@ -190,6 +197,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
     const Eigen::Quaterniond& gravity_alignment,
     const sensor::PointCloud& high_resolution_point_cloud,
     const sensor::PointCloud& low_resolution_point_cloud,
+    const Eigen::VectorXf& rotational_scan_matcher_histogram,
     const transform::Rigid3d& pose_observation) {
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
@@ -209,11 +217,11 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::TrajectoryNode::Data{
               time,
-              sensor::Compress(range_data_in_tracking),
               gravity_alignment,
               {},  // 'filtered_point_cloud' is only used in 2D.
               high_resolution_point_cloud,
-              low_resolution_point_cloud}),
+              low_resolution_point_cloud,
+              rotational_scan_matcher_histogram}),
       pose_observation, std::move(insertion_submaps)});
 }
 

cartographer/mapping_3d/local_trajectory_builder.h

@@ -68,6 +68,7 @@ class LocalTrajectoryBuilder {
       const Eigen::Quaterniond& gravity_alignment,
       const sensor::PointCloud& high_resolution_point_cloud,
       const sensor::PointCloud& low_resolution_point_cloud,
+      const Eigen::VectorXf& rotational_scan_matcher_histogram,
       const transform::Rigid3d& pose_observation);
 
   const proto::LocalTrajectoryBuilderOptions options_;

cartographer/mapping_3d/local_trajectory_builder_options.cc

@@ -59,6 +59,8 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
       parameter_dictionary->GetDictionary("motion_filter").get());
   options.set_imu_gravity_time_constant(
       parameter_dictionary->GetDouble("imu_gravity_time_constant"));
+  options.set_rotational_histogram_size(
+      parameter_dictionary->GetInt("rotational_histogram_size"));
   *options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions(
       parameter_dictionary->GetDictionary("submaps").get());
   return options;

cartographer/mapping_3d/local_trajectory_builder_test.cc

@@ -92,6 +92,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
           },
 
           imu_gravity_time_constant = 1.,
+          rotational_histogram_size = 120,
 
           submaps = {
             high_resolution = 0.2,

cartographer/mapping_3d/proto/local_trajectory_builder_options.proto

@@ -22,7 +22,7 @@ import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_m
 import "cartographer/mapping_3d/proto/submaps_options.proto";
 import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
 
-// NEXT ID: 17
+// NEXT ID: 18
 message LocalTrajectoryBuilderOptions {
   // Rangefinder points outside these ranges will be dropped.
   optional float min_range = 1;
@@ -59,5 +59,8 @@ message LocalTrajectoryBuilderOptions {
   // constant is increased) is balanced.
   optional double imu_gravity_time_constant = 15;
 
+  // Number of histogram buckets for the rotational scan matcher.
+  optional int32 rotational_histogram_size = 17;
+
   optional SubmapsOptions submaps_options = 8;
 }

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -42,8 +42,6 @@ CreateFastCorrelativeScanMatcherOptions(
       parameter_dictionary->GetInt("branch_and_bound_depth"));
   options.set_full_resolution_depth(
       parameter_dictionary->GetInt("full_resolution_depth"));
-  options.set_rotational_histogram_size(
-      parameter_dictionary->GetInt("rotational_histogram_size"));
   options.set_min_rotational_score(
       parameter_dictionary->GetDouble("min_rotational_score"));
   options.set_min_low_resolution_score(
@@ -127,18 +125,17 @@ struct Candidate {
 
 namespace {
 
-scan_matching::RotationalScanMatcher CreateRotationalScanMatcher(
+std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
-    const std::vector<mapping::TrajectoryNode>& nodes,
+    const std::vector<mapping::TrajectoryNode>& nodes) {
-    const int histogram_size) {
+  std::vector<std::pair<Eigen::VectorXf, float>> histograms_at_angles;
-  Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size);
+  for (const auto& node : nodes) {
-  for (const mapping::TrajectoryNode& node : nodes) {
+    histograms_at_angles.emplace_back(
-    histogram += scan_matching::RotationalScanMatcher::ComputeHistogram(
+        node.constant_data->rotational_scan_matcher_histogram,
-        sensor::TransformPointCloud(
+        transform::GetYaw(
-            node.constant_data->range_data.returns.Decompress(),
+            node.pose * transform::Rigid3d::Rotation(
-            node.pose.cast<float>()),
+                            node.constant_data->gravity_alignment.inverse())));
-        histogram_size);
   }
-  return scan_matching::RotationalScanMatcher({{histogram, 0.f}});
+  return histograms_at_angles;
 }
 
 }  // namespace
@@ -154,8 +151,7 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
       precomputation_grid_stack_(
           common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
       low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
-      rotational_scan_matcher_(CreateRotationalScanMatcher(
+      rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
-          nodes, options_.rotational_histogram_size())) {}
 
 FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
 
@@ -173,8 +169,9 @@ bool FastCorrelativeScanMatcher::Match(
   return MatchWithSearchParameters(
       search_parameters, initial_pose_estimate,
       constant_data.high_resolution_point_cloud,
-      constant_data.range_data.returns.Decompress(), min_score, score,
+      constant_data.rotational_scan_matcher_histogram,
-      pose_estimate, rotational_score, low_resolution_score);
+      constant_data.gravity_alignment, min_score, score, pose_estimate,
+      rotational_score, low_resolution_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchFullSubmap(
@@ -199,23 +196,25 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
   return MatchWithSearchParameters(
       search_parameters, initial_pose_estimate,
       constant_data.high_resolution_point_cloud,
-      constant_data.range_data.returns.Decompress(), min_score, score,
+      constant_data.rotational_scan_matcher_histogram,
-      pose_estimate, rotational_score, low_resolution_score);
+      constant_data.gravity_alignment, min_score, score, pose_estimate,
+      rotational_score, low_resolution_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
     const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const transform::Rigid3d& initial_pose_estimate,
-    const sensor::PointCloud& coarse_point_cloud,
+    const sensor::PointCloud& point_cloud,
-    const sensor::PointCloud& fine_point_cloud, const float min_score,
+    const Eigen::VectorXf& rotational_scan_matcher_histogram,
+    const Eigen::Quaterniond& gravity_alignment, const float min_score,
     float* const score, transform::Rigid3d* const pose_estimate,
     float* const rotational_score, float* const low_resolution_score) const {
   CHECK_NOTNULL(score);
   CHECK_NOTNULL(pose_estimate);
 
   const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
-      search_parameters, coarse_point_cloud, fine_point_cloud,
+      search_parameters, point_cloud, rotational_scan_matcher_histogram,
-      initial_pose_estimate.cast<float>());
+      gravity_alignment, initial_pose_estimate.cast<float>());
 
   const std::vector<Candidate> lowest_resolution_candidates =
       ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
@@ -281,14 +280,15 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
 
 std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
     const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
-    const sensor::PointCloud& coarse_point_cloud,
+    const sensor::PointCloud& point_cloud,
-    const sensor::PointCloud& fine_point_cloud,
+    const Eigen::VectorXf& rotational_scan_matcher_histogram,
+    const Eigen::Quaterniond& gravity_alignment,
     const transform::Rigid3f& initial_pose) const {
   std::vector<DiscreteScan> result;
   // We set this value to something on the order of resolution to make sure that
   // the std::acos() below is defined.
   float max_scan_range = 3.f * resolution_;
-  for (const Eigen::Vector3f& point : coarse_point_cloud) {
+  for (const Eigen::Vector3f& point : point_cloud) {
     const float range = point.norm();
     max_scan_range = std::max(range, max_scan_range);
   }
@@ -305,10 +305,10 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
     angles.push_back(rz * angular_step_size);
   }
   const std::vector<float> scores = rotational_scan_matcher_.Match(
-      RotationalScanMatcher::ComputeHistogram(
+      rotational_scan_matcher_histogram,
-          sensor::TransformPointCloud(fine_point_cloud, initial_pose),
+      transform::GetYaw(initial_pose.rotation() *
-          options_.rotational_histogram_size()),
+                        gravity_alignment.inverse().cast<float>()),
-      0.f /* initial_angle */, angles);
+      angles);
   for (size_t i = 0; i != angles.size(); ++i) {
     if (scores[i] < options_.min_rotational_score()) {
       continue;
@@ -322,7 +322,7 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
         transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
             initial_pose.rotation());
     result.push_back(
-        DiscretizeScan(search_parameters, coarse_point_cloud, pose, scores[i]));
+        DiscretizeScan(search_parameters, point_cloud, pose, scores[i]));
   }
   return result;
 }

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -60,22 +60,20 @@ class FastCorrelativeScanMatcher {
   FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) =
       delete;
 
-  // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an
+  // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
-  // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
+  // given an 'initial_pose_estimate'. If a score above 'min_score' (excluding
-  // is possible, true is returned, and 'score', 'pose_estimate',
+  // equality) is possible, true is returned, and 'score', 'pose_estimate',
   // 'rotational_score', and 'low_resolution_score' are updated with the result.
-  // 'fine_point_cloud' is used to compute the rotational scan matcher score.
   bool Match(const transform::Rigid3d& initial_pose_estimate,
              const mapping::TrajectoryNode::Data& constant_data,
              float min_score, float* score, transform::Rigid3d* pose_estimate,
              float* rotational_score, float* low_resolution_score) const;
 
-  // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which
+  // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
-  // is expected to be approximately gravity aligned. If a score above
+  // given a rotation which is expected to be approximately gravity aligned.
-  // 'min_score' (excluding equality) is possible, true is returned, and
+  // If a score above 'min_score' (excluding equality) is possible, true is
-  // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score'
+  // returned, and 'score', 'pose_estimate', 'rotational_score', and
-  // are updated with the result. 'fine_point_cloud' is used to compute the
+  // 'low_resolution_score' are updated with the result.
-  // rotational scan matcher score.
   bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
                        const mapping::TrajectoryNode::Data& constant_data,
                        float min_score, float* score,
@@ -94,9 +92,10 @@ class FastCorrelativeScanMatcher {
   bool MatchWithSearchParameters(
       const SearchParameters& search_parameters,
       const transform::Rigid3d& initial_pose_estimate,
-      const sensor::PointCloud& coarse_point_cloud,
+      const sensor::PointCloud& point_cloud,
-      const sensor::PointCloud& fine_point_cloud, float min_score, float* score,
+      const Eigen::VectorXf& rotational_scan_matcher_histogram,
-      transform::Rigid3d* pose_estimate, float* rotational_score,
+      const Eigen::Quaterniond& gravity_alignment, float min_score,
+      float* score, transform::Rigid3d* pose_estimate, float* rotational_score,
       float* low_resolution_score) const;
   DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
                               const sensor::PointCloud& point_cloud,
@@ -104,8 +103,9 @@ class FastCorrelativeScanMatcher {
                               float rotational_score) const;
   std::vector<DiscreteScan> GenerateDiscreteScans(
       const SearchParameters& search_parameters,
-      const sensor::PointCloud& coarse_point_cloud,
+      const sensor::PointCloud& point_cloud,
-      const sensor::PointCloud& fine_point_cloud,
+      const Eigen::VectorXf& rotational_scan_matcher_histogram,
+      const Eigen::Quaterniond& gravity_alignment,
       const transform::Rigid3f& initial_pose) const;
   std::vector<Candidate> GenerateLowestResolutionCandidates(
       const SearchParameters& search_parameters, int num_discrete_scans) const;

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -70,7 +70,6 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
         "full_resolution_depth = " +
         std::to_string(branch_and_bound_depth) +
         ", "
-        "rotational_histogram_size = 30, "
         "min_rotational_score = 0.1, "
         // Unknown space has kMinProbability = 0.1, so we need to make sure here
         // to pick a larger number otherwise we always find matches.
@@ -106,18 +105,21 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
 
     return common::make_unique<FastCorrelativeScanMatcher>(
         *hybrid_grid_, hybrid_grid_.get(),
-        std::vector<mapping::TrajectoryNode>(), options);
+        std::vector<mapping::TrajectoryNode>(
+            {{std::make_shared<const mapping::TrajectoryNode::Data>(
+                  CreateConstantData(point_cloud_)),
+              pose.cast<double>()}}),
+        options);
   }
 
   mapping::TrajectoryNode::Data CreateConstantData(
       const sensor::PointCloud& low_resolution_point_cloud) {
-    return mapping::TrajectoryNode::Data{
+    return mapping::TrajectoryNode::Data{common::FromUniversal(0),
-        common::FromUniversal(0),
+                                         Eigen::Quaterniond::Identity(),
-        Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}),
+                                         {},
-        Eigen::Quaterniond::Identity(),
+                                         point_cloud_,
-        {},
+                                         low_resolution_point_cloud,
-        point_cloud_,
+                                         Eigen::VectorXf::Zero(10)};
-        low_resolution_point_cloud};
   }
 
   std::mt19937 prng_ = std::mt19937(42);

cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto

@@ -24,9 +24,6 @@ message FastCorrelativeScanMatcherOptions {
   // resolution by half each.
   optional int32 full_resolution_depth = 8;
 
-  // Number of histogram buckets for the rotational scan matcher.
-  optional int32 rotational_histogram_size = 3;
-
   // Minimum score for the rotational scan matcher.
   optional double min_rotational_score = 4;
 

configuration_files/sparse_pose_graph.lua

@@ -40,7 +40,6 @@ SPARSE_POSE_GRAPH = {
     fast_correlative_scan_matcher_3d = {
       branch_and_bound_depth = 8,
       full_resolution_depth = 3,
-      rotational_histogram_size = 120,
       min_rotational_score = 0.77,
       min_low_resolution_score = 0.55,
       linear_xy_search_window = 5.,

configuration_files/trajectory_builder_3d.lua

@@ -60,6 +60,7 @@ TRAJECTORY_BUILDER_3D = {
   },
 
   imu_gravity_time_constant = 10.,
+  rotational_histogram_size = 120,
 
   submaps = {
     high_resolution = 0.10,