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{
-        mapping_3d::HybridGridBase<bool>(voxel_size, Eigen::Vector3f::Zero()),
-        {}});
+    aggregations_.emplace_back(
+        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
-  // center of the cell at index (0, 0, 0). Each voxel has edge length
-  // 'resolution'.
-  HybridGridBase(const float resolution, const Eigen::Vector3f& origin)
-      : resolution_(resolution), origin_(origin) {}
+  // Creates a new tree-based probability grid with voxels having edge length
+  // 'resolution' around the origin which becomes the center of the cell at
+  // index (0, 0, 0).
+  explicit HybridGridBase(const float resolution) : resolution_(resolution) {}
 
   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
-  // is rounded to the next multiple of the resolution.
+  // vectors identifying cells, for this the coordinates are rounded to the next
+  // 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)
-      : HybridGridBase<uint16>(resolution, origin) {}
+  explicit HybridGrid(const float resolution)
+      : HybridGridBase<uint16>(resolution) {}
 
-  HybridGrid(const proto::HybridGrid& proto)
-      : HybridGrid(proto.resolution(), transform::ToEigen(proto.origin())) {
+  explicit HybridGrid(const proto::HybridGrid& proto)
+      : 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,62 +74,61 @@ 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(.5f, 0.5, 0.5f)),
-        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(1.f, 1.f, 1.f)),
+        hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) {
     EXPECT_FALSE(hybrid_grid.IsKnown(index));
   }
 }
 
 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(-9.f, center.y(), 1e-6);
-  EXPECT_NEAR(0.f, center.z(), 1e-6);
+  EXPECT_NEAR(6.f, center.x(), 1e-6);
+  EXPECT_NEAR(4.f, center.y(), 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()
-      : hybrid_grid_(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)), values_() {
+  RandomHybridGridTest() : hybrid_grid_(2.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,
-        submaps_->high_resolution_matching_grid(), &initial_ceres_pose);
+        initial_pose, filtered_point_cloud_in_tracking,
+        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()}},
-                             &pose_observation, &summary);
+                               &matching_submap->low_resolution_hybrid_grid}},
+                             &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{
-      {{orientation.w(), orientation.x(), orientation.y(), orientation.z()}},
-      {{position.x(), position.y(), position.z()}},
-      {{velocity.x(), velocity.y(), velocity.z()}}};
+  return State(position, orientation, velocity);
 }
 
 }  // 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];