Always use the origin as the origin for grids. (#294)

Also removes convenience functions from mapping_3d::Submap.

PAIR=SirVer

cartographer/io/outlier_removing_points_processor.cc

@@ -36,7 +36,7 @@ OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor(
     : voxel_size_(voxel_size),
       next_(next),
       state_(State::kPhase1),
-      voxels_(voxel_size_, Eigen::Vector3f::Zero()) {
+      voxels_(voxel_size_) {
   LOG(INFO) << "Marking hits...";
 }
 

cartographer/io/xray_points_processor.cc

@@ -142,9 +142,8 @@ XRayPointsProcessor::XRayPointsProcessor(
       output_filename_(output_filename),
       transform_(transform) {
   for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
-    aggregations_.emplace_back(Aggregation{
+    aggregations_.emplace_back(
-        mapping_3d::HybridGridBase<bool>(voxel_size, Eigen::Vector3f::Zero()),
+        Aggregation{mapping_3d::HybridGridBase<bool>(voxel_size), {}});
-        {}});
   }
 }
 

cartographer/mapping_3d/hybrid_grid.h

@@ -415,20 +415,18 @@ class HybridGridBase : public Grid<ValueType> {
  public:
   using Iterator = typename Grid<ValueType>::Iterator;
 
-  // Creates a new tree-based probability grid around 'origin' which becomes the
+  // Creates a new tree-based probability grid with voxels having edge length
-  // center of the cell at index (0, 0, 0). Each voxel has edge length
+  // 'resolution' around the origin which becomes the center of the cell at
-  // 'resolution'.
+  // index (0, 0, 0).
-  HybridGridBase(const float resolution, const Eigen::Vector3f& origin)
+  explicit HybridGridBase(const float resolution) : resolution_(resolution) {}
-      : resolution_(resolution), origin_(origin) {}
 
   float resolution() const { return resolution_; }
-  Eigen::Vector3f origin() const { return origin_; }
 
   // Returns the index of the cell containing the 'point'. Indices are integer
-  // vectors identifying cells, for this the relative distance from the origin
+  // vectors identifying cells, for this the coordinates are rounded to the next
-  // is rounded to the next multiple of the resolution.
+  // multiple of the resolution.
   Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const {
-    Eigen::Array3f index = (point - origin_).array() / resolution_;
+    Eigen::Array3f index = point.array() / resolution_;
     return Eigen::Array3i(common::RoundToInt(index.x()),
                           common::RoundToInt(index.y()),
                           common::RoundToInt(index.z()));
@@ -444,7 +442,7 @@ class HybridGridBase : public Grid<ValueType> {
 
   // Returns the center of the cell at 'index'.
   Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const {
-    return index.matrix().cast<float>() * resolution_ + origin_;
+    return index.matrix().cast<float>() * resolution_;
   }
 
   // Iterator functions for range-based for loops.
@@ -459,20 +457,17 @@ class HybridGridBase : public Grid<ValueType> {
  private:
   // Edge length of each voxel.
   const float resolution_;
-
-  // Position of the center of the octree.
-  const Eigen::Vector3f origin_;
 };
 
 // A grid containing probability values stored using 15 bits, and an update
 // marker per voxel.
 class HybridGrid : public HybridGridBase<uint16> {
  public:
-  HybridGrid(const float resolution, const Eigen::Vector3f& origin)
+  explicit HybridGrid(const float resolution)
-      : HybridGridBase<uint16>(resolution, origin) {}
+      : HybridGridBase<uint16>(resolution) {}
 
-  HybridGrid(const proto::HybridGrid& proto)
+  explicit HybridGrid(const proto::HybridGrid& proto)
-      : HybridGrid(proto.resolution(), transform::ToEigen(proto.origin())) {
+      : HybridGrid(proto.resolution()) {
     CHECK_EQ(proto.values_size(), proto.x_indices_size());
     CHECK_EQ(proto.values_size(), proto.y_indices_size());
     CHECK_EQ(proto.values_size(), proto.z_indices_size());
@@ -535,7 +530,6 @@ class HybridGrid : public HybridGridBase<uint16> {
 inline proto::HybridGrid ToProto(const HybridGrid& grid) {
   proto::HybridGrid result;
   result.set_resolution(grid.resolution());
-  *result.mutable_origin() = transform::ToProto(grid.origin());
   for (const auto it : grid) {
     result.add_x_indices(it.first.x());
     result.add_y_indices(it.first.y());

cartographer/mapping_3d/hybrid_grid_test.cc

@@ -27,7 +27,7 @@ namespace mapping_3d {
 namespace {
 
 TEST(HybridGridTest, ApplyOdds) {
-  HybridGrid hybrid_grid(1.f, Eigen::Vector3f(-0.5f, -0.5f, -0.5f));
+  HybridGrid hybrid_grid(1.f);
 
   EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 0)));
   EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 0)));
@@ -74,18 +74,18 @@ TEST(HybridGridTest, ApplyOdds) {
 }
 
 TEST(HybridGridTest, GetProbability) {
-  HybridGrid hybrid_grid(1.f, Eigen::Vector3f(-0.5f, -0.5f, -0.5f));
+  HybridGrid hybrid_grid(1.f);
 
   hybrid_grid.SetProbability(
-      hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 0.5f, 0.5f)),
+      hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)),
       mapping::kMaxProbability);
   EXPECT_NEAR(hybrid_grid.GetProbability(
-                  hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 0.5f, 0.5f))),
+                  hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))),
               mapping::kMaxProbability, 1e-6);
   for (const Eigen::Array3i& index :
-       {hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 1.5, 0.5f)),
+       {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)),
-        hybrid_grid.GetCellIndex(Eigen::Vector3f(.5f, 0.5, 0.5f)),
+        hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)),
-        hybrid_grid.GetCellIndex(Eigen::Vector3f(0.5f, 1.5, 0.5f))}) {
+        hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) {
     EXPECT_FALSE(hybrid_grid.IsKnown(index));
   }
 }
@@ -93,43 +93,42 @@ TEST(HybridGridTest, GetProbability) {
 MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); }
 
 TEST(HybridGridTest, GetCellIndex) {
-  HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -13.f, -2.f));
+  HybridGrid hybrid_grid(2.f);
 
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-7.f, -13.f, -2.f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 0.f, 0.f)),
               AllCwiseEqual(Eigen::Array3i(0, 0, 0)));
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-7.f, 13.f, 8.f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 26.f, 10.f)),
               AllCwiseEqual(Eigen::Array3i(0, 13, 5)));
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.f, -13.f, 8.f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 0.f, 10.f)),
               AllCwiseEqual(Eigen::Array3i(7, 0, 5)));
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.f, 13.f, -2.f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 26.f, 0.f)),
               AllCwiseEqual(Eigen::Array3i(7, 13, 0)));
 
   // Check around the origin.
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(1.5f, -1.5f, -1.5f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(8.5f, 11.5f, 0.5f)),
               AllCwiseEqual(Eigen::Array3i(4, 6, 0)));
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.5f, -0.5f, -0.5f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.5f, 12.5f, 1.5f)),
               AllCwiseEqual(Eigen::Array3i(4, 6, 1)));
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 1.5f, 0.5f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(6.5f, 14.5f, 2.5f)),
               AllCwiseEqual(Eigen::Array3i(3, 7, 1)));
-  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-1.5f, 0.5f, 1.5f)),
+  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(5.5f, 13.5f, 3.5f)),
               AllCwiseEqual(Eigen::Array3i(3, 7, 2)));
 }
 
 TEST(HybridGridTest, GetCenterOfCell) {
-  HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -13.f, -2.f));
+  HybridGrid hybrid_grid(2.f);
 
   const Eigen::Array3i index(3, 2, 1);
   const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index);
-  EXPECT_NEAR(-1.f, center.x(), 1e-6);
+  EXPECT_NEAR(6.f, center.x(), 1e-6);
-  EXPECT_NEAR(-9.f, center.y(), 1e-6);
+  EXPECT_NEAR(4.f, center.y(), 1e-6);
-  EXPECT_NEAR(0.f, center.z(), 1e-6);
+  EXPECT_NEAR(2.f, center.z(), 1e-6);
   EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index));
 }
 
 class RandomHybridGridTest : public ::testing::Test {
  public:
-  RandomHybridGridTest()
+  RandomHybridGridTest() : hybrid_grid_(2.f), values_() {
-      : hybrid_grid_(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)), values_() {
     std::mt19937 rng(1285120005);
     std::uniform_real_distribution<float> value_distribution(
         mapping::kMinProbability, mapping::kMaxProbability);
@@ -189,9 +188,6 @@ TEST_F(RandomHybridGridTest, TestIteration) {
 TEST_F(RandomHybridGridTest, ToProto) {
   const auto proto = ToProto(hybrid_grid_);
   EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution());
-  EXPECT_EQ(hybrid_grid_.origin().x(), proto.origin().x());
-  EXPECT_EQ(hybrid_grid_.origin().y(), proto.origin().y());
-  EXPECT_EQ(hybrid_grid_.origin().z(), proto.origin().z());
 
   ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size());
   ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size());

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -140,19 +140,24 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
   pose_tracker_->GetPoseEstimateMeanAndCovariance(
       time, &pose_prediction, &unused_covariance_prediction);
 
-  transform::Rigid3d initial_ceres_pose = pose_prediction;
+  const Submap* const matching_submap =
+      submaps_->Get(submaps_->matching_index());
+  transform::Rigid3d initial_ceres_pose =
+      matching_submap->local_pose().inverse() * pose_prediction;
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.high_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud filtered_point_cloud_in_tracking =
       adaptive_voxel_filter.Filter(filtered_range_data.returns);
   if (options_.kalman_local_trajectory_builder_options()
           .use_online_correlative_scan_matching()) {
+    // We take a copy since we use 'intial_ceres_pose' as an output argument.
+    const transform::Rigid3d initial_pose = initial_ceres_pose;
     real_time_correlative_scan_matcher_->Match(
-        pose_prediction, filtered_point_cloud_in_tracking,
+        initial_pose, filtered_point_cloud_in_tracking,
-        submaps_->high_resolution_matching_grid(), &initial_ceres_pose);
+        matching_submap->high_resolution_hybrid_grid, &initial_ceres_pose);
   }
 
-  transform::Rigid3d pose_observation;
+  transform::Rigid3d pose_observation_in_submap;
   ceres::Solver::Summary summary;
 
   sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
@@ -161,10 +166,12 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
       low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
   ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose,
                              {{&filtered_point_cloud_in_tracking,
-                               &submaps_->high_resolution_matching_grid()},
+                               &matching_submap->high_resolution_hybrid_grid},
                               {&low_resolution_point_cloud_in_tracking,
-                               &submaps_->low_resolution_matching_grid()}},
+                               &matching_submap->low_resolution_hybrid_grid}},
-                             &pose_observation, &summary);
+                             &pose_observation_in_submap, &summary);
+  const transform::Rigid3d pose_observation =
+      matching_submap->local_pose() * pose_observation_in_submap;
   pose_tracker_->AddPoseObservation(
       time, pose_observation,
       options_.kalman_local_trajectory_builder_options()

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -176,7 +176,8 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
     batches_.push_back(
         Batch{time, point_cloud, high_resolution_filtered_points,
               low_resolution_filtered_points,
-              State{{{1., 0., 0., 0.}}, {{0., 0., 0.}}, {{0., 0., 0.}}}});
+              State(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity(),
+                    Eigen::Vector3d::Zero())});
   } else {
     const Batch& last_batch = batches_.back();
     batches_.push_back(Batch{
@@ -214,6 +215,19 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
   }
 }
 
+void OptimizingLocalTrajectoryBuilder::TransformStates(
+    const transform::Rigid3d& transform) {
+  for (Batch& batch : batches_) {
+    const transform::Rigid3d new_pose = transform * batch.state.ToRigid();
+    const auto& velocity = batch.state.velocity;
+    const Eigen::Vector3d new_velocity =
+        transform.rotation() *
+        Eigen::Vector3d(velocity[0], velocity[1], velocity[2]);
+    batch.state =
+        State(new_pose.translation(), new_pose.rotation(), new_velocity);
+  }
+}
+
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
 OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   // TODO(hrapp): Make the number of optimizations configurable.
@@ -223,6 +237,12 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   }
 
   ceres::Problem problem;
+  const Submap* const matching_submap =
+      submaps_->Get(submaps_->matching_index());
+  // We transform the states in 'batches_' in place to be in the submap frame as
+  // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
+  // the optimization problem.
+  TransformStates(matching_submap->local_pose().inverse());
   for (size_t i = 0; i < batches_.size(); ++i) {
     Batch& batch = batches_[i];
     problem.AddResidualBlock(
@@ -234,7 +254,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
                     std::sqrt(static_cast<double>(
                         batch.high_resolution_filtered_points.size())),
                 batch.high_resolution_filtered_points,
-                submaps_->high_resolution_matching_grid()),
+                matching_submap->high_resolution_hybrid_grid),
             batch.high_resolution_filtered_points.size()),
         nullptr, batch.state.translation.data(), batch.state.rotation.data());
     problem.AddResidualBlock(
@@ -246,15 +266,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
                     std::sqrt(static_cast<double>(
                         batch.low_resolution_filtered_points.size())),
                 batch.low_resolution_filtered_points,
-                submaps_->low_resolution_matching_grid()),
+                matching_submap->low_resolution_hybrid_grid),
             batch.low_resolution_filtered_points.size()),
         nullptr, batch.state.translation.data(), batch.state.rotation.data());
 
     if (i == 0) {
       problem.SetParameterBlockConstant(batch.state.translation.data());
+      problem.SetParameterBlockConstant(batch.state.rotation.data());
       problem.AddParameterBlock(batch.state.velocity.data(), 3);
       problem.SetParameterBlockConstant(batch.state.velocity.data());
-      problem.SetParameterBlockConstant(batch.state.rotation.data());
     } else {
       problem.SetParameterization(batch.state.rotation.data(),
                                   new ceres::QuaternionParameterization());
@@ -329,6 +349,9 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
 
   ceres::Solver::Summary summary;
   ceres::Solve(ceres_solver_options_, &problem, &summary);
+  // The optimized states in 'batches_' are in the submap frame and we transform
+  // them in place to be in the local SLAM frame again.
+  TransformStates(matching_submap->local_pose());
   if (num_accumulated_ < options_.scans_per_accumulation()) {
     return nullptr;
   }
@@ -433,10 +456,7 @@ OptimizingLocalTrajectoryBuilder::PredictState(const State& start_state,
       start_rotation * result.delta_velocity -
       gravity_constant_ * delta_time_seconds * Eigen::Vector3d::UnitZ();
 
-  return State{
+  return State(position, orientation, velocity);
-      {{orientation.w(), orientation.x(), orientation.y(), orientation.z()}},
-      {{position.x(), position.y(), position.z()}},
-      {{velocity.x(), velocity.y(), velocity.z()}}};
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -61,12 +61,16 @@ class OptimizingLocalTrajectoryBuilder
 
  private:
   struct State {
-    // TODO(hrapp): This should maybe use a CeresPose.
-    // Rotation quaternion as (w, x, y, z).
-    std::array<double, 4> rotation;
     std::array<double, 3> translation;
+    std::array<double, 4> rotation;  // Rotation quaternion as (w, x, y, z).
     std::array<double, 3> velocity;
 
+    State(const Eigen::Vector3d& translation,
+          const Eigen::Quaterniond& rotation, const Eigen::Vector3d& velocity)
+        : translation{{translation.x(), translation.y(), translation.z()}},
+          rotation{{rotation.w(), rotation.x(), rotation.y(), rotation.z()}},
+          velocity{{velocity.x(), velocity.y(), velocity.z()}} {}
+
     Eigen::Quaterniond ToQuaternion() const {
       return Eigen::Quaterniond(rotation[0], rotation[1], rotation[2],
                                 rotation[3]);
@@ -107,6 +111,7 @@ class OptimizingLocalTrajectoryBuilder
       const common::Time time, const sensor::RangeData& range_data_in_tracking,
       const transform::Rigid3d& pose_observation);
 
+  void TransformStates(const transform::Rigid3d& transform);
   std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
 
   const proto::LocalTrajectoryBuilderOptions options_;

cartographer/mapping_3d/proto/hybrid_grid.proto

@@ -14,13 +14,10 @@
 
 syntax = "proto2";
 
-import "cartographer/transform/proto/transform.proto";
-
 package cartographer.mapping_3d.proto;
 
 message HybridGrid {
   optional float resolution = 1;
-  optional transform.proto.Vector3f origin = 2;
   // '{x, y, z}_indices[i]' is the index of 'values[i]'.
   repeated sint32 x_indices = 3 [packed = true];
   repeated sint32 y_indices = 4 [packed = true];

cartographer/mapping_3d/range_data_inserter_test.cc

@@ -28,8 +28,7 @@ namespace {
 
 class RangeDataInserterTest : public ::testing::Test {
  protected:
-  RangeDataInserterTest()
+  RangeDataInserterTest() : hybrid_grid_(1.f) {
-      : hybrid_grid_(1.f, Eigen::Vector3f(0.5f, 0.5f, 0.5f)) {
     auto parameter_dictionary = common::MakeDictionary(
         "return { "
         "hit_probability = 0.7, "
@@ -41,11 +40,9 @@ class RangeDataInserterTest : public ::testing::Test {
   }
 
   void InsertPointCloud() {
-    const Eigen::Vector3f origin = Eigen::Vector3f(0.5f, 0.5f, -3.5f);
+    const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
-    sensor::PointCloud returns = {{-2.5f, -0.5f, 4.5f},
+    sensor::PointCloud returns = {
-                                  {-1.5f, 0.5f, 4.5f},
+        {-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}};
-                                  {-0.5f, 1.5f, 4.5f},
-                                  {0.5f, 2.5f, 4.5f}};
     range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
                                  &hybrid_grid_);
   }
@@ -70,19 +67,19 @@ class RangeDataInserterTest : public ::testing::Test {
 
 TEST_F(RangeDataInserterTest, InsertPointCloud) {
   InsertPointCloud();
-  EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -3.5f),
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f),
               1e-4);
-  EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -2.5f),
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
               1e-4);
-  EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -1.5f),
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f),
               1e-4);
   for (int x = -4; x <= 4; ++x) {
     for (int y = -4; y <= 4; ++y) {
       if (x < -3 || x > 0 || y != x + 2) {
-        EXPECT_FALSE(IsKnown(x + 0.5f, y + 0.5f, 4.5f));
+        EXPECT_FALSE(IsKnown(x, y, 4.f));
       } else {
-        EXPECT_NEAR(options().hit_probability(),
+        EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f),
-                    GetProbability(x + 0.5f, y + 0.5f, 4.5f), 1e-4);
+                    1e-4);
       }
     }
   }
@@ -90,22 +87,19 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) {
 
 TEST_F(RangeDataInserterTest, ProbabilityProgression) {
   InsertPointCloud();
-  EXPECT_NEAR(options().hit_probability(), GetProbability(-1.5f, 0.5f, 4.5f),
+  EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f),
               1e-4);
-  EXPECT_NEAR(options().miss_probability(), GetProbability(-1.5f, 0.5f, 3.5f),
+  EXPECT_NEAR(options().miss_probability(), GetProbability(-2.f, 0.f, 3.f),
               1e-4);
-  EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -2.5f),
+  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
               1e-4);
 
   for (int i = 0; i < 1000; ++i) {
     InsertPointCloud();
   }
-  EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-1.5f, 0.5f, 4.5f),
+  EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3);
-              1e-3);
+  EXPECT_NEAR(mapping::kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3);
-  EXPECT_NEAR(mapping::kMinProbability, GetProbability(-1.5f, 0.5f, 3.5f),
+  EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3);
-              1e-3);
-  EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.5f, 0.5f, -2.5f),
-              1e-3);
 }
 
 }  // namespace

cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc

@@ -34,10 +34,9 @@ namespace {
 class CeresScanMatcherTest : public ::testing::Test {
  protected:
   CeresScanMatcherTest()
-      : hybrid_grid_(1.f /* resolution */,
+      : hybrid_grid_(1.f),
-                     Eigen::Vector3f(0.5f, 0.5f, 0.5f) /* origin */),
         expected_pose_(
-            transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5))) {
+            transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
     for (const auto& point :
          {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
           Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
@@ -84,23 +83,23 @@ class CeresScanMatcherTest : public ::testing::Test {
 
 TEST_F(CeresScanMatcherTest, PerfectEstimate) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
 }
 
 TEST_F(CeresScanMatcherTest, AlongX) {
   ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.3, 0.5, 0.5)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
 }
 
 TEST_F(CeresScanMatcherTest, AlongZ) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.3)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
 }
 
 TEST_F(CeresScanMatcherTest, AlongXYZ) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.4, 0.3, 0.7)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
 }
 
 TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
@@ -112,8 +111,7 @@ TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
   expected_pose_ = expected_pose_ * additional_transform.inverse();
   // ...starting initially with rotation around x.
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.45, 0.45, 0.55)) *
+      transform::Rigid3d(Eigen::Vector3d(-0.95, -0.05, 0.05),
-      transform::Rigid3d::Rotation(
                          Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.))));
 }
 

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -95,7 +95,6 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
     const proto::FastCorrelativeScanMatcherOptions& options)
     : options_(options),
       resolution_(hybrid_grid.resolution()),
-      origin_(hybrid_grid.origin()),
       width_in_voxels_(hybrid_grid.grid_size()),
       precomputation_grid_stack_(
           common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
@@ -122,7 +121,7 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
     float* const score, transform::Rigid3d* const pose_estimate) const {
-  const transform::Rigid3d initial_pose_estimate(origin_.cast<double>(),
+  const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
                                                  gravity_alignment);
   float max_point_distance = 0.f;
   for (const Eigen::Vector3f& point : coarse_point_cloud) {

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -136,7 +136,6 @@ class FastCorrelativeScanMatcher {
 
   const proto::FastCorrelativeScanMatcherOptions options_;
   const float resolution_;
-  const Eigen::Vector3f origin_;
   const int width_in_voxels_;
   std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
   RotationalScanMatcher rotational_scan_matcher_;

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -87,8 +87,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
         transform::Rigid3f::Rotation(
             Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
 
-    HybridGrid hybrid_grid(0.05f /* resolution */,
+    HybridGrid hybrid_grid(0.05f);
-                           Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */);
     hybrid_grid.StartUpdate();
     range_data_inserter.Insert(
         sensor::RangeData{

cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc

@@ -28,9 +28,7 @@ namespace {
 class InterpolatedGridTest : public ::testing::Test {
  protected:
   InterpolatedGridTest()
-      : hybrid_grid_(0.1f /* resolution */,
+      : hybrid_grid_(0.1f), interpolated_grid_(hybrid_grid_) {
-                     Eigen::Vector3f(1.5f, 2.5f, 3.5f) /* origin */),
-        interpolated_grid_(hybrid_grid_) {
     for (const auto& point :
          {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
           Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),

cartographer/mapping_3d/scan_matching/precomputation_grid.cc

@@ -48,7 +48,7 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
 }  // namespace
 
 PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) {
-  PrecomputationGrid result(hybrid_grid.resolution(), hybrid_grid.origin());
+  PrecomputationGrid result(hybrid_grid.resolution());
   for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
     const int cell_value = common::RoundToInt(
         (mapping::ValueToProbability(it.GetValue()) -
@@ -64,7 +64,7 @@ PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) {
 PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
                                   const bool half_resolution,
                                   const Eigen::Array3i& shift) {
-  PrecomputationGrid result(grid.resolution(), grid.origin());
+  PrecomputationGrid result(grid.resolution());
   for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) {
     for (int i = 0; i != 8; ++i) {
       // We use this value to update 8 values in the resulting grid, at

cartographer/mapping_3d/scan_matching/precomputation_grid.h

@@ -25,8 +25,8 @@ namespace scan_matching {
 
 class PrecomputationGrid : public HybridGridBase<uint8> {
  public:
-  PrecomputationGrid(const float resolution, const Eigen::Vector3f& origin)
+  explicit PrecomputationGrid(const float resolution)
-      : HybridGridBase<uint8>(resolution, origin) {}
+      : HybridGridBase<uint8>(resolution) {}
 
   // Maps values from [0, 255] to [kMinProbability, kMaxProbability].
   static float ToProbability(float value) {

cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc

@@ -29,7 +29,7 @@ namespace scan_matching {
 namespace {
 
 TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
-  HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f));
+  HybridGrid hybrid_grid(2.f);
 
   std::mt19937 rng(23847);
   std::uniform_int_distribution<int> coordinate_distribution(-50, 49);

cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -36,9 +36,8 @@ namespace {
 class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
  protected:
   RealTimeCorrelativeScanMatcherTest()
-      : hybrid_grid_(0.1f /* resolution */,
+      : hybrid_grid_(0.1f),
-                     Eigen::Vector3f(0.5f, 0.5f, 0.5f) /* origin */),
+        expected_pose_(Eigen::Vector3d(-1., 0., 0.),
-        expected_pose_(Eigen::Vector3d(-0.5, 0.5, 0.5),
                        Eigen::Quaterniond::Identity()) {
     for (const auto& point :
          {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
@@ -82,42 +81,39 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.3, 0.5, 0.5)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.3)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
   TestFromInitialPose(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.4, 0.3, 0.7)));
+      transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
-  TestFromInitialPose(
+  TestFromInitialPose(transform::Rigid3d(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) *
+      Eigen::Vector3d(-1., 0., 0.),
-      transform::Rigid3d::Rotation(
       Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
-  TestFromInitialPose(
+  TestFromInitialPose(transform::Rigid3d(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) *
+      Eigen::Vector3d(-1., 0., 0.),
-      transform::Rigid3d::Rotation(
       Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
-  TestFromInitialPose(
+  TestFromInitialPose(transform::Rigid3d(
-      transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) *
+      Eigen::Vector3d(-1., 0., 0.),
-      transform::Rigid3d::Rotation(
       Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
 }
 

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -61,9 +61,8 @@ void ConstraintBuilder::MaybeAddConstraint(
     const mapping::NodeId& node_id,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
-    const transform::Rigid3d& initial_relative_pose) {
+    const transform::Rigid3d& initial_pose) {
-  if (initial_relative_pose.translation().norm() >
+  if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
-      options_.max_constraint_distance()) {
     return;
   }
   if (sampler_.Pulse()) {
@@ -75,10 +74,10 @@ void ConstraintBuilder::MaybeAddConstraint(
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
         [=]() EXCLUDES(mutex_) {
-          ComputeConstraint(
+          ComputeConstraint(submap_id, submap, node_id,
-              submap_id, submap, node_id, false, /* match_full_submap */
+                            false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
-              compressed_point_cloud, initial_relative_pose, constraint);
+                            compressed_point_cloud, initial_pose, constraint);
           FinishComputation(current_computation);
         });
   }
@@ -171,23 +170,19 @@ void ConstraintBuilder::ComputeConstraint(
     const mapping::NodeId& node_id, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
-    const transform::Rigid3d& initial_relative_pose,
+    const transform::Rigid3d& initial_pose,
     std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
-  const transform::Rigid3d initial_pose =
-      submap->local_pose() * initial_relative_pose;
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);
   const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
   const sensor::PointCloud filtered_point_cloud =
       adaptive_voxel_filter_.Filter(point_cloud);
 
-  // The 'constraint_transform' (i <- j) is computed from:
+  // The 'constraint_transform' (submap 'i' <- scan 'j') is computed from the
-  // - a 'filtered_point_cloud' in j,
+  // initial guess 'initial_pose' for (submap 'i' <- scan 'j') and a
-  // - the initial guess 'initial_pose' for (map <- j),
+  // 'filtered_point_cloud' in 'j'.
-  // - the result 'pose_estimate' of Match() (map <- j).
-  // - the submap->pose() (map <- i)
   float score = 0.;
-  transform::Rigid3d pose_estimate = transform::Rigid3d::Identity();
+  transform::Rigid3d pose_estimate;
 
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
@@ -220,13 +215,12 @@ void ConstraintBuilder::ComputeConstraint(
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
+  transform::Rigid3d constraint_transform;
   ceres_scan_matcher_.Match(
       pose_estimate, pose_estimate,
       {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
-      &pose_estimate, &unused_summary);
+      &constraint_transform, &unused_summary);
 
-  const transform::Rigid3d constraint_transform =
-      submap->local_pose().inverse() * pose_estimate;
   constraint->reset(new OptimizationProblem::Constraint{
       submap_id,
       node_id,
@@ -243,7 +237,7 @@ void ConstraintBuilder::ComputeConstraint(
       info << " matches";
     } else {
       const transform::Rigid3d difference =
-          initial_pose.inverse() * pose_estimate;
+          initial_pose.inverse() * constraint_transform;
       info << " differs by translation " << std::setprecision(2)
            << difference.translation().norm() << " rotation "
            << std::setprecision(3) << transform::GetAngle(difference);

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -68,8 +68,8 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id', and the 'point_cloud' for 'node_id'.
+  // 'submap_id', and the 'point_cloud' for 'node_id'. The 'intial_pose' is
-  // The 'initial_relative_pose' is relative to the 'submap'.
+  // relative to the 'submap'.
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
@@ -78,7 +78,7 @@ class ConstraintBuilder {
       const mapping::NodeId& node_id,
       const sensor::CompressedPointCloud* compressed_point_cloud,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
-      const transform::Rigid3d& initial_relative_pose);
+      const transform::Rigid3d& initial_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
   // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
@@ -141,7 +141,7 @@ class ConstraintBuilder {
       const mapping::NodeId& node_id, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::CompressedPointCloud* compressed_point_cloud,
-      const transform::Rigid3d& initial_relative_pose,
+      const transform::Rigid3d& initial_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 
   // Decrements the 'pending_computations_' count. If all computations are done,

cartographer/mapping_3d/submaps.cc

@@ -219,8 +219,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
 Submap::Submap(const float high_resolution, const float low_resolution,
                const Eigen::Vector3f& origin)
     : mapping::Submap(origin),
-      high_resolution_hybrid_grid(high_resolution, origin),
+      high_resolution_hybrid_grid(high_resolution),
-      low_resolution_hybrid_grid(low_resolution, origin) {}
+      low_resolution_hybrid_grid(low_resolution) {}
 
 Submaps::Submaps(const proto::SubmapsOptions& options)
     : options_(options),
@@ -250,9 +250,7 @@ void Submaps::SubmapToProto(
   Eigen::Array2i min_index(INT_MAX, INT_MAX);
   Eigen::Array2i max_index(INT_MIN, INT_MIN);
   const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
-      ExtractVoxelData(hybrid_grid,
+      ExtractVoxelData(hybrid_grid, global_submap_pose.cast<float>(),
-                       (global_submap_pose * Get(index)->local_pose().inverse())
-                           .cast<float>(),
                        &min_index, &max_index);
 
   const int width = max_index.y() - min_index.y() + 1;
@@ -276,11 +274,13 @@ void Submaps::SubmapToProto(
 void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
+    const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
+        range_data, submap->local_pose().inverse().cast<float>());
     range_data_inserter_.Insert(
-        FilterRangeDataByMaxRange(range_data,
+        FilterRangeDataByMaxRange(transformed_range_data,
                                   options_.high_resolution_max_range()),
         &submap->high_resolution_hybrid_grid);
-    range_data_inserter_.Insert(range_data,
+    range_data_inserter_.Insert(transformed_range_data,
                                 &submap->low_resolution_hybrid_grid);
     ++submap->num_range_data;
   }
@@ -290,14 +290,6 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
   }
 }
 
-const HybridGrid& Submaps::high_resolution_matching_grid() const {
-  return submaps_[matching_index()]->high_resolution_hybrid_grid;
-}
-
-const HybridGrid& Submaps::low_resolution_matching_grid() const {
-  return submaps_[matching_index()]->low_resolution_hybrid_grid;
-}
-
 void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
   if (size() > 1) {
     Submap* submap = submaps_[size() - 2].get();
@@ -350,9 +342,9 @@ std::vector<Eigen::Array4i> Submaps::ExtractVoxelData(
       continue;
     }
 
-    const Eigen::Vector3f cell_center_local =
+    const Eigen::Vector3f cell_center_submap =
         hybrid_grid.GetCenterOfCell(it.GetCellIndex());
-    const Eigen::Vector3f cell_center_global = transform * cell_center_local;
+    const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
     const Eigen::Array4i voxel_index_and_probability(
         common::RoundToInt(cell_center_global.x() * resolution_inverse),
         common::RoundToInt(cell_center_global.y() * resolution_inverse),

cartographer/mapping_3d/submaps.h

@@ -72,12 +72,6 @@ class Submaps : public mapping::Submaps {
   // Inserts 'range_data' into the Submap collection.
   void InsertRangeData(const sensor::RangeData& range_data);
 
-  // Returns the 'high_resolution' HybridGrid to be used for matching.
-  const HybridGrid& high_resolution_matching_grid() const;
-
-  // Returns the 'low_resolution' HybridGrid to be used for matching.
-  const HybridGrid& low_resolution_matching_grid() const;
-
  private:
   struct PixelData {
     int min_z = INT_MAX;

cartographer/sensor/compressed_point_cloud.cc

@@ -104,7 +104,7 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
     int index;
   };
   using Blocks = mapping_3d::HybridGridBase<std::vector<RasterPoint>>;
-  Blocks blocks(kPrecision, Eigen::Vector3f::Zero());
+  Blocks blocks(kPrecision);
   int num_blocks = 0;
   CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());
   for (int point_index = 0; point_index < static_cast<int>(point_cloud.size());

cartographer/sensor/voxel_filter.cc

@@ -83,8 +83,7 @@ PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
   return voxel_filter.point_cloud();
 }
 
-VoxelFilter::VoxelFilter(const float size)
+VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
-    : voxels_(size, Eigen::Vector3f::Zero()) {}
 
 void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
   for (const Eigen::Vector3f& point : point_cloud) {