Purge 'mapping_3d::'. (#927) (#928)

[Code structure RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0016-code-structure.md)

cartographer/internal/mapping/global_trajectory_builder.cc

@@ -139,13 +139,12 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
 }
 
 std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
-    std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
+    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
-        local_trajectory_builder,
     const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
     const TrajectoryBuilderInterface::LocalSlamResultCallback&
         local_slam_result_callback) {
-  return common::make_unique<GlobalTrajectoryBuilder<
+  return common::make_unique<
-      mapping_3d::LocalTrajectoryBuilder, mapping::PoseGraph3D>>(
+      GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
       std::move(local_trajectory_builder), trajectory_id, pose_graph,
       local_slam_result_callback);
 }

cartographer/internal/mapping/global_trajectory_builder.h

@@ -20,7 +20,7 @@
 #include <memory>
 
 #include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
-#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
+#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h"
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer/mapping_2d/pose_graph_2d.h"
@@ -36,8 +36,7 @@ std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
         local_slam_result_callback);
 
 std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
-    std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
+    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
-        local_trajectory_builder,
     const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
     const TrajectoryBuilderInterface::LocalSlamResultCallback&
         local_slam_result_callback);

cartographer/internal/mapping_3d/acceleration_cost_function_3d.h

@@ -14,17 +14,17 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
+#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
+#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 
 // Penalizes differences between IMU data and optimized accelerations.
-class AccelerationCostFunction {
+class AccelerationCostFunction3D {
  public:
   static ceres::CostFunction* CreateAutoDiffCostFunction(
       const double scaling_factor,
@@ -32,12 +32,13 @@ class AccelerationCostFunction {
       const double first_delta_time_seconds,
       const double second_delta_time_seconds) {
     return new ceres::AutoDiffCostFunction<
-        AccelerationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
+        AccelerationCostFunction3D, 3 /* residuals */,
+        4 /* rotation variables */, 3 /* position variables */,
         3 /* position variables */, 3 /* position variables */,
-        3 /* position variables */, 1 /* gravity variables */,
+        1 /* gravity variables */, 4 /* rotation variables */>(
-        4 /* rotation variables */>(new AccelerationCostFunction(
+        new AccelerationCostFunction3D(scaling_factor, delta_velocity_imu_frame,
-        scaling_factor, delta_velocity_imu_frame, first_delta_time_seconds,
+                                       first_delta_time_seconds,
-        second_delta_time_seconds));
+                                       second_delta_time_seconds));
   }
 
   template <typename T>
@@ -71,17 +72,18 @@ class AccelerationCostFunction {
   }
 
  private:
-  AccelerationCostFunction(const double scaling_factor,
+  AccelerationCostFunction3D(const double scaling_factor,
-                           const Eigen::Vector3d& delta_velocity_imu_frame,
+                             const Eigen::Vector3d& delta_velocity_imu_frame,
-                           const double first_delta_time_seconds,
+                             const double first_delta_time_seconds,
-                           const double second_delta_time_seconds)
+                             const double second_delta_time_seconds)
       : scaling_factor_(scaling_factor),
         delta_velocity_imu_frame_(delta_velocity_imu_frame),
         first_delta_time_seconds_(first_delta_time_seconds),
         second_delta_time_seconds_(second_delta_time_seconds) {}
 
-  AccelerationCostFunction(const AccelerationCostFunction&) = delete;
+  AccelerationCostFunction3D(const AccelerationCostFunction3D&) = delete;
-  AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete;
+  AccelerationCostFunction3D& operator=(const AccelerationCostFunction3D&) =
+      delete;
 
   template <typename T>
   static Eigen::Quaternion<T> ToEigen(const T* const quaternion) {
@@ -95,7 +97,7 @@ class AccelerationCostFunction {
   const double second_delta_time_seconds_;
 };
 
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
+#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_ACCELERATION_COST_FUNCTION_3D_H_

cartographer/internal/mapping_3d/local_trajectory_builder_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
+#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h"
 
 #include <memory>
 
@@ -23,28 +23,29 @@
 #include "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
 #include "cartographer/mapping_3d/proto/submaps_options_3d.pb.h"
-#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h"
 #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 
-LocalTrajectoryBuilder::LocalTrajectoryBuilder(
+LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
     const mapping::proto::LocalTrajectoryBuilderOptions3D& options)
     : options_(options),
       active_submaps_(options.submaps_options()),
       motion_filter_(options.motion_filter_options()),
       real_time_correlative_scan_matcher_(
-          common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
+          common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher3D>(
               options_.real_time_correlative_scan_matcher_options())),
-      ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
+      ceres_scan_matcher_(
-          options_.ceres_scan_matcher_options())),
+          common::make_unique<scan_matching::CeresScanMatcher3D>(
+              options_.ceres_scan_matcher_options())),
       accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
 
-LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
+LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {}
 
-void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
+void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
   if (extrapolator_ != nullptr) {
     extrapolator_->AddImuData(imu_data);
     return;
@@ -58,9 +59,9 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
       options_.imu_gravity_time_constant(), imu_data);
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
+std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
-LocalTrajectoryBuilder::AddRangeData(const common::Time time,
+LocalTrajectoryBuilder3D::AddRangeData(
-                                     const sensor::TimedRangeData& range_data) {
+    const common::Time time, const sensor::TimedRangeData& range_data) {
   if (extrapolator_ == nullptr) {
     // Until we've initialized the extrapolator with our first IMU message, we
     // cannot compute the orientation of the rangefinder.
@@ -130,8 +131,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
   return nullptr;
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
+std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
-LocalTrajectoryBuilder::AddAccumulatedRangeData(
+LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     const common::Time time,
     const sensor::RangeData& filtered_range_data_in_tracking) {
   if (filtered_range_data_in_tracking.returns.empty()) {
@@ -199,7 +200,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       std::move(insertion_result)});
 }
 
-void LocalTrajectoryBuilder::AddOdometryData(
+void LocalTrajectoryBuilder3D::AddOdometryData(
     const sensor::OdometryData& odometry_data) {
   if (extrapolator_ == nullptr) {
     // Until we've initialized the extrapolator we cannot add odometry data.
@@ -209,8 +210,8 @@ void LocalTrajectoryBuilder::AddOdometryData(
   extrapolator_->AddOdometryData(odometry_data);
 }
 
-std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+std::unique_ptr<LocalTrajectoryBuilder3D::InsertionResult>
-LocalTrajectoryBuilder::InsertIntoSubmap(
+LocalTrajectoryBuilder3D::InsertIntoSubmap(
     const common::Time time,
     const sensor::RangeData& filtered_range_data_in_local,
     const sensor::RangeData& filtered_range_data_in_tracking,
@@ -249,5 +250,5 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
                       std::move(insertion_submaps)});
 }
 
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/internal/mapping_3d/local_trajectory_builder_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
+#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_
-#define CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
+#define CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_
 
 #include <memory>
 
@@ -23,8 +23,8 @@
 #include "cartographer/internal/mapping/motion_filter.h"
 #include "cartographer/mapping/pose_extrapolator.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.pb.h"
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
-#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
 #include "cartographer/mapping_3d/submap_3d.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/odometry_data.h"
@@ -33,11 +33,11 @@
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 
 // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
 // without loop closure.
-class LocalTrajectoryBuilder {
+class LocalTrajectoryBuilder3D {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
@@ -51,12 +51,12 @@ class LocalTrajectoryBuilder {
     std::unique_ptr<const InsertionResult> insertion_result;
   };
 
-  explicit LocalTrajectoryBuilder(
+  explicit LocalTrajectoryBuilder3D(
       const mapping::proto::LocalTrajectoryBuilderOptions3D& options);
-  ~LocalTrajectoryBuilder();
+  ~LocalTrajectoryBuilder3D();
 
-  LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
+  LocalTrajectoryBuilder3D(const LocalTrajectoryBuilder3D&) = delete;
-  LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
+  LocalTrajectoryBuilder3D& operator=(const LocalTrajectoryBuilder3D&) = delete;
 
   void AddImuData(const sensor::ImuData& imu_data);
   // Returns 'MatchingResult' when range data accumulation completed,
@@ -84,9 +84,9 @@ class LocalTrajectoryBuilder {
   mapping::ActiveSubmaps3D active_submaps_;
 
   mapping::MotionFilter motion_filter_;
-  std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>
+  std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher3D>
       real_time_correlative_scan_matcher_;
-  std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
+  std::unique_ptr<scan_matching::CeresScanMatcher3D> ceres_scan_matcher_;
 
   std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
 
@@ -94,7 +94,7 @@ class LocalTrajectoryBuilder {
   sensor::RangeData accumulated_range_data_;
 };
 
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
+#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_

cartographer/internal/mapping_3d/local_trajectory_builder_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
+#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h"
 
 #include <memory>
 #include <random>
@@ -32,7 +32,7 @@
 #include "gmock/gmock.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace {
 
 class LocalTrajectoryBuilderTest : public ::testing::Test {
@@ -253,7 +253,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
     for (const TrajectoryNode& node : expected_trajectory) {
       AddLinearOnlyImuObservation(node.time, node.pose);
       const auto range_data = GenerateRangeData(node.pose);
-      const std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
+      const std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
           matching_result = local_trajectory_builder_->AddRangeData(
               node.time, sensor::TimedRangeData{
                              range_data.origin, range_data.returns, {}});
@@ -266,16 +266,16 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
     }
   }
 
-  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
+  std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder_;
   std::vector<Eigen::Vector3f> bubbles_;
 };
 
 TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
   local_trajectory_builder_.reset(
-      new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions3D()));
+      new LocalTrajectoryBuilder3D(CreateTrajectoryBuilderOptions3D()));
   VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
 }
 
 }  // namespace
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/internal/mapping_3d/rotation_cost_function_3d.h

@@ -14,14 +14,14 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_
+#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_
+#define CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 
 // Penalizes differences between IMU data and optimized orientations.
 class RotationCostFunction {
@@ -67,7 +67,7 @@ class RotationCostFunction {
   const Eigen::Quaterniond delta_rotation_imu_frame_;
 };
 
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_H_
+#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_ROTATION_COST_FUNCTION_3D_H_

cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
+#ifndef CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
+#define CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "cartographer/mapping_3d/hybrid_grid.h"
@@ -25,21 +25,22 @@
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 // Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
 // 'translation' and 'rotation'. The cost increases when points fall into less
 // occupied space, i.e. at voxels with lower values.
-class OccupiedSpaceCostFunction {
+class OccupiedSpaceCostFunction3D {
  public:
   static ceres::CostFunction* CreateAutoDiffCostFunction(
       const double scaling_factor, const sensor::PointCloud& point_cloud,
       const mapping::HybridGrid& hybrid_grid) {
     return new ceres::AutoDiffCostFunction<
-        OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */,
+        OccupiedSpaceCostFunction3D, ceres::DYNAMIC /* residuals */,
         3 /* translation variables */, 4 /* rotation variables */>(
-        new OccupiedSpaceCostFunction(scaling_factor, point_cloud, hybrid_grid),
+        new OccupiedSpaceCostFunction3D(scaling_factor, point_cloud,
+                                        hybrid_grid),
         point_cloud.size());
   }
 
@@ -54,15 +55,15 @@ class OccupiedSpaceCostFunction {
   }
 
  private:
-  OccupiedSpaceCostFunction(const double scaling_factor,
+  OccupiedSpaceCostFunction3D(const double scaling_factor,
-                            const sensor::PointCloud& point_cloud,
+                              const sensor::PointCloud& point_cloud,
-                            const mapping::HybridGrid& hybrid_grid)
+                              const mapping::HybridGrid& hybrid_grid)
       : scaling_factor_(scaling_factor),
         point_cloud_(point_cloud),
         interpolated_grid_(hybrid_grid) {}
 
-  OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
+  OccupiedSpaceCostFunction3D(const OccupiedSpaceCostFunction3D&) = delete;
-  OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
+  OccupiedSpaceCostFunction3D& operator=(const OccupiedSpaceCostFunction3D&) =
       delete;
 
   template <typename T>
@@ -84,7 +85,7 @@ class OccupiedSpaceCostFunction {
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_H_
+#endif  // CARTOGRAPHER_INTERNAL_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_3D_H_

cartographer/mapping/map_builder.cc

@@ -26,7 +26,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/internal/mapping/global_trajectory_builder.h"
 #include "cartographer/internal/mapping_2d/local_trajectory_builder_2d.h"
-#include "cartographer/internal/mapping_3d/local_trajectory_builder.h"
+#include "cartographer/internal/mapping_3d/local_trajectory_builder_3d.h"
 #include "cartographer/mapping/collated_trajectory_builder.h"
 #include "cartographer/sensor/collator.h"
 #include "cartographer/sensor/range_data.h"
@@ -80,12 +80,10 @@ int MapBuilder::AddTrajectoryBuilder(
     LocalSlamResultCallback local_slam_result_callback) {
   const int trajectory_id = trajectory_builders_.size();
   if (options_.use_trajectory_builder_3d()) {
-    std::unique_ptr<mapping_3d::LocalTrajectoryBuilder>
+    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
-        local_trajectory_builder;
     if (trajectory_options.has_trajectory_builder_3d_options()) {
-      local_trajectory_builder =
+      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(
-          common::make_unique<mapping_3d::LocalTrajectoryBuilder>(
+          trajectory_options.trajectory_builder_3d_options());
-              trajectory_options.trajectory_builder_3d_options());
     }
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(

cartographer/mapping/pose_graph/constraint_builder.cc

@@ -18,8 +18,8 @@
 
 #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher_2d.h"
 #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
-#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
 #include "cartographer/sensor/voxel_filter.h"
 
 namespace cartographer {
@@ -48,12 +48,12 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
       scan_matching::CreateCeresScanMatcherOptions2D(
           parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
   *options.mutable_fast_correlative_scan_matcher_options_3d() =
-      mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
+      scan_matching::CreateFastCorrelativeScanMatcherOptions3D(
           parameter_dictionary
               ->GetDictionary("fast_correlative_scan_matcher_3d")
               .get());
   *options.mutable_ceres_scan_matcher_options_3d() =
-      mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
+      scan_matching::CreateCeresScanMatcherOptions3D(
           parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get());
   return options;
 }

cartographer/mapping/pose_graph/proto/constraint_builder_options.proto

@@ -18,8 +18,8 @@ package cartographer.mapping.pose_graph.proto;
 
 import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto";
 import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto";
-import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
+import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto";
-import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
+import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto";
 
 message ConstraintBuilderOptions {
   // A constraint will be added if the proportion of added constraints to
@@ -52,8 +52,8 @@ message ConstraintBuilderOptions {
       fast_correlative_scan_matcher_options = 9;
   mapping.scan_matching.proto.CeresScanMatcherOptions2D
       ceres_scan_matcher_options = 11;
-  mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
+  mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions3D
       fast_correlative_scan_matcher_options_3d = 10;
-  mapping_3d.scan_matching.proto.CeresScanMatcherOptions
+  mapping.scan_matching.proto.CeresScanMatcherOptions3D
       ceres_scan_matcher_options_3d = 12;
 }

cartographer/mapping/proto/submap.proto

@@ -28,7 +28,7 @@ message Submap2D {
   ProbabilityGrid probability_grid = 4;
 }
 
-// Serialized state of a mapping_3d::Submap.
+// Serialized state of a Submap3D.
 message Submap3D {
   transform.proto.Rigid3d local_pose = 1;
   int32 num_range_data = 2;

cartographer/mapping_2d/pose_graph/landmark_cost_function_2d.h

@@ -55,10 +55,6 @@ class LandmarkCostFunction2D {
   bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
                   const T* const landmark_rotation,
                   const T* const landmark_translation, T* const e) const {
-    using pose_graph::ComputeUnscaledError;
-    using pose_graph::ScaleError;
-    using pose_graph::SlerpQuaternions;
-
     const std::array<T, 3> interpolated_pose_translation{
         {prev_node_pose[0] +
              interpolation_parameter_ * (next_node_pose[0] - prev_node_pose[0]),

cartographer/mapping_2d/pose_graph_2d_test.cc

@@ -40,7 +40,7 @@ class PoseGraph2DTest : public ::testing::Test {
   PoseGraph2DTest() : thread_pool_(1) {
     // Builds a wavy, irregularly circular point cloud that is unique
     // rotationally. This gives us good rotational texture and avoids any
-    // possibility of the CeresScanMatcher preferring free space (>
+    // possibility of the CeresScanMatcher2D preferring free space (>
     // kMinProbability) to unknown space (== kMinProbability).
     for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
       const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);

cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.cc

@@ -70,7 +70,7 @@ SearchParameters::SearchParameters(const int num_linear_perturbations,
   }
 }
 
-void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
+void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                                    const CellLimits& cell_limits) {
   CHECK_EQ(scans.size(), num_scans);
   CHECK_EQ(linear_bounds.size(), num_scans);
@@ -108,10 +108,10 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
   return rotated_scans;
 }
 
-std::vector<DiscreteScan> DiscretizeScans(
+std::vector<DiscreteScan2D> DiscretizeScans(
     const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
     const Eigen::Translation2f& initial_translation) {
-  std::vector<DiscreteScan> discrete_scans;
+  std::vector<DiscreteScan2D> discrete_scans;
   discrete_scans.reserve(scans.size());
   for (const sensor::PointCloud& scan : scans) {
     discrete_scans.emplace_back();

cartographer/mapping_2d/scan_matching/correlative_scan_matcher_2d.h

@@ -29,7 +29,7 @@ namespace cartographer {
 namespace mapping {
 namespace scan_matching {
 
-typedef std::vector<Eigen::Array2i> DiscreteScan;
+typedef std::vector<Eigen::Array2i> DiscreteScan2D;
 
 // Describes the search space.
 struct SearchParameters {
@@ -49,7 +49,7 @@ struct SearchParameters {
                    double angular_perturbation_step_size, double resolution);
 
   // Tightens the search window as much as possible.
-  void ShrinkToFit(const std::vector<DiscreteScan>& scans,
+  void ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                    const CellLimits& cell_limits);
 
   int num_angular_perturbations;
@@ -66,15 +66,15 @@ std::vector<sensor::PointCloud> GenerateRotatedScans(
 
 // Translates and discretizes the rotated scans into a vector of integer
 // indices.
-std::vector<DiscreteScan> DiscretizeScans(
+std::vector<DiscreteScan2D> DiscretizeScans(
     const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
     const Eigen::Translation2f& initial_translation);
 
 // A possible solution.
-struct Candidate {
+struct Candidate2D {
-  Candidate(const int init_scan_index, const int init_x_index_offset,
+  Candidate2D(const int init_scan_index, const int init_x_index_offset,
-            const int init_y_index_offset,
+              const int init_y_index_offset,
-            const SearchParameters& search_parameters)
+              const SearchParameters& search_parameters)
       : scan_index(init_scan_index),
         x_index_offset(init_x_index_offset),
         y_index_offset(init_y_index_offset),
@@ -90,7 +90,7 @@ struct Candidate {
   int x_index_offset = 0;
   int y_index_offset = 0;
 
-  // Pose of this Candidate relative to the initial pose.
+  // Pose of this Candidate2D relative to the initial pose.
   double x = 0.;
   double y = 0.;
   double orientation = 0.;
@@ -98,8 +98,8 @@ struct Candidate {
   // Score, higher is better.
   float score = 0.f;
 
-  bool operator<(const Candidate& other) const { return score < other.score; }
+  bool operator<(const Candidate2D& other) const { return score < other.score; }
-  bool operator>(const Candidate& other) const { return score > other.score; }
+  bool operator>(const Candidate2D& other) const { return score > other.score; }
 };
 
 }  // namespace scan_matching

cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc

@@ -42,7 +42,7 @@ TEST(SearchParameters, Construction) {
 
 TEST(Candidate, Construction) {
   const SearchParameters search_parameters(4, 5, 0.03, 0.05);
-  const Candidate candidate(3, 4, -5, search_parameters);
+  const Candidate2D candidate(3, 4, -5, search_parameters);
   EXPECT_EQ(3, candidate.scan_index);
   EXPECT_EQ(4, candidate.x_index_offset);
   EXPECT_EQ(-5, candidate.y_index_offset);
@@ -51,7 +51,7 @@ TEST(Candidate, Construction) {
   EXPECT_NEAR(-0.06, candidate.orientation, 1e-9);
   EXPECT_NEAR(0., candidate.score, 1e-9);
 
-  Candidate bigger_candidate(3, 4, 5, search_parameters);
+  Candidate2D bigger_candidate(3, 4, 5, search_parameters);
   bigger_candidate.score = 1.;
   EXPECT_LT(candidate, bigger_candidate);
 }
@@ -83,7 +83,7 @@ TEST(DiscretizeScans, DiscretizeScans) {
                              CellLimits(6, 6));
   const std::vector<sensor::PointCloud> scans =
       GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
-  const std::vector<DiscreteScan> discrete_scans =
+  const std::vector<DiscreteScan2D> discrete_scans =
       DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity());
   EXPECT_EQ(1, discrete_scans.size());
   EXPECT_EQ(7, discrete_scans[0].size());

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.cc

@@ -87,7 +87,7 @@ CreateFastCorrelativeScanMatcherOptions2D(
   return options;
 }
 
-PrecomputationGrid::PrecomputationGrid(
+PrecomputationGrid2D::PrecomputationGrid2D(
     const ProbabilityGrid& probability_grid, const CellLimits& limits,
     const int width, std::vector<float>* reusable_intermediate_grid)
     : offset_(-width + 1, -width + 1),
@@ -157,7 +157,7 @@ PrecomputationGrid::PrecomputationGrid(
   }
 }
 
-uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
+uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
   const int cell_value =
       common::RoundToInt((probability - kMinProbability) *
                          (255.f / (kMaxProbability - kMinProbability)));
@@ -185,14 +185,14 @@ class PrecomputationGridStack {
     }
   }
 
-  const PrecomputationGrid& Get(int index) {
+  const PrecomputationGrid2D& Get(int index) {
     return precomputation_grids_[index];
   }
 
   int max_depth() const { return precomputation_grids_.size() - 1; }
 
  private:
-  std::vector<PrecomputationGrid> precomputation_grids_;
+  std::vector<PrecomputationGrid2D> precomputation_grids_;
 };
 
 FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
@@ -249,15 +249,15 @@ bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
           initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
   const std::vector<sensor::PointCloud> rotated_scans =
       GenerateRotatedScans(rotated_point_cloud, search_parameters);
-  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
+  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
       limits_, rotated_scans,
       Eigen::Translation2f(initial_pose_estimate.translation().x(),
                            initial_pose_estimate.translation().y()));
   search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
 
-  const std::vector<Candidate> lowest_resolution_candidates =
+  const std::vector<Candidate2D> lowest_resolution_candidates =
       ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
-  const Candidate best_candidate = BranchAndBound(
+  const Candidate2D best_candidate = BranchAndBound(
       discrete_scans, search_parameters, lowest_resolution_candidates,
       precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
@@ -271,11 +271,11 @@ bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
   return false;
 }
 
-std::vector<Candidate>
+std::vector<Candidate2D>
 FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
-    const std::vector<DiscreteScan>& discrete_scans,
+    const std::vector<DiscreteScan2D>& discrete_scans,
     const SearchParameters& search_parameters) const {
-  std::vector<Candidate> lowest_resolution_candidates =
+  std::vector<Candidate2D> lowest_resolution_candidates =
       GenerateLowestResolutionCandidates(search_parameters);
   ScoreCandidates(
       precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
@@ -283,7 +283,7 @@ FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
   return lowest_resolution_candidates;
 }
 
-std::vector<Candidate>
+std::vector<Candidate2D>
 FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
     const SearchParameters& search_parameters) const {
   const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
@@ -301,7 +301,7 @@ FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
     num_candidates += num_lowest_resolution_linear_x_candidates *
                       num_lowest_resolution_linear_y_candidates;
   }
-  std::vector<Candidate> candidates;
+  std::vector<Candidate2D> candidates;
   candidates.reserve(num_candidates);
   for (int scan_index = 0; scan_index != search_parameters.num_scans;
        ++scan_index) {
@@ -322,11 +322,11 @@ FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
 }
 
 void FastCorrelativeScanMatcher2D::ScoreCandidates(
-    const PrecomputationGrid& precomputation_grid,
+    const PrecomputationGrid2D& precomputation_grid,
-    const std::vector<DiscreteScan>& discrete_scans,
+    const std::vector<DiscreteScan2D>& discrete_scans,
     const SearchParameters& search_parameters,
-    std::vector<Candidate>* const candidates) const {
+    std::vector<Candidate2D>* const candidates) const {
-  for (Candidate& candidate : *candidates) {
+  for (Candidate2D& candidate : *candidates) {
     int sum = 0;
     for (const Eigen::Array2i& xy_index :
          discrete_scans[candidate.scan_index]) {
@@ -335,29 +335,30 @@ void FastCorrelativeScanMatcher2D::ScoreCandidates(
           xy_index.y() + candidate.y_index_offset);
       sum += precomputation_grid.GetValue(proposed_xy_index);
     }
-    candidate.score = PrecomputationGrid::ToProbability(
+    candidate.score = PrecomputationGrid2D::ToProbability(
         sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
   }
-  std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
+  std::sort(candidates->begin(), candidates->end(),
+            std::greater<Candidate2D>());
 }
 
-Candidate FastCorrelativeScanMatcher2D::BranchAndBound(
+Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
-    const std::vector<DiscreteScan>& discrete_scans,
+    const std::vector<DiscreteScan2D>& discrete_scans,
     const SearchParameters& search_parameters,
-    const std::vector<Candidate>& candidates, const int candidate_depth,
+    const std::vector<Candidate2D>& candidates, const int candidate_depth,
     float min_score) const {
   if (candidate_depth == 0) {
     // Return the best candidate.
     return *candidates.begin();
   }
 
-  Candidate best_high_resolution_candidate(0, 0, 0, search_parameters);
+  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
   best_high_resolution_candidate.score = min_score;
-  for (const Candidate& candidate : candidates) {
+  for (const Candidate2D& candidate : candidates) {
     if (candidate.score <= min_score) {
       break;
     }
-    std::vector<Candidate> higher_resolution_candidates;
+    std::vector<Candidate2D> higher_resolution_candidates;
     const int half_width = 1 << (candidate_depth - 1);
     for (int x_offset : {0, half_width}) {
       if (candidate.x_index_offset + x_offset >

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h

@@ -47,11 +47,11 @@ CreateFastCorrelativeScanMatcherOptions2D(
 // A precomputed grid that contains in each cell (x0, y0) the maximum
 // probability in the width x width area defined by x0 <= x < x0 + width and
 // y0 <= y < y0.
-class PrecomputationGrid {
+class PrecomputationGrid2D {
  public:
-  PrecomputationGrid(const ProbabilityGrid& probability_grid,
+  PrecomputationGrid2D(const ProbabilityGrid& probability_grid,
-                     const CellLimits& limits, int width,
+                       const CellLimits& limits, int width,
-                     std::vector<float>* reusable_intermediate_grid);
+                       std::vector<float>* reusable_intermediate_grid);
 
   // Returns a value between 0 and 255 to represent probabilities between
   // kMinProbability and kMaxProbability.
@@ -130,19 +130,19 @@ class FastCorrelativeScanMatcher2D {
       const transform::Rigid2d& initial_pose_estimate,
       const sensor::PointCloud& point_cloud, float min_score, float* score,
       transform::Rigid2d* pose_estimate) const;
-  std::vector<Candidate> ComputeLowestResolutionCandidates(
+  std::vector<Candidate2D> ComputeLowestResolutionCandidates(
-      const std::vector<DiscreteScan>& discrete_scans,
+      const std::vector<DiscreteScan2D>& discrete_scans,
       const SearchParameters& search_parameters) const;
-  std::vector<Candidate> GenerateLowestResolutionCandidates(
+  std::vector<Candidate2D> GenerateLowestResolutionCandidates(
       const SearchParameters& search_parameters) const;
-  void ScoreCandidates(const PrecomputationGrid& precomputation_grid,
+  void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid,
-                       const std::vector<DiscreteScan>& discrete_scans,
+                       const std::vector<DiscreteScan2D>& discrete_scans,
                        const SearchParameters& search_parameters,
-                       std::vector<Candidate>* const candidates) const;
+                       std::vector<Candidate2D>* const candidates) const;
-  Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
+  Candidate2D BranchAndBound(const std::vector<DiscreteScan2D>& discrete_scans,
-                           const SearchParameters& search_parameters,
+                             const SearchParameters& search_parameters,
-                           const std::vector<Candidate>& candidates,
+                             const std::vector<Candidate2D>& candidates,
-                           int candidate_depth, float min_score) const;
+                             int candidate_depth, float min_score) const;
 
   const proto::FastCorrelativeScanMatcherOptions2D options_;
   MapLimits limits_;

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc

@@ -44,12 +44,12 @@ TEST(PrecomputationGridTest, CorrectValues) {
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
     probability_grid.SetProbability(
-        xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
+        xy_index, PrecomputationGrid2D::ToProbability(distribution(prng)));
   }
 
   std::vector<float> reusable_intermediate_grid;
   for (const int width : {1, 2, 3, 8}) {
-    PrecomputationGrid precomputation_grid(
+    PrecomputationGrid2D precomputation_grid(
         probability_grid, probability_grid.limits().cell_limits(), width,
         &reusable_intermediate_grid);
     for (const Eigen::Array2i& xy_index :
@@ -63,7 +63,7 @@ TEST(PrecomputationGridTest, CorrectValues) {
         }
       }
       EXPECT_NEAR(max_score,
-                  PrecomputationGrid::ToProbability(
+                  PrecomputationGrid2D::ToProbability(
                       precomputation_grid.GetValue(xy_index)),
                   1e-4);
     }
@@ -78,12 +78,12 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
     probability_grid.SetProbability(
-        xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
+        xy_index, PrecomputationGrid2D::ToProbability(distribution(prng)));
   }
 
   std::vector<float> reusable_intermediate_grid;
   for (const int width : {1, 2, 3, 8, 200}) {
-    PrecomputationGrid precomputation_grid(
+    PrecomputationGrid2D precomputation_grid(
         probability_grid, probability_grid.limits().cell_limits(), width,
         &reusable_intermediate_grid);
     for (const Eigen::Array2i& xy_index :
@@ -97,7 +97,7 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
         }
       }
       EXPECT_NEAR(max_score,
-                  PrecomputationGrid::ToProbability(
+                  PrecomputationGrid2D::ToProbability(
                       precomputation_grid.GetValue(xy_index)),
                   1e-4);
     }

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.cc

@@ -37,7 +37,7 @@ RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D(
     const proto::RealTimeCorrelativeScanMatcherOptions& options)
     : options_(options) {}
 
-std::vector<Candidate>
+std::vector<Candidate2D>
 RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
     const SearchParameters& search_parameters) const {
   int num_candidates = 0;
@@ -51,7 +51,7 @@ RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
          search_parameters.linear_bounds[scan_index].min_y + 1);
     num_candidates += num_linear_x_candidates * num_linear_y_candidates;
   }
-  std::vector<Candidate> candidates;
+  std::vector<Candidate2D> candidates;
   candidates.reserve(num_candidates);
   for (int scan_index = 0; scan_index != search_parameters.num_scans;
        ++scan_index) {
@@ -89,16 +89,16 @@ double RealTimeCorrelativeScanMatcher2D::Match(
 
   const std::vector<sensor::PointCloud> rotated_scans =
       GenerateRotatedScans(rotated_point_cloud, search_parameters);
-  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
+  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
       probability_grid.limits(), rotated_scans,
       Eigen::Translation2f(initial_pose_estimate.translation().x(),
                            initial_pose_estimate.translation().y()));
-  std::vector<Candidate> candidates =
+  std::vector<Candidate2D> candidates =
       GenerateExhaustiveSearchCandidates(search_parameters);
   ScoreCandidates(probability_grid, discrete_scans, search_parameters,
                   &candidates);
 
-  const Candidate& best_candidate =
+  const Candidate2D& best_candidate =
       *std::max_element(candidates.begin(), candidates.end());
   *pose_estimate = transform::Rigid2d(
       {initial_pose_estimate.translation().x() + best_candidate.x,
@@ -109,10 +109,10 @@ double RealTimeCorrelativeScanMatcher2D::Match(
 
 void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
     const ProbabilityGrid& probability_grid,
-    const std::vector<DiscreteScan>& discrete_scans,
+    const std::vector<DiscreteScan2D>& discrete_scans,
     const SearchParameters& search_parameters,
-    std::vector<Candidate>* const candidates) const {
+    std::vector<Candidate2D>* const candidates) const {
-  for (Candidate& candidate : *candidates) {
+  for (Candidate2D& candidate : *candidates) {
     candidate.score = 0.f;
     for (const Eigen::Array2i& xy_index :
          discrete_scans[candidate.scan_index]) {

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d.h

@@ -69,18 +69,18 @@ class RealTimeCorrelativeScanMatcher2D {
                const ProbabilityGrid& probability_grid,
                transform::Rigid2d* pose_estimate) const;
 
-  // Computes the score for each Candidate in a collection. The cost is computed
+  // Computes the score for each Candidate2D in a collection. The cost is
-  // as the sum of probabilities, different from the Ceres CostFunctions:
+  // computed as the sum of probabilities, different from the Ceres
-  // http://ceres-solver.org/modeling.html
+  // CostFunctions: http://ceres-solver.org/modeling.html
   //
   // Visible for testing.
   void ScoreCandidates(const ProbabilityGrid& probability_grid,
-                       const std::vector<DiscreteScan>& discrete_scans,
+                       const std::vector<DiscreteScan2D>& discrete_scans,
                        const SearchParameters& search_parameters,
-                       std::vector<Candidate>* candidates) const;
+                       std::vector<Candidate2D>* candidates) const;
 
  private:
-  std::vector<Candidate> GenerateExhaustiveSearchCandidates(
+  std::vector<Candidate2D> GenerateExhaustiveSearchCandidates(
       const SearchParameters& search_parameters) const;
 
   const proto::RealTimeCorrelativeScanMatcherOptions options_;

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc

@@ -85,9 +85,9 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
        ScorePerfectHighResolutionCandidate) {
   const std::vector<sensor::PointCloud> scans =
       GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
-  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
+  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
       probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
-  std::vector<Candidate> candidates;
+  std::vector<Candidate2D> candidates;
   candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
   real_time_correlative_scan_matcher_->ScoreCandidates(
       probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
@@ -103,9 +103,9 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
        ScorePartiallyCorrectHighResolutionCandidate) {
   const std::vector<sensor::PointCloud> scans =
       GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
-  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
+  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
       probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
-  std::vector<Candidate> candidates;
+  std::vector<Candidate2D> candidates;
   candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
   real_time_correlative_scan_matcher_->ScoreCandidates(
       probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),

cartographer/mapping_3d/imu_integration.h

@@ -28,7 +28,7 @@
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 
 template <typename T>
 struct IntegrateImuResult {
@@ -92,7 +92,7 @@ IntegrateImuResult<double> IntegrateImu(const RangeType& imu_data,
       start_time, end_time, it);
 }
 
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
 #endif  // CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_

cartographer/mapping_3d/local_trajectory_builder_options_3d.cc

@@ -18,7 +18,7 @@
 
 #include "cartographer/internal/mapping/motion_filter.h"
 #include "cartographer/mapping/scan_matching/real_time_correlative_scan_matcher.h"
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
 #include "cartographer/mapping_3d/submap_3d.h"
 #include "cartographer/sensor/voxel_filter.h"
 #include "glog/logging.h"
@@ -53,7 +53,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
               ->GetDictionary("real_time_correlative_scan_matcher")
               .get());
   *options.mutable_ceres_scan_matcher_options() =
-      mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
+      mapping::scan_matching::CreateCeresScanMatcherOptions3D(
           parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
   *options.mutable_motion_filter_options() = CreateMotionFilterOptions(
       parameter_dictionary->GetDictionary("motion_filter").get());

cartographer/mapping_3d/pose_graph/constraint_builder_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
+#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h"
 
 #include <cmath>
 #include <functional>
@@ -29,8 +29,8 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/thread_pool.h"
-#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h"
-#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
 #include "cartographer/metrics/counter.h"
 #include "cartographer/metrics/gauge.h"
 #include "cartographer/metrics/histogram.h"
@@ -38,7 +38,7 @@
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 
 static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
@@ -55,15 +55,15 @@ static auto* kGlobalConstraintRotationalScoresMetric =
 static auto* kGlobalConstraintLowResolutionScoresMetric =
     metrics::Histogram::Null();
 
-ConstraintBuilder::ConstraintBuilder(
+ConstraintBuilder3D::ConstraintBuilder3D(
-    const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
+    const proto::ConstraintBuilderOptions& options,
     common::ThreadPool* const thread_pool)
     : options_(options),
       thread_pool_(thread_pool),
       sampler_(options.sampling_ratio()),
       ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {}
 
-ConstraintBuilder::~ConstraintBuilder() {
+ConstraintBuilder3D::~ConstraintBuilder3D() {
   common::MutexLocker locker(&mutex_);
   CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
   CHECK_EQ(pending_computations_.size(), 0);
@@ -71,11 +71,10 @@ ConstraintBuilder::~ConstraintBuilder() {
   CHECK(when_done_ == nullptr);
 }
 
-void ConstraintBuilder::MaybeAddConstraint(
+void ConstraintBuilder3D::MaybeAddConstraint(
-    const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap,
+    const SubmapId& submap_id, const Submap3D* const submap,
-    const mapping::NodeId& node_id,
+    const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
-    const mapping::TrajectoryNode::Data* const constant_data,
+    const std::vector<TrajectoryNode>& submap_nodes,
-    const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const transform::Rigid3d& global_node_pose,
     const transform::Rigid3d& global_submap_pose) {
   if ((global_node_pose.translation() - global_submap_pose.translation())
@@ -99,11 +98,10 @@ void ConstraintBuilder::MaybeAddConstraint(
   }
 }
 
-void ConstraintBuilder::MaybeAddGlobalConstraint(
+void ConstraintBuilder3D::MaybeAddGlobalConstraint(
-    const mapping::SubmapId& submap_id, const mapping::Submap3D* const submap,
+    const SubmapId& submap_id, const Submap3D* const submap,
-    const mapping::NodeId& node_id,
+    const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
-    const mapping::TrajectoryNode::Data* const constant_data,
+    const std::vector<TrajectoryNode>& submap_nodes,
-    const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const Eigen::Quaterniond& global_node_rotation,
     const Eigen::Quaterniond& global_submap_rotation) {
   common::MutexLocker locker(&mutex_);
@@ -122,13 +120,13 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
       });
 }
 
-void ConstraintBuilder::NotifyEndOfNode() {
+void ConstraintBuilder3D::NotifyEndOfNode() {
   common::MutexLocker locker(&mutex_);
   ++current_computation_;
 }
 
-void ConstraintBuilder::WhenDone(
+void ConstraintBuilder3D::WhenDone(
-    const std::function<void(const ConstraintBuilder::Result&)>& callback) {
+    const std::function<void(const ConstraintBuilder3D::Result&)>& callback) {
   common::MutexLocker locker(&mutex_);
   CHECK(when_done_ == nullptr);
   when_done_ =
@@ -139,11 +137,9 @@ void ConstraintBuilder::WhenDone(
       [this, current_computation] { FinishComputation(current_computation); });
 }
 
-void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
+void ConstraintBuilder3D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-    const mapping::SubmapId& submap_id,
+    const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes,
-    const std::vector<mapping::TrajectoryNode>& submap_nodes,
+    const Submap3D* const submap, const std::function<void()>& work_item) {
-    const mapping::Submap3D* const submap,
-    const std::function<void()>& work_item) {
   if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
       nullptr) {
     thread_pool_->Schedule(work_item);
@@ -157,12 +153,11 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
   }
 }
 
-void ConstraintBuilder::ConstructSubmapScanMatcher(
+void ConstraintBuilder3D::ConstructSubmapScanMatcher(
-    const mapping::SubmapId& submap_id,
+    const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes,
-    const std::vector<mapping::TrajectoryNode>& submap_nodes,
+    const Submap3D* const submap) {
-    const mapping::Submap3D* const submap) {
   auto submap_scan_matcher =
-      common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
+      common::make_unique<scan_matching::FastCorrelativeScanMatcher3D>(
           submap->high_resolution_hybrid_grid(),
           &submap->low_resolution_hybrid_grid(), submap_nodes,
           options_.fast_correlative_scan_matcher_options_3d());
@@ -177,8 +172,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
   submap_queued_work_items_.erase(submap_id);
 }
 
-const ConstraintBuilder::SubmapScanMatcher*
+const ConstraintBuilder3D::SubmapScanMatcher*
-ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
+ConstraintBuilder3D::GetSubmapScanMatcher(const SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
   const SubmapScanMatcher* submap_scan_matcher =
       &submap_scan_matchers_[submap_id];
@@ -186,10 +181,9 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
   return submap_scan_matcher;
 }
 
-void ConstraintBuilder::ComputeConstraint(
+void ConstraintBuilder3D::ComputeConstraint(
-    const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
+    const SubmapId& submap_id, const NodeId& node_id, bool match_full_submap,
-    bool match_full_submap,
+    const TrajectoryNode::Data* const constant_data,
-    const mapping::TrajectoryNode::Data* const constant_data,
     const transform::Rigid3d& global_node_pose,
     const transform::Rigid3d& global_submap_pose,
     std::unique_ptr<Constraint>* constraint) {
@@ -199,7 +193,7 @@ void ConstraintBuilder::ComputeConstraint(
   // The 'constraint_transform' (submap i <- node j) is computed from:
   // - a 'high_resolution_point_cloud' in node j and
   // - the initial guess 'initial_pose' (submap i <- node j).
-  std::unique_ptr<scan_matching::FastCorrelativeScanMatcher::Result>
+  std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D::Result>
       match_result;
 
   // Compute 'pose_estimate' in three stages:
@@ -293,7 +287,7 @@ void ConstraintBuilder::ComputeConstraint(
   }
 }
 
-void ConstraintBuilder::FinishComputation(const int computation_index) {
+void ConstraintBuilder3D::FinishComputation(const int computation_index) {
   Result result;
   std::unique_ptr<std::function<void(const Result&)>> callback;
   {
@@ -330,7 +324,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
   }
 }
 
-int ConstraintBuilder::GetNumFinishedNodes() {
+int ConstraintBuilder3D::GetNumFinishedNodes() {
   common::MutexLocker locker(&mutex_);
   if (pending_computations_.empty()) {
     return current_computation_;
@@ -338,13 +332,13 @@ int ConstraintBuilder::GetNumFinishedNodes() {
   return pending_computations_.begin()->first;
 }
 
-void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
+void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
   CHECK(pending_computations_.empty());
   submap_scan_matchers_.erase(submap_id);
 }
 
-void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
+void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) {
   auto* counts = factory->NewCounterFamily(
       "/mapping_3d/pose_graph/constraint_builder/constraints",
       "Constraints computed");
@@ -378,5 +372,5 @@ void ConstraintBuilder::RegisterMetrics(metrics::FamilyFactory* factory) {
 }
 
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/pose_graph/constraint_builder_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
 
 #include <array>
 #include <deque>
@@ -34,8 +34,8 @@
 #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/mapping/trajectory_node.h"
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
-#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
 #include "cartographer/mapping_3d/submap_3d.h"
 #include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/compressed_point_cloud.h"
@@ -43,7 +43,7 @@
 #include "cartographer/sensor/voxel_filter.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 
 // Asynchronously computes constraints.
@@ -54,18 +54,17 @@ namespace pose_graph {
 // MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
 //
 // This class is thread-safe.
-class ConstraintBuilder {
+class ConstraintBuilder3D {
  public:
   using Constraint = mapping::PoseGraphInterface::Constraint;
   using Result = std::vector<Constraint>;
 
-  ConstraintBuilder(
+  ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options,
-      const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
+                      common::ThreadPool* thread_pool);
-      common::ThreadPool* thread_pool);
+  ~ConstraintBuilder3D();
-  ~ConstraintBuilder();
 
-  ConstraintBuilder(const ConstraintBuilder&) = delete;
+  ConstraintBuilder3D(const ConstraintBuilder3D&) = delete;
-  ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
+  ConstraintBuilder3D& operator=(const ConstraintBuilder3D&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
   // 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
@@ -75,13 +74,12 @@ class ConstraintBuilder {
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
-  void MaybeAddConstraint(
+  void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap,
-      const mapping::SubmapId& submap_id, const mapping::Submap3D* submap,
+                          const NodeId& node_id,
-      const mapping::NodeId& node_id,
+                          const TrajectoryNode::Data* const constant_data,
-      const mapping::TrajectoryNode::Data* const constant_data,
+                          const std::vector<TrajectoryNode>& submap_nodes,
-      const std::vector<mapping::TrajectoryNode>& submap_nodes,
+                          const transform::Rigid3d& global_node_pose,
-      const transform::Rigid3d& global_node_pose,
+                          const transform::Rigid3d& global_submap_pose);
-      const transform::Rigid3d& global_submap_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
   // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
@@ -93,10 +91,9 @@ class ConstraintBuilder {
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
   void MaybeAddGlobalConstraint(
-      const mapping::SubmapId& submap_id, const mapping::Submap3D* submap,
+      const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id,
-      const mapping::NodeId& node_id,
+      const TrajectoryNode::Data* const constant_data,
-      const mapping::TrajectoryNode::Data* const constant_data,
+      const std::vector<TrajectoryNode>& submap_nodes,
-      const std::vector<mapping::TrajectoryNode>& submap_nodes,
       const Eigen::Quaterniond& global_node_rotation,
       const Eigen::Quaterniond& global_submap_rotation);
 
@@ -111,52 +108,51 @@ class ConstraintBuilder {
   int GetNumFinishedNodes();
 
   // Delete data related to 'submap_id'.
-  void DeleteScanMatcher(const mapping::SubmapId& submap_id);
+  void DeleteScanMatcher(const SubmapId& submap_id);
 
   static void RegisterMetrics(metrics::FamilyFactory* family_factory);
 
  private:
   struct SubmapScanMatcher {
-    const mapping::HybridGrid* high_resolution_hybrid_grid;
+    const HybridGrid* high_resolution_hybrid_grid;
-    const mapping::HybridGrid* low_resolution_hybrid_grid;
+    const HybridGrid* low_resolution_hybrid_grid;
-    std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
+    std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D>
         fast_correlative_scan_matcher;
   };
 
   // Either schedules the 'work_item', or if needed, schedules the scan matcher
   // construction and queues the 'work_item'.
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      const mapping::SubmapId& submap_id,
+      const SubmapId& submap_id,
-      const std::vector<mapping::TrajectoryNode>& submap_nodes,
+      const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap,
-      const mapping::Submap3D* submap, const std::function<void()>& work_item)
+      const std::function<void()>& work_item) REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
   void ConstructSubmapScanMatcher(
-      const mapping::SubmapId& submap_id,
+      const SubmapId& submap_id,
-      const std::vector<mapping::TrajectoryNode>& submap_nodes,
+      const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
-      const mapping::Submap3D* submap) EXCLUDES(mutex_);
+      EXCLUDES(mutex_);
 
   // Returns the scan matcher for a submap, which has to exist.
-  const SubmapScanMatcher* GetSubmapScanMatcher(
+  const SubmapScanMatcher* GetSubmapScanMatcher(const SubmapId& submap_id)
-      const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
+      EXCLUDES(mutex_);
 
   // Runs in a background thread and does computations for an additional
   // constraint.
   // As output, it may create a new Constraint in 'constraint'.
-  void ComputeConstraint(
+  void ComputeConstraint(const SubmapId& submap_id, const NodeId& node_id,
-      const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
+                         bool match_full_submap,
-      bool match_full_submap,
+                         const TrajectoryNode::Data* const constant_data,
-      const mapping::TrajectoryNode::Data* const constant_data,
+                         const transform::Rigid3d& global_node_pose,
-      const transform::Rigid3d& global_node_pose,
+                         const transform::Rigid3d& global_submap_pose,
-      const transform::Rigid3d& global_submap_pose,
+                         std::unique_ptr<Constraint>* constraint)
-      std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
+      EXCLUDES(mutex_);
 
   // Decrements the 'pending_computations_' count. If all computations are done,
   // runs the 'when_done_' callback and resets the state.
   void FinishComputation(int computation_index) EXCLUDES(mutex_);
 
-  const mapping::pose_graph::proto::ConstraintBuilderOptions options_;
+  const proto::ConstraintBuilderOptions options_;
   common::ThreadPool* thread_pool_;
   common::Mutex mutex_;
 
@@ -178,16 +174,16 @@ class ConstraintBuilder {
   std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
 
   // Map of already constructed scan matchers by 'submap_id'.
-  std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
+  std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
       GUARDED_BY(mutex_);
 
   // Map by 'submap_id' of scan matchers under construction, and the work
   // to do once construction is done.
-  std::map<mapping::SubmapId, std::vector<std::function<void()>>>
+  std::map<SubmapId, std::vector<std::function<void()>>>
       submap_queued_work_items_ GUARDED_BY(mutex_);
 
   common::FixedRatioSampler sampler_;
-  scan_matching::CeresScanMatcher ceres_scan_matcher_;
+  scan_matching::CeresScanMatcher3D ceres_scan_matcher_;
 
   // Histograms of scan matcher scores.
   common::Histogram score_histogram_ GUARDED_BY(mutex_);
@@ -196,7 +192,7 @@ class ConstraintBuilder {
 };
 
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_

cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h

@@ -14,42 +14,43 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
+#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/mapping/pose_graph/cost_helpers.h"
 #include "cartographer/mapping/pose_graph_interface.h"
-#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
+#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
 #include "ceres/jet.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 
 // Cost function measuring the weighted error between the observed pose given by
 // the landmark measurement and the linearly interpolated pose.
-class LandmarkCostFunction {
+class LandmarkCostFunction3D {
  public:
   using LandmarkObservation =
-      mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
+      PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
   static ceres::CostFunction* CreateAutoDiffCostFunction(
-      const LandmarkObservation& observation, const NodeData& prev_node,
+      const LandmarkObservation& observation,
-      const NodeData& next_node) {
+      const OptimizationProblem3D::NodeData& prev_node,
+      const OptimizationProblem3D::NodeData& next_node) {
     return new ceres::AutoDiffCostFunction<
-        LandmarkCostFunction, 6 /* residuals */,
+        LandmarkCostFunction3D, 6 /* residuals */,
         4 /* previous node rotation variables */,
         3 /* previous node translation variables */,
         4 /* next node rotation variables */,
         3 /* next node translation variables */,
         4 /* landmark rotation variables */,
         3 /* landmark translation variables */>(
-        new LandmarkCostFunction(observation, prev_node, next_node));
+        new LandmarkCostFunction3D(observation, prev_node, next_node));
   }
 
   template <typename T>
@@ -59,10 +60,6 @@ class LandmarkCostFunction {
                   const T* const next_node_translation,
                   const T* const landmark_rotation,
                   const T* const landmark_translation, T* const e) const {
-    using mapping::pose_graph::ComputeUnscaledError;
-    using mapping::pose_graph::ScaleError;
-    using mapping::pose_graph::SlerpQuaternions;
-
     const std::array<T, 3> interpolated_pose_translation{
         {prev_node_translation[0] +
              interpolation_parameter_ *
@@ -87,8 +84,9 @@ class LandmarkCostFunction {
   }
 
  private:
-  LandmarkCostFunction(const LandmarkObservation& observation,
+  LandmarkCostFunction3D(const LandmarkObservation& observation,
-                       const NodeData& prev_node, const NodeData& next_node)
+                         const OptimizationProblem3D::NodeData& prev_node,
+                         const OptimizationProblem3D::NodeData& next_node)
       : landmark_to_tracking_transform_(
             observation.landmark_to_tracking_transform),
         translation_weight_(observation.translation_weight),
@@ -104,7 +102,7 @@ class LandmarkCostFunction {
 };
 
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_

cartographer/mapping_3d/pose_graph/landmark_cost_function_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h"
+#include "cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h"
 
 #include <memory>
 
@@ -23,7 +23,7 @@
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 namespace {
 
@@ -31,16 +31,16 @@ using ::testing::DoubleEq;
 using ::testing::ElementsAre;
 
 using LandmarkObservation =
-    mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
+    PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
-TEST(LandmarkCostFunctionTest, SmokeTest) {
+TEST(LandmarkCostFunction3DTest, SmokeTest) {
-  NodeData prev_node;
+  OptimizationProblem3D::NodeData prev_node;
   prev_node.time = common::FromUniversal(0);
-  NodeData next_node;
+  OptimizationProblem3D::NodeData next_node;
   next_node.time = common::FromUniversal(10);
 
   std::unique_ptr<ceres::CostFunction> cost_function(
-      LandmarkCostFunction::CreateAutoDiffCostFunction(
+      LandmarkCostFunction3D::CreateAutoDiffCostFunction(
           LandmarkObservation{
               0 /* trajectory ID */,
               common::FromUniversal(5) /* time */,
@@ -73,5 +73,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
 
 }  // namespace
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/pose_graph/optimization_problem_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
+#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
 
 #include <algorithm>
 #include <array>
@@ -30,12 +30,12 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/time.h"
-#include "cartographer/internal/mapping_3d/acceleration_cost_function.h"
+#include "cartographer/internal/mapping_3d/acceleration_cost_function_3d.h"
-#include "cartographer/internal/mapping_3d/rotation_cost_function.h"
+#include "cartographer/internal/mapping_3d/rotation_cost_function_3d.h"
 #include "cartographer/mapping/pose_graph/ceres_pose.h"
 #include "cartographer/mapping_3d/imu_integration.h"
-#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h"
+#include "cartographer/mapping_3d/pose_graph/landmark_cost_function_3d.h"
-#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
+#include "cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h"
 #include "cartographer/mapping_3d/rotation_parameterization.h"
 #include "cartographer/transform/timestamped_transform.h"
 #include "cartographer/transform/transform.h"
@@ -45,14 +45,15 @@
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 namespace {
 
-using ::cartographer::mapping::pose_graph::CeresPose;
 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
 using TrajectoryData =
     ::cartographer::mapping::PoseGraphInterface::TrajectoryData;
+using NodeData = OptimizationProblem3D::NodeData;
+using SubmapData = OptimizationProblem3D::SubmapData;
 
 // For odometry.
 std::unique_ptr<transform::Rigid3d> Interpolate(
@@ -116,8 +117,8 @@ transform::Rigid3d GetInitialLandmarkPose(
 
 void AddLandmarkCostFunctions(
     const std::map<std::string, LandmarkNode>& landmark_nodes,
-    const mapping::MapById<mapping::NodeId, NodeData>& node_data,
+    const MapById<NodeId, NodeData>& node_data,
-    mapping::MapById<mapping::NodeId, CeresPose>* C_nodes,
+    MapById<NodeId, CeresPose>* C_nodes,
     std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
   for (const auto& landmark_node : landmark_nodes) {
     for (const auto& observation : landmark_node.second.landmark_observations) {
@@ -153,7 +154,7 @@ void AddLandmarkCostFunctions(
                       problem));
       }
       problem->AddResidualBlock(
-          LandmarkCostFunction::CreateAutoDiffCostFunction(
+          LandmarkCostFunction3D::CreateAutoDiffCostFunction(
               observation, prev->data, next->data),
           nullptr /* loss function */, C_nodes->at(prev->id).rotation(),
           C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(),
@@ -166,30 +167,29 @@ void AddLandmarkCostFunctions(
 
 }  // namespace
 
-OptimizationProblem::OptimizationProblem(
+OptimizationProblem3D::OptimizationProblem3D(
-    const mapping::pose_graph::proto::OptimizationProblemOptions& options,
+    const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z)
-    FixZ fix_z)
     : options_(options), fix_z_(fix_z) {}
 
-OptimizationProblem::~OptimizationProblem() {}
+OptimizationProblem3D::~OptimizationProblem3D() {}
 
-void OptimizationProblem::AddImuData(const int trajectory_id,
+void OptimizationProblem3D::AddImuData(const int trajectory_id,
-                                     const sensor::ImuData& imu_data) {
+                                       const sensor::ImuData& imu_data) {
   imu_data_.Append(trajectory_id, imu_data);
 }
 
-void OptimizationProblem::AddOdometryData(
+void OptimizationProblem3D::AddOdometryData(
     const int trajectory_id, const sensor::OdometryData& odometry_data) {
   odometry_data_.Append(trajectory_id, odometry_data);
 }
 
-void OptimizationProblem::AddFixedFramePoseData(
+void OptimizationProblem3D::AddFixedFramePoseData(
     const int trajectory_id,
     const sensor::FixedFramePoseData& fixed_frame_pose_data) {
   fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
 }
 
-void OptimizationProblem::AddTrajectoryNode(
+void OptimizationProblem3D::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid3d& local_pose,
     const transform::Rigid3d& global_pose) {
@@ -197,20 +197,20 @@ void OptimizationProblem::AddTrajectoryNode(
   trajectory_data_[trajectory_id];
 }
 
-void OptimizationProblem::SetTrajectoryData(
+void OptimizationProblem3D::SetTrajectoryData(
     int trajectory_id, const TrajectoryData& trajectory_data) {
   trajectory_data_[trajectory_id] = trajectory_data;
 }
 
-void OptimizationProblem::InsertTrajectoryNode(
+void OptimizationProblem3D::InsertTrajectoryNode(
-    const mapping::NodeId& node_id, const common::Time time,
+    const NodeId& node_id, const common::Time time,
     const transform::Rigid3d& local_pose,
     const transform::Rigid3d& global_pose) {
   node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
   trajectory_data_[node_id.trajectory_id];
 }
 
-void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
+void OptimizationProblem3D::TrimTrajectoryNode(const NodeId& node_id) {
   imu_data_.Trim(node_data_, node_id);
   odometry_data_.Trim(node_data_, node_id);
   fixed_frame_pose_data_.Trim(node_data_, node_id);
@@ -220,27 +220,27 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   }
 }
 
-void OptimizationProblem::AddSubmap(
+void OptimizationProblem3D::AddSubmap(
     const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
   submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
 }
 
-void OptimizationProblem::InsertSubmap(
+void OptimizationProblem3D::InsertSubmap(
-    const mapping::SubmapId& submap_id,
+    const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose) {
-    const transform::Rigid3d& global_submap_pose) {
   submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
 }
 
-void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
+void OptimizationProblem3D::TrimSubmap(const SubmapId& submap_id) {
   submap_data_.Trim(submap_id);
 }
 
-void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
+void OptimizationProblem3D::SetMaxNumIterations(
+    const int32 max_num_iterations) {
   options_.mutable_ceres_solver_options()->set_max_num_iterations(
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(
+void OptimizationProblem3D::Solve(
     const std::vector<Constraint>& constraints,
     const std::set<int>& frozen_trajectories,
     const std::map<std::string, LandmarkNode>& landmark_nodes) {
@@ -262,9 +262,9 @@ void OptimizationProblem::Solve(
 
   // Set the starting point.
   CHECK(!submap_data_.empty());
-  CHECK(submap_data_.Contains(mapping::SubmapId{0, 0}));
+  CHECK(submap_data_.Contains(SubmapId{0, 0}));
-  mapping::MapById<mapping::SubmapId, CeresPose> C_submaps;
+  MapById<SubmapId, CeresPose> C_submaps;
-  mapping::MapById<mapping::NodeId, CeresPose> C_nodes;
+  MapById<NodeId, CeresPose> C_nodes;
   std::map<std::string, CeresPose> C_landmarks;
   bool first_submap = true;
   for (const auto& submap_id_data : submap_data_) {
@@ -279,7 +279,7 @@ void OptimizationProblem::Solve(
           CeresPose(submap_id_data.data.global_pose,
                     translation_parameterization(),
                     common::make_unique<ceres::AutoDiffLocalParameterization<
-                        mapping::ConstantYawQuaternionPlus, 4, 2>>(),
+                        ConstantYawQuaternionPlus, 4, 2>>(),
                     &problem));
       problem.SetParameterBlockConstant(
           C_submaps.at(submap_id_data.id).translation());
@@ -315,7 +315,7 @@ void OptimizationProblem::Solve(
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     problem.AddResidualBlock(
-        SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
+        SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose),
         // Only loop closure constraints should have a loss function.
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
@@ -350,10 +350,10 @@ void OptimizationProblem::Solve(
       auto imu_it = imu_data.begin();
       auto prev_node_it = node_it;
       for (++node_it; node_it != trajectory_end; ++node_it) {
-        const mapping::NodeId first_node_id = prev_node_it->id;
+        const NodeId first_node_id = prev_node_it->id;
         const NodeData& first_node_data = prev_node_it->data;
         prev_node_it = node_it;
-        const mapping::NodeId second_node_id = node_it->id;
+        const NodeId second_node_id = node_it->id;
         const NodeData& second_node_data = node_it->data;
 
         if (second_node_id.node_index != first_node_id.node_index + 1) {
@@ -372,7 +372,7 @@ void OptimizationProblem::Solve(
         const auto next_node_it = std::next(node_it);
         if (next_node_it != trajectory_end &&
             next_node_it->id.node_index == second_node_id.node_index + 1) {
-          const mapping::NodeId third_node_id = next_node_it->id;
+          const NodeId third_node_id = next_node_it->id;
           const NodeData& third_node_data = next_node_it->data;
           const common::Time first_time = first_node_data.time;
           const common::Time second_time = second_node_data.time;
@@ -395,7 +395,7 @@ void OptimizationProblem::Solve(
                result_to_first_center.delta_rotation) *
               result_center_to_center.delta_velocity;
           problem.AddResidualBlock(
-              AccelerationCostFunction::CreateAutoDiffCostFunction(
+              AccelerationCostFunction3D::CreateAutoDiffCostFunction(
                   options_.acceleration_weight(), delta_velocity,
                   common::ToSeconds(first_duration),
                   common::ToSeconds(second_duration)),
@@ -430,10 +430,10 @@ void OptimizationProblem::Solve(
 
       auto prev_node_it = node_it;
       for (++node_it; node_it != trajectory_end; ++node_it) {
-        const mapping::NodeId first_node_id = prev_node_it->id;
+        const NodeId first_node_id = prev_node_it->id;
         const NodeData& first_node_data = prev_node_it->data;
         prev_node_it = node_it;
-        const mapping::NodeId second_node_id = node_it->id;
+        const NodeId second_node_id = node_it->id;
         const NodeData& second_node_data = node_it->data;
 
         if (second_node_id.node_index != first_node_id.node_index + 1) {
@@ -443,7 +443,7 @@ void OptimizationProblem::Solve(
         const transform::Rigid3d relative_pose = ComputeRelativePose(
             trajectory_id, first_node_data, second_node_data);
         problem.AddResidualBlock(
-            SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
+            SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{
                 relative_pose, options_.consecutive_node_translation_weight(),
                 options_.consecutive_node_rotation_weight()}),
             nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
@@ -467,7 +467,7 @@ void OptimizationProblem::Solve(
     const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
     bool fixed_frame_pose_initialized = false;
     for (; node_it != trajectory_end; ++node_it) {
-      const mapping::NodeId node_id = node_it->id;
+      const NodeId node_id = node_it->id;
       const NodeData& node_data = node_it->data;
 
       const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
@@ -499,13 +499,13 @@ void OptimizationProblem::Solve(
                         Eigen::Vector3d::UnitZ())),
                 nullptr,
                 common::make_unique<ceres::AutoDiffLocalParameterization<
-                    mapping::YawOnlyQuaternionPlus, 4, 1>>(),
+                    YawOnlyQuaternionPlus, 4, 1>>(),
                 &problem));
         fixed_frame_pose_initialized = true;
       }
 
       problem.AddResidualBlock(
-          SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose),
+          SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose),
           nullptr /* loss function */,
           C_fixed_frames.at(trajectory_id).rotation(),
           C_fixed_frames.at(trajectory_id).translation(),
@@ -553,42 +553,41 @@ void OptimizationProblem::Solve(
   }
 }
 
-const mapping::MapById<mapping::NodeId, NodeData>&
+const MapById<NodeId, NodeData>& OptimizationProblem3D::node_data() const {
-OptimizationProblem::node_data() const {
   return node_data_;
 }
 
-const mapping::MapById<mapping::SubmapId, SubmapData>&
+const MapById<SubmapId, SubmapData>& OptimizationProblem3D::submap_data()
-OptimizationProblem::submap_data() const {
+    const {
   return submap_data_;
 }
 
 const std::map<std::string, transform::Rigid3d>&
-OptimizationProblem::landmark_data() const {
+OptimizationProblem3D::landmark_data() const {
   return landmark_data_;
 }
 
-const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
+const sensor::MapByTime<sensor::ImuData>& OptimizationProblem3D::imu_data()
     const {
   return imu_data_;
 }
 
 const sensor::MapByTime<sensor::OdometryData>&
-OptimizationProblem::odometry_data() const {
+OptimizationProblem3D::odometry_data() const {
   return odometry_data_;
 }
 
 const sensor::MapByTime<sensor::FixedFramePoseData>&
-OptimizationProblem::fixed_frame_pose_data() const {
+OptimizationProblem3D::fixed_frame_pose_data() const {
   return fixed_frame_pose_data_;
 }
 
-const std::map<int, TrajectoryData>& OptimizationProblem::trajectory_data()
+const std::map<int, TrajectoryData>& OptimizationProblem3D::trajectory_data()
     const {
   return trajectory_data_;
 }
 
-transform::Rigid3d OptimizationProblem::ComputeRelativePose(
+transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
     const int trajectory_id, const NodeData& first_node_data,
     const NodeData& second_node_data) const {
   if (odometry_data_.HasTrajectory(trajectory_id)) {
@@ -604,5 +603,5 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose(
 }
 
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/pose_graph/optimization_problem_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
+#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
 
 #include <array>
 #include <map>
@@ -37,34 +37,33 @@
 #include "cartographer/transform/transform_interpolation_buffer.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 
-struct NodeData {
-  common::Time time;
-  transform::Rigid3d local_pose;
-  transform::Rigid3d global_pose;
-};
-
-struct SubmapData {
-  transform::Rigid3d global_pose;
-};
-
 // Implements the SPA loop closure method.
-class OptimizationProblem {
+class OptimizationProblem3D {
  public:
-  using Constraint = mapping::PoseGraphInterface::Constraint;
+  using Constraint = PoseGraphInterface::Constraint;
-  using LandmarkNode = mapping::PoseGraphInterface::LandmarkNode;
+  using LandmarkNode = PoseGraphInterface::LandmarkNode;
+
+  struct NodeData {
+    common::Time time;
+    transform::Rigid3d local_pose;
+    transform::Rigid3d global_pose;
+  };
+
+  struct SubmapData {
+    transform::Rigid3d global_pose;
+  };
 
   enum class FixZ { kYes, kNo };
 
-  OptimizationProblem(
+  OptimizationProblem3D(
-      const mapping::pose_graph::proto::OptimizationProblemOptions& options,
+      const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z);
-      FixZ fix_z);
+  ~OptimizationProblem3D();
-  ~OptimizationProblem();
 
-  OptimizationProblem(const OptimizationProblem&) = delete;
+  OptimizationProblem3D(const OptimizationProblem3D&) = delete;
-  OptimizationProblem& operator=(const OptimizationProblem&) = delete;
+  OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete;
 
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
   void AddOdometryData(int trajectory_id,
@@ -77,16 +76,16 @@ class OptimizationProblem {
                          const transform::Rigid3d& global_pose);
   void SetTrajectoryData(
       int trajectory_id,
-      const mapping::PoseGraphInterface::TrajectoryData& trajectory_data);
+      const PoseGraphInterface::TrajectoryData& trajectory_data);
-  void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
+  void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
                             const transform::Rigid3d& local_pose,
                             const transform::Rigid3d& global_pose);
-  void TrimTrajectoryNode(const mapping::NodeId& node_id);
+  void TrimTrajectoryNode(const NodeId& node_id);
   void AddSubmap(int trajectory_id,
                  const transform::Rigid3d& global_submap_pose);
-  void InsertSubmap(const mapping::SubmapId& submap_id,
+  void InsertSubmap(const SubmapId& submap_id,
                     const transform::Rigid3d& global_submap_pose);
-  void TrimSubmap(const mapping::SubmapId& submap_id);
+  void TrimSubmap(const SubmapId& submap_id);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 
@@ -95,15 +94,15 @@ class OptimizationProblem {
              const std::set<int>& frozen_trajectories,
              const std::map<std::string, LandmarkNode>& landmark_nodes);
 
-  const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
+  const MapById<NodeId, NodeData>& node_data() const;
-  const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
+  const MapById<SubmapId, SubmapData>& submap_data() const;
   const std::map<std::string, transform::Rigid3d>& landmark_data() const;
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
   const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
   const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
       const;
-  const std::map<int, mapping::PoseGraphInterface::TrajectoryData>&
+  const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
-  trajectory_data() const;
+      const;
 
  private:
   // Uses odometry if available, otherwise the local SLAM results.
@@ -111,19 +110,19 @@ class OptimizationProblem {
       int trajectory_id, const NodeData& first_node_data,
       const NodeData& second_node_data) const;
 
-  mapping::pose_graph::proto::OptimizationProblemOptions options_;
+  pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
-  mapping::MapById<mapping::NodeId, NodeData> node_data_;
+  MapById<NodeId, NodeData> node_data_;
-  mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
+  MapById<SubmapId, SubmapData> submap_data_;
   std::map<std::string, transform::Rigid3d> landmark_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
   sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
-  std::map<int, mapping::PoseGraphInterface::TrajectoryData> trajectory_data_;
+  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
 };
 
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_

cartographer/mapping_3d/pose_graph/optimization_problem_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
+#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
 
 #include <random>
 
@@ -27,17 +27,18 @@
 #include "gmock/gmock.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 namespace {
 
-class OptimizationProblemTest : public ::testing::Test {
+class OptimizationProblem3DTest : public ::testing::Test {
  protected:
-  OptimizationProblemTest()
+  OptimizationProblem3DTest()
-      : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo),
+      : optimization_problem_(CreateOptions(),
+                              OptimizationProblem3D::FixZ::kNo),
         rng_(45387) {}
 
-  mapping::pose_graph::proto::OptimizationProblemOptions CreateOptions() {
+  pose_graph::proto::OptimizationProblemOptions CreateOptions() {
     auto parameter_dictionary = common::MakeDictionary(R"text(
         return {
           acceleration_weight = 1e-4,
@@ -54,7 +55,7 @@ class OptimizationProblemTest : public ::testing::Test {
             num_threads = 4,
           },
         })text");
-    return mapping::pose_graph::CreateOptimizationProblemOptions(
+    return pose_graph::CreateOptimizationProblemOptions(
         parameter_dictionary.get());
   }
 
@@ -90,7 +91,7 @@ class OptimizationProblemTest : public ::testing::Test {
                                   Eigen::Vector3d(0., 0., rz)));
   }
 
-  OptimizationProblem optimization_problem_;
+  OptimizationProblem3D optimization_problem_;
   std::mt19937 rng_;
 };
 
@@ -102,7 +103,7 @@ transform::Rigid3d AddNoise(const transform::Rigid3d& transform,
                             noisy_rotation);
 }
 
-TEST_F(OptimizationProblemTest, ReducesNoise) {
+TEST_F(OptimizationProblem3DTest, ReducesNoise) {
   constexpr int kNumNodes = 100;
   const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity();
   const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
@@ -130,24 +131,24 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
     now += common::FromSeconds(0.01);
   }
 
-  std::vector<OptimizationProblem::Constraint> constraints;
+  std::vector<OptimizationProblem3D::Constraint> constraints;
   for (int j = 0; j != kNumNodes; ++j) {
-    constraints.push_back(OptimizationProblem::Constraint{
+    constraints.push_back(OptimizationProblem3D::Constraint{
-        mapping::SubmapId{kTrajectoryId, 0}, mapping::NodeId{kTrajectoryId, j},
+        SubmapId{kTrajectoryId, 0}, NodeId{kTrajectoryId, j},
-        OptimizationProblem::Constraint::Pose{
+        OptimizationProblem3D::Constraint::Pose{
             AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1.,
             1.}});
     // We add an additional independent, but equally noisy observation.
-    constraints.push_back(OptimizationProblem::Constraint{
+    constraints.push_back(OptimizationProblem3D::Constraint{
-        mapping::SubmapId{kTrajectoryId, 1}, mapping::NodeId{kTrajectoryId, j},
+        SubmapId{kTrajectoryId, 1}, NodeId{kTrajectoryId, j},
-        OptimizationProblem::Constraint::Pose{
+        OptimizationProblem3D::Constraint::Pose{
             AddNoise(test_data[j].ground_truth_pose,
                      RandomYawOnlyTransform(0.2, 0.3)),
             1., 1.}});
     // We add very noisy data with a low weight to verify it is mostly ignored.
-    constraints.push_back(OptimizationProblem::Constraint{
+    constraints.push_back(OptimizationProblem3D::Constraint{
-        mapping::SubmapId{kTrajectoryId, 2}, mapping::NodeId{kTrajectoryId, j},
+        SubmapId{kTrajectoryId, 2}, NodeId{kTrajectoryId, j},
-        OptimizationProblem::Constraint::Pose{
+        OptimizationProblem3D::Constraint::Pose{
             kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
                 RandomTransform(1e3, 3.),
             1e-9, 1e-9}});
@@ -157,13 +158,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   double rotation_error_before = 0.;
   const auto& node_data = optimization_problem_.node_data();
   for (int j = 0; j != kNumNodes; ++j) {
-    translation_error_before += (test_data[j].ground_truth_pose.translation() -
+    translation_error_before +=
-                                 node_data.at(mapping::NodeId{kTrajectoryId, j})
+        (test_data[j].ground_truth_pose.translation() -
-                                     .global_pose.translation())
+         node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation())
-                                    .norm();
+            .norm();
-    rotation_error_before += transform::GetAngle(
+    rotation_error_before +=
-        test_data[j].ground_truth_pose.inverse() *
+        transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
-        node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose);
+                            node_data.at(NodeId{kTrajectoryId, j}).global_pose);
   }
 
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
@@ -175,13 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   double translation_error_after = 0.;
   double rotation_error_after = 0.;
   for (int j = 0; j != kNumNodes; ++j) {
-    translation_error_after += (test_data[j].ground_truth_pose.translation() -
+    translation_error_after +=
-                                node_data.at(mapping::NodeId{kTrajectoryId, j})
+        (test_data[j].ground_truth_pose.translation() -
-                                    .global_pose.translation())
+         node_data.at(NodeId{kTrajectoryId, j}).global_pose.translation())
-                                   .norm();
+            .norm();
-    rotation_error_after += transform::GetAngle(
+    rotation_error_after +=
-        test_data[j].ground_truth_pose.inverse() *
+        transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
-        node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose);
+                            node_data.at(NodeId{kTrajectoryId, j}).global_pose);
   }
 
   EXPECT_GT(0.8 * translation_error_before, translation_error_after);
@@ -190,5 +191,5 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
 
 }  // namespace
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/pose_graph/spa_cost_function_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
+#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
 
 #include <array>
 
@@ -30,28 +30,23 @@
 #include "ceres/jet.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace pose_graph {
 
-class SpaCostFunction {
+class SpaCostFunction3D {
  public:
-  using Constraint = mapping::PoseGraph::Constraint;
-
   static ceres::CostFunction* CreateAutoDiffCostFunction(
-      const Constraint::Pose& pose) {
+      const PoseGraph::Constraint::Pose& pose) {
     return new ceres::AutoDiffCostFunction<
-        SpaCostFunction, 6 /* residuals */, 4 /* rotation variables */,
+        SpaCostFunction3D, 6 /* residuals */, 4 /* rotation variables */,
         3 /* translation variables */, 4 /* rotation variables */,
-        3 /* translation variables */>(new SpaCostFunction(pose));
+        3 /* translation variables */>(new SpaCostFunction3D(pose));
   }
 
   template <typename T>
   bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
                   const T* const c_j_rotation, const T* const c_j_translation,
                   T* const e) const {
-    using mapping::pose_graph::ComputeUnscaledError;
-    using mapping::pose_graph::ScaleError;
-
     const std::array<T, 6> error = ScaleError(
         ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation,
                              c_j_rotation, c_j_translation),
@@ -61,13 +56,14 @@ class SpaCostFunction {
   }
 
  private:
-  explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
+  explicit SpaCostFunction3D(const PoseGraph::Constraint::Pose& pose)
+      : pose_(pose) {}
 
-  const Constraint::Pose pose_;
+  const PoseGraph::Constraint::Pose pose_;
 };
 
 }  // namespace pose_graph
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_

cartographer/mapping_3d/pose_graph_3d.cc

@@ -42,9 +42,8 @@ namespace mapping {
 PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
                          common::ThreadPool* thread_pool)
     : options_(options),
-      optimization_problem_(
+      optimization_problem_(options_.optimization_problem_options(),
-          options_.optimization_problem_options(),
+                            pose_graph::OptimizationProblem3D::FixZ::kNo),
-          mapping_3d::pose_graph::OptimizationProblem::FixZ::kNo),
       constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
 
 PoseGraph3D::~PoseGraph3D() {
@@ -340,7 +339,7 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
 
 void PoseGraph3D::HandleWorkQueue() {
   constraint_builder_.WhenDone(
-      [this](const mapping_3d::pose_graph::ConstraintBuilder::Result& result) {
+      [this](const pose_graph::ConstraintBuilder3D::Result& result) {
         {
           common::MutexLocker locker(&mutex_);
           constraints_.insert(constraints_.end(), result.begin(), result.end());
@@ -401,8 +400,8 @@ void PoseGraph3D::WaitForAllComputations() {
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
   constraint_builder_.WhenDone(
-      [this, &notification](
+      [this,
-          const mapping_3d::pose_graph::ConstraintBuilder::Result& result) {
+       &notification](const pose_graph::ConstraintBuilder3D::Result& result) {
         common::MutexLocker locker(&mutex_);
         constraints_.insert(constraints_.end(), result.begin(), result.end());
         notification = true;
@@ -453,7 +452,8 @@ void PoseGraph3D::AddSubmapFromProto(
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
   global_submap_poses_.Insert(
-      submap_id, mapping_3d::pose_graph::SubmapData{global_submap_pose});
+      submap_id,
+      pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
   AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
     CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
     submap_data_.at(submap_id).state = SubmapState::kFinished;
@@ -759,7 +759,7 @@ PoseGraph3D::GetAllSubmapPoses() {
 }
 
 transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
-    const MapById<SubmapId, mapping_3d::pose_graph::SubmapData>&
+    const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
         global_submap_poses,
     const int trajectory_id) const {
   auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);

cartographer/mapping_3d/pose_graph_3d.h

@@ -35,8 +35,8 @@
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/mapping/trajectory_connectivity_state.h"
-#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
+#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h"
-#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
+#include "cartographer/mapping_3d/pose_graph/optimization_problem_3d.h"
 #include "cartographer/mapping_3d/submap_3d.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/landmark_data.h"
@@ -190,7 +190,7 @@ class PoseGraph3D : public PoseGraph {
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const MapById<SubmapId, mapping_3d::pose_graph::SubmapData>&
+      const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
           global_submap_poses,
       int trajectory_id) const REQUIRES(mutex_);
 
@@ -234,9 +234,8 @@ class PoseGraph3D : public PoseGraph {
   void DispatchOptimization() REQUIRES(mutex_);
 
   // Current optimization problem.
-  mapping_3d::pose_graph::OptimizationProblem optimization_problem_;
+  pose_graph::OptimizationProblem3D optimization_problem_;
-  mapping_3d::pose_graph::ConstraintBuilder constraint_builder_
+  pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
-      GUARDED_BY(mutex_);
   std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
 
   // Submaps get assigned an ID and state as soon as they are seen, even
@@ -248,8 +247,8 @@ class PoseGraph3D : public PoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Global submap poses currently used for displaying data.
-  MapById<SubmapId, mapping_3d::pose_graph::SubmapData> global_submap_poses_
+  MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>
-      GUARDED_BY(mutex_);
+      global_submap_poses_ GUARDED_BY(mutex_);
 
   // Global landmark poses with all observations.
   std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

cartographer/mapping_3d/proto/local_trajectory_builder_options_3d.proto

@@ -20,7 +20,7 @@ import "cartographer/mapping/proto/motion_filter_options.proto";
 import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
 import "cartographer/mapping/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
 import "cartographer/mapping_3d/proto/submaps_options_3d.proto";
-import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
+import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto";
 
 // NEXT ID: 18
 message LocalTrajectoryBuilderOptions3D {
@@ -47,7 +47,7 @@ message LocalTrajectoryBuilderOptions3D {
   bool use_online_correlative_scan_matching = 13;
   mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
       real_time_correlative_scan_matcher_options = 14;
-  mapping_3d.scan_matching.proto.CeresScanMatcherOptions
+  mapping.scan_matching.proto.CeresScanMatcherOptions3D
       ceres_scan_matcher_options = 6;
   mapping.proto.MotionFilterOptions motion_filter_options = 7;
 

cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
 
 #include <string>
 #include <utility>
@@ -22,23 +22,23 @@
 
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/make_unique.h"
-#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h"
+#include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function_3d.h"
 #include "cartographer/mapping/pose_graph/ceres_pose.h"
 #include "cartographer/mapping_3d/rotation_parameterization.h"
-#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h"
+#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h"
-#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h"
+#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
+proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
     common::LuaParameterDictionary* const parameter_dictionary) {
-  proto::CeresScanMatcherOptions options;
+  proto::CeresScanMatcherOptions3D options;
   for (int i = 0;; ++i) {
     const std::string lua_identifier =
         "occupied_space_weight_" + std::to_string(i);
@@ -60,27 +60,28 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
   return options;
 }
 
-CeresScanMatcher::CeresScanMatcher(
+CeresScanMatcher3D::CeresScanMatcher3D(
-    const proto::CeresScanMatcherOptions& options)
+    const proto::CeresScanMatcherOptions3D& options)
     : options_(options),
       ceres_solver_options_(
           common::CreateCeresSolverOptions(options.ceres_solver_options())) {
   ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
 }
 
-void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
+void CeresScanMatcher3D::Match(
-                             const transform::Rigid3d& initial_pose_estimate,
+    const Eigen::Vector3d& target_translation,
-                             const std::vector<PointCloudAndHybridGridPointers>&
+    const transform::Rigid3d& initial_pose_estimate,
-                                 point_clouds_and_hybrid_grids,
+    const std::vector<PointCloudAndHybridGridPointers>&
-                             transform::Rigid3d* const pose_estimate,
+        point_clouds_and_hybrid_grids,
-                             ceres::Solver::Summary* const summary) {
+    transform::Rigid3d* const pose_estimate,
+    ceres::Solver::Summary* const summary) {
   ceres::Problem problem;
-  mapping::pose_graph::CeresPose ceres_pose(
+  pose_graph::CeresPose ceres_pose(
       initial_pose_estimate, nullptr /* translation_parameterization */,
       options_.only_optimize_yaw()
           ? std::unique_ptr<ceres::LocalParameterization>(
                 common::make_unique<ceres::AutoDiffLocalParameterization<
-                    mapping::YawOnlyQuaternionPlus, 4, 1>>())
+                    YawOnlyQuaternionPlus, 4, 1>>())
           : std::unique_ptr<ceres::LocalParameterization>(
                 common::make_unique<ceres::QuaternionParameterization>()),
       &problem);
@@ -91,10 +92,9 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
     CHECK_GT(options_.occupied_space_weight(i), 0.);
     const sensor::PointCloud& point_cloud =
         *point_clouds_and_hybrid_grids[i].first;
-    const mapping::HybridGrid& hybrid_grid =
+    const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
-        *point_clouds_and_hybrid_grids[i].second;
     problem.AddResidualBlock(
-        OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
+        OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction(
             options_.occupied_space_weight(i) /
                 std::sqrt(static_cast<double>(point_cloud.size())),
             point_cloud, hybrid_grid),
@@ -103,12 +103,12 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
   }
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(
-      TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
+      TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(
           options_.translation_weight(), target_translation),
       nullptr /* loss function */, ceres_pose.translation());
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(
-      RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
+      RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(
           options_.rotation_weight(), initial_pose_estimate.rotation()),
       nullptr /* loss function */, ceres_pose.rotation());
 
@@ -118,5 +118,5 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
 }
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_
 
 #include <utility>
 #include <vector>
@@ -23,27 +23,27 @@
 #include "Eigen/Core"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
-#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.pb.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
+proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
     common::LuaParameterDictionary* parameter_dictionary);
 
 using PointCloudAndHybridGridPointers =
-    std::pair<const sensor::PointCloud*, const mapping::HybridGrid*>;
+    std::pair<const sensor::PointCloud*, const HybridGrid*>;
 
 // This scan matcher uses Ceres to align scans with an existing map.
-class CeresScanMatcher {
+class CeresScanMatcher3D {
  public:
-  explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options);
+  explicit CeresScanMatcher3D(const proto::CeresScanMatcherOptions3D& options);
 
-  CeresScanMatcher(const CeresScanMatcher&) = delete;
+  CeresScanMatcher3D(const CeresScanMatcher3D&) = delete;
-  CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
+  CeresScanMatcher3D& operator=(const CeresScanMatcher3D&) = delete;
 
   // Aligns 'point_clouds' within the 'hybrid_grids' given an
   // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
@@ -56,12 +56,12 @@ class CeresScanMatcher {
              ceres::Solver::Summary* summary);
 
  private:
-  const proto::CeresScanMatcherOptions options_;
+  const proto::CeresScanMatcherOptions3D options_;
   ceres::Solver::Options ceres_solver_options_;
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_3D_H_

cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher_3d.h"
 
 #include <memory>
 
@@ -27,13 +27,13 @@
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
-class CeresScanMatcherTest : public ::testing::Test {
+class CeresScanMatcher3DTest : public ::testing::Test {
  protected:
-  CeresScanMatcherTest()
+  CeresScanMatcher3DTest()
       : hybrid_grid_(1.f),
         expected_pose_(
             transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
@@ -59,8 +59,8 @@ class CeresScanMatcherTest : public ::testing::Test {
             num_threads = 1,
           },
         })text");
-    options_ = CreateCeresScanMatcherOptions(parameter_dictionary.get());
+    options_ = CreateCeresScanMatcherOptions3D(parameter_dictionary.get());
-    ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
+    ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
   }
 
   void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
@@ -74,35 +74,35 @@ class CeresScanMatcherTest : public ::testing::Test {
     EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
   }
 
-  mapping::HybridGrid hybrid_grid_;
+  HybridGrid hybrid_grid_;
   transform::Rigid3d expected_pose_;
   sensor::PointCloud point_cloud_;
-  proto::CeresScanMatcherOptions options_;
+  proto::CeresScanMatcherOptions3D options_;
-  std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
+  std::unique_ptr<CeresScanMatcher3D> ceres_scan_matcher_;
 };
 
-TEST_F(CeresScanMatcherTest, PerfectEstimate) {
+TEST_F(CeresScanMatcher3DTest, PerfectEstimate) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
 }
 
-TEST_F(CeresScanMatcherTest, AlongX) {
+TEST_F(CeresScanMatcher3DTest, AlongX) {
-  ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
+  ceres_scan_matcher_.reset(new CeresScanMatcher3D(options_));
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
 }
 
-TEST_F(CeresScanMatcherTest, AlongZ) {
+TEST_F(CeresScanMatcher3DTest, AlongZ) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
 }
 
-TEST_F(CeresScanMatcherTest, AlongXYZ) {
+TEST_F(CeresScanMatcher3DTest, AlongXYZ) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
 }
 
-TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
+TEST_F(CeresScanMatcher3DTest, FullPoseCorrection) {
   // We try to find the rotation around z...
   const auto additional_transform = transform::Rigid3d::Rotation(
       Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.)));
@@ -117,5 +117,5 @@ TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
 
 #include <algorithm>
 #include <cmath>
@@ -25,19 +25,19 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
-#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
+#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h"
-#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-proto::FastCorrelativeScanMatcherOptions
+proto::FastCorrelativeScanMatcherOptions3D
-CreateFastCorrelativeScanMatcherOptions(
+CreateFastCorrelativeScanMatcherOptions3D(
     common::LuaParameterDictionary* const parameter_dictionary) {
-  proto::FastCorrelativeScanMatcherOptions options;
+  proto::FastCorrelativeScanMatcherOptions3D options;
   options.set_branch_and_bound_depth(
       parameter_dictionary->GetInt("branch_and_bound_depth"));
   options.set_full_resolution_depth(
@@ -58,8 +58,8 @@ CreateFastCorrelativeScanMatcherOptions(
 class PrecomputationGridStack {
  public:
   PrecomputationGridStack(
-      const mapping::HybridGrid& hybrid_grid,
+      const HybridGrid& hybrid_grid,
-      const proto::FastCorrelativeScanMatcherOptions& options) {
+      const proto::FastCorrelativeScanMatcherOptions3D& options) {
     CHECK_GE(options.branch_and_bound_depth(), 1);
     CHECK_GE(options.full_resolution_depth(), 1);
     precomputation_grids_.reserve(options.branch_and_bound_depth());
@@ -80,29 +80,29 @@ class PrecomputationGridStack {
     }
   }
 
-  const PrecomputationGrid& Get(int depth) const {
+  const PrecomputationGrid3D& Get(int depth) const {
     return precomputation_grids_.at(depth);
   }
 
   int max_depth() const { return precomputation_grids_.size() - 1; }
 
  private:
-  std::vector<PrecomputationGrid> precomputation_grids_;
+  std::vector<PrecomputationGrid3D> precomputation_grids_;
 };
 
-struct DiscreteScan {
+struct DiscreteScan3D {
   transform::Rigid3f pose;
   // Contains a vector of discretized scans for each 'depth'.
   std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
   float rotational_score;
 };
 
-struct Candidate {
+struct Candidate3D {
-  Candidate(const int scan_index, const Eigen::Array3i& offset)
+  Candidate3D(const int scan_index, const Eigen::Array3i& offset)
       : scan_index(scan_index), offset(offset) {}
 
-  static Candidate Unsuccessful() {
+  static Candidate3D Unsuccessful() {
-    return Candidate(0, Eigen::Array3i::Zero());
+    return Candidate3D(0, Eigen::Array3i::Zero());
   }
 
   // Index into the discrete scans vectors.
@@ -119,14 +119,14 @@ struct Candidate {
   // Score of the low resolution matcher.
   float low_resolution_score = 0.f;
 
-  bool operator<(const Candidate& other) const { return score < other.score; }
+  bool operator<(const Candidate3D& other) const { return score < other.score; }
-  bool operator>(const Candidate& other) const { return score > other.score; }
+  bool operator>(const Candidate3D& other) const { return score > other.score; }
 };
 
 namespace {
 
 std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
-    const std::vector<mapping::TrajectoryNode>& nodes) {
+    const std::vector<TrajectoryNode>& nodes) {
   std::vector<std::pair<Eigen::VectorXf, float>> histograms_at_angles;
   for (const auto& node : nodes) {
     histograms_at_angles.emplace_back(
@@ -141,11 +141,11 @@ std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
 
 }  // namespace
 
-FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
+FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
-    const mapping::HybridGrid& hybrid_grid,
+    const HybridGrid& hybrid_grid,
-    const mapping::HybridGrid* const low_resolution_hybrid_grid,
+    const HybridGrid* const low_resolution_hybrid_grid,
-    const std::vector<mapping::TrajectoryNode>& nodes,
+    const std::vector<TrajectoryNode>& nodes,
-    const proto::FastCorrelativeScanMatcherOptions& options)
+    const proto::FastCorrelativeScanMatcherOptions3D& options)
     : options_(options),
       resolution_(hybrid_grid.resolution()),
       width_in_voxels_(hybrid_grid.grid_size()),
@@ -154,14 +154,13 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
       low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
       rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
 
-FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
+FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {}
 
-std::unique_ptr<FastCorrelativeScanMatcher::Result>
+std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
-FastCorrelativeScanMatcher::Match(
+FastCorrelativeScanMatcher3D::Match(
     const transform::Rigid3d& global_node_pose,
     const transform::Rigid3d& global_submap_pose,
-    const mapping::TrajectoryNode::Data& constant_data,
+    const TrajectoryNode::Data& constant_data, const float min_score) const {
-    const float min_score) const {
   const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
       low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
   const SearchParameters search_parameters{
@@ -176,12 +175,11 @@ FastCorrelativeScanMatcher::Match(
       constant_data.gravity_alignment, min_score);
 }
 
-std::unique_ptr<FastCorrelativeScanMatcher::Result>
+std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
-FastCorrelativeScanMatcher::MatchFullSubmap(
+FastCorrelativeScanMatcher3D::MatchFullSubmap(
     const Eigen::Quaterniond& global_node_rotation,
     const Eigen::Quaterniond& global_submap_rotation,
-    const mapping::TrajectoryNode::Data& constant_data,
+    const TrajectoryNode::Data& constant_data, const float min_score) const {
-    const float min_score) const {
   float max_point_distance = 0.f;
   for (const Eigen::Vector3f& point :
        constant_data.high_resolution_point_cloud) {
@@ -203,22 +201,22 @@ FastCorrelativeScanMatcher::MatchFullSubmap(
       constant_data.gravity_alignment, min_score);
 }
 
-std::unique_ptr<FastCorrelativeScanMatcher::Result>
+std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
-FastCorrelativeScanMatcher::MatchWithSearchParameters(
+FastCorrelativeScanMatcher3D::MatchWithSearchParameters(
-    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
     const transform::Rigid3f& global_node_pose,
     const transform::Rigid3f& global_submap_pose,
     const sensor::PointCloud& point_cloud,
     const Eigen::VectorXf& rotational_scan_matcher_histogram,
     const Eigen::Quaterniond& gravity_alignment, const float min_score) const {
-  const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
+  const std::vector<DiscreteScan3D> discrete_scans = GenerateDiscreteScans(
       search_parameters, point_cloud, rotational_scan_matcher_histogram,
       gravity_alignment, global_node_pose, global_submap_pose);
 
-  const std::vector<Candidate> lowest_resolution_candidates =
+  const std::vector<Candidate3D> lowest_resolution_candidates =
       ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
 
-  const Candidate best_candidate = BranchAndBound(
+  const Candidate3D best_candidate = BranchAndBound(
       search_parameters, discrete_scans, lowest_resolution_candidates,
       precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
@@ -231,12 +229,13 @@ FastCorrelativeScanMatcher::MatchWithSearchParameters(
   return nullptr;
 }
 
-DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
+DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan(
-    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
     const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose,
     const float rotational_score) const {
   std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
-  const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0);
+  const PrecomputationGrid3D& original_grid =
+      precomputation_grid_stack_->Get(0);
   std::vector<Eigen::Array3i> full_resolution_cell_indices;
   for (const Eigen::Vector3f& point :
        sensor::TransformPointCloud(point_cloud, pose)) {
@@ -272,17 +271,17 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
           low_resolution_cell_at_start - low_resolution_search_window_start);
     }
   }
-  return DiscreteScan{pose, cell_indices_per_depth, rotational_score};
+  return DiscreteScan3D{pose, cell_indices_per_depth, rotational_score};
 }
 
-std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
+std::vector<DiscreteScan3D> FastCorrelativeScanMatcher3D::GenerateDiscreteScans(
-    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
     const sensor::PointCloud& point_cloud,
     const Eigen::VectorXf& rotational_scan_matcher_histogram,
     const Eigen::Quaterniond& gravity_alignment,
     const transform::Rigid3f& global_node_pose,
     const transform::Rigid3f& global_submap_pose) const {
-  std::vector<DiscreteScan> result;
+  std::vector<DiscreteScan3D> 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_;
@@ -326,9 +325,9 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
   return result;
 }
 
-std::vector<Candidate>
+std::vector<Candidate3D>
-FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
+FastCorrelativeScanMatcher3D::GenerateLowestResolutionCandidates(
-    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
     const int num_discrete_scans) const {
   const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
   const int num_lowest_resolution_linear_xy_candidates =
@@ -341,7 +340,7 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
       num_discrete_scans *
       common::Power(num_lowest_resolution_linear_xy_candidates, 2) *
       num_lowest_resolution_linear_z_candidates;
-  std::vector<Candidate> candidates;
+  std::vector<Candidate3D> candidates;
   candidates.reserve(num_candidates);
   for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) {
     for (int z = -search_parameters.linear_z_window_size;
@@ -361,14 +360,14 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
   return candidates;
 }
 
-void FastCorrelativeScanMatcher::ScoreCandidates(
+void FastCorrelativeScanMatcher3D::ScoreCandidates(
-    const int depth, const std::vector<DiscreteScan>& discrete_scans,
+    const int depth, const std::vector<DiscreteScan3D>& discrete_scans,
-    std::vector<Candidate>* const candidates) const {
+    std::vector<Candidate3D>* const candidates) const {
   const int reduction_exponent =
       std::max(0, depth - options_.full_resolution_depth() + 1);
-  for (Candidate& candidate : *candidates) {
+  for (Candidate3D& candidate : *candidates) {
     int sum = 0;
-    const DiscreteScan& discrete_scan = discrete_scans[candidate.scan_index];
+    const DiscreteScan3D& discrete_scan = discrete_scans[candidate.scan_index];
     const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent,
                                 candidate.offset[1] >> reduction_exponent,
                                 candidate.offset[2] >> reduction_exponent);
@@ -378,18 +377,19 @@ void FastCorrelativeScanMatcher::ScoreCandidates(
       const Eigen::Array3i proposed_cell_index = cell_index + offset;
       sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index);
     }
-    candidate.score = PrecomputationGrid::ToProbability(
+    candidate.score = PrecomputationGrid3D::ToProbability(
         sum /
         static_cast<float>(discrete_scan.cell_indices_per_depth[depth].size()));
   }
-  std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
+  std::sort(candidates->begin(), candidates->end(),
+            std::greater<Candidate3D>());
 }
 
-std::vector<Candidate>
+std::vector<Candidate3D>
-FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
+FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates(
-    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
-    const std::vector<DiscreteScan>& discrete_scans) const {
+    const std::vector<DiscreteScan3D>& discrete_scans) const {
-  std::vector<Candidate> lowest_resolution_candidates =
+  std::vector<Candidate3D> lowest_resolution_candidates =
       GenerateLowestResolutionCandidates(search_parameters,
                                          discrete_scans.size());
   ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans,
@@ -397,48 +397,48 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
   return lowest_resolution_candidates;
 }
 
-transform::Rigid3f FastCorrelativeScanMatcher::GetPoseFromCandidate(
+transform::Rigid3f FastCorrelativeScanMatcher3D::GetPoseFromCandidate(
-    const std::vector<DiscreteScan>& discrete_scans,
+    const std::vector<DiscreteScan3D>& discrete_scans,
-    const Candidate& candidate) const {
+    const Candidate3D& candidate) const {
   return transform::Rigid3f::Translation(
              resolution_ * candidate.offset.matrix().cast<float>()) *
          discrete_scans[candidate.scan_index].pose;
 }
 
-Candidate FastCorrelativeScanMatcher::BranchAndBound(
+Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound(
-    const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
+    const FastCorrelativeScanMatcher3D::SearchParameters& search_parameters,
-    const std::vector<DiscreteScan>& discrete_scans,
+    const std::vector<DiscreteScan3D>& discrete_scans,
-    const std::vector<Candidate>& candidates, const int candidate_depth,
+    const std::vector<Candidate3D>& candidates, const int candidate_depth,
     float min_score) const {
   if (candidate_depth == 0) {
-    for (const Candidate& candidate : candidates) {
+    for (const Candidate3D& candidate : candidates) {
       if (candidate.score <= min_score) {
         // Return if the candidate is bad because the following candidate will
         // not have better score.
-        return Candidate::Unsuccessful();
+        return Candidate3D::Unsuccessful();
       }
       const float low_resolution_score =
           (*search_parameters.low_resolution_matcher)(
               GetPoseFromCandidate(discrete_scans, candidate));
       if (low_resolution_score >= options_.min_low_resolution_score()) {
         // We found the best candidate that passes the matching function.
-        Candidate best_candidate = candidate;
+        Candidate3D best_candidate = candidate;
         best_candidate.low_resolution_score = low_resolution_score;
         return best_candidate;
       }
     }
 
     // All candidates have good scores but none passes the matching function.
-    return Candidate::Unsuccessful();
+    return Candidate3D::Unsuccessful();
   }
 
-  Candidate best_high_resolution_candidate = Candidate::Unsuccessful();
+  Candidate3D best_high_resolution_candidate = Candidate3D::Unsuccessful();
   best_high_resolution_candidate.score = min_score;
-  for (const Candidate& candidate : candidates) {
+  for (const Candidate3D& candidate : candidates) {
     if (candidate.score <= min_score) {
       break;
     }
-    std::vector<Candidate> higher_resolution_candidates;
+    std::vector<Candidate3D> higher_resolution_candidates;
     const int half_width = 1 << (candidate_depth - 1);
     for (int z : {0, half_width}) {
       if (candidate.offset.z() + z > search_parameters.linear_z_window_size) {
@@ -471,5 +471,5 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
 }
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h

@@ -17,8 +17,8 @@
 // This is an implementation of a 3D branch-and-bound algorithm similar to
 // FastCorrelativeScanMatcher2D.
 
-#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_
 
 #include <memory>
 #include <vector>
@@ -28,26 +28,26 @@
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_2d.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
-#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
 #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h"
 #include "cartographer/sensor/point_cloud.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-proto::FastCorrelativeScanMatcherOptions
+proto::FastCorrelativeScanMatcherOptions3D
-CreateFastCorrelativeScanMatcherOptions(
+CreateFastCorrelativeScanMatcherOptions3D(
     common::LuaParameterDictionary* parameter_dictionary);
 
 class PrecomputationGridStack;
-struct DiscreteScan;
+struct DiscreteScan3D;
-struct Candidate;
+struct Candidate3D;
 
 // Used to compute scores between 0 and 1 how well the given pose matches.
 using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
 
-class FastCorrelativeScanMatcher {
+class FastCorrelativeScanMatcher3D {
  public:
   struct Result {
     float score;
@@ -56,25 +56,24 @@ class FastCorrelativeScanMatcher {
     float low_resolution_score;
   };
 
-  FastCorrelativeScanMatcher(
+  FastCorrelativeScanMatcher3D(
-      const mapping::HybridGrid& hybrid_grid,
+      const HybridGrid& hybrid_grid,
-      const mapping::HybridGrid* low_resolution_hybrid_grid,
+      const HybridGrid* low_resolution_hybrid_grid,
-      const std::vector<mapping::TrajectoryNode>& nodes,
+      const std::vector<TrajectoryNode>& nodes,
-      const proto::FastCorrelativeScanMatcherOptions& options);
+      const proto::FastCorrelativeScanMatcherOptions3D& options);
-  ~FastCorrelativeScanMatcher();
+  ~FastCorrelativeScanMatcher3D();
 
-  FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete;
+  FastCorrelativeScanMatcher3D(const FastCorrelativeScanMatcher3D&) = delete;
-  FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) =
+  FastCorrelativeScanMatcher3D& operator=(const FastCorrelativeScanMatcher3D&) =
       delete;
 
   // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
   // given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
   // returned if a score above 'min_score' (excluding equality) is possible.
-  std::unique_ptr<Result> Match(
+  std::unique_ptr<Result> Match(const transform::Rigid3d& global_node_pose,
-      const transform::Rigid3d& global_node_pose,
+                                const transform::Rigid3d& global_submap_pose,
-      const transform::Rigid3d& global_submap_pose,
+                                const TrajectoryNode::Data& constant_data,
-      const mapping::TrajectoryNode::Data& constant_data,
+                                float min_score) const;
-      float min_score) const;
 
   // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
   // given rotations which are expected to be approximately gravity aligned.
@@ -83,8 +82,7 @@ class FastCorrelativeScanMatcher {
   std::unique_ptr<Result> MatchFullSubmap(
       const Eigen::Quaterniond& global_node_rotation,
       const Eigen::Quaterniond& global_submap_rotation,
-      const mapping::TrajectoryNode::Data& constant_data,
+      const TrajectoryNode::Data& constant_data, float min_score) const;
-      float min_score) const;
 
  private:
   struct SearchParameters {
@@ -101,43 +99,43 @@ class FastCorrelativeScanMatcher {
       const sensor::PointCloud& point_cloud,
       const Eigen::VectorXf& rotational_scan_matcher_histogram,
       const Eigen::Quaterniond& gravity_alignment, float min_score) const;
-  DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
+  DiscreteScan3D DiscretizeScan(const SearchParameters& search_parameters,
-                              const sensor::PointCloud& point_cloud,
+                                const sensor::PointCloud& point_cloud,
-                              const transform::Rigid3f& pose,
+                                const transform::Rigid3f& pose,
-                              float rotational_score) const;
+                                float rotational_score) const;
-  std::vector<DiscreteScan> GenerateDiscreteScans(
+  std::vector<DiscreteScan3D> GenerateDiscreteScans(
       const SearchParameters& search_parameters,
       const sensor::PointCloud& point_cloud,
       const Eigen::VectorXf& rotational_scan_matcher_histogram,
       const Eigen::Quaterniond& gravity_alignment,
       const transform::Rigid3f& global_node_pose,
       const transform::Rigid3f& global_submap_pose) const;
-  std::vector<Candidate> GenerateLowestResolutionCandidates(
+  std::vector<Candidate3D> GenerateLowestResolutionCandidates(
       const SearchParameters& search_parameters, int num_discrete_scans) const;
   void ScoreCandidates(int depth,
-                       const std::vector<DiscreteScan>& discrete_scans,
+                       const std::vector<DiscreteScan3D>& discrete_scans,
-                       std::vector<Candidate>* const candidates) const;
+                       std::vector<Candidate3D>* const candidates) const;
-  std::vector<Candidate> ComputeLowestResolutionCandidates(
+  std::vector<Candidate3D> ComputeLowestResolutionCandidates(
       const SearchParameters& search_parameters,
-      const std::vector<DiscreteScan>& discrete_scans) const;
+      const std::vector<DiscreteScan3D>& discrete_scans) const;
-  Candidate BranchAndBound(const SearchParameters& search_parameters,
+  Candidate3D BranchAndBound(const SearchParameters& search_parameters,
-                           const std::vector<DiscreteScan>& discrete_scans,
+                             const std::vector<DiscreteScan3D>& discrete_scans,
-                           const std::vector<Candidate>& candidates,
+                             const std::vector<Candidate3D>& candidates,
-                           int candidate_depth, float min_score) const;
+                             int candidate_depth, float min_score) const;
   transform::Rigid3f GetPoseFromCandidate(
-      const std::vector<DiscreteScan>& discrete_scans,
+      const std::vector<DiscreteScan3D>& discrete_scans,
-      const Candidate& candidate) const;
+      const Candidate3D& candidate) const;
 
-  const proto::FastCorrelativeScanMatcherOptions options_;
+  const proto::FastCorrelativeScanMatcherOptions3D options_;
   const float resolution_;
   const int width_in_voxels_;
   std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
-  const mapping::HybridGrid* const low_resolution_hybrid_grid_;
+  const HybridGrid* const low_resolution_hybrid_grid_;
   RotationalScanMatcher rotational_scan_matcher_;
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_3D_H_

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_3d.h"
 
 #include <algorithm>
 #include <cmath>
@@ -29,15 +29,15 @@
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
-class FastCorrelativeScanMatcherTest : public ::testing::Test {
+class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
  protected:
-  FastCorrelativeScanMatcherTest()
+  FastCorrelativeScanMatcher3DTest()
-      : range_data_inserter_(CreateRangeDataInserterTestOptions()),
+      : range_data_inserter_(CreateRangeDataInserterTestOptions3D()),
-        options_(CreateFastCorrelativeScanMatcherTestOptions(5)) {}
+        options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(5)) {}
 
   void SetUp() override {
     point_cloud_ = {
@@ -59,8 +59,8 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
                Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
   }
 
-  static proto::FastCorrelativeScanMatcherOptions
+  static proto::FastCorrelativeScanMatcherOptions3D
-  CreateFastCorrelativeScanMatcherTestOptions(
+  CreateFastCorrelativeScanMatcher3DTestOptions3D(
       const int branch_and_bound_depth) {
     auto parameter_dictionary = common::MakeDictionary(
         "return {"
@@ -78,25 +78,25 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
         "linear_z_search_window = 0.8, "
         "angular_search_window = 0.3, "
         "}");
-    return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
+    return CreateFastCorrelativeScanMatcherOptions3D(
+        parameter_dictionary.get());
   }
 
   static mapping::proto::RangeDataInserterOptions3D
-  CreateRangeDataInserterTestOptions() {
+  CreateRangeDataInserterTestOptions3D() {
     auto parameter_dictionary = common::MakeDictionary(
         "return { "
         "hit_probability = 0.7, "
         "miss_probability = 0.4, "
         "num_free_space_voxels = 5, "
         "}");
-    return mapping::CreateRangeDataInserterOptions3D(
+    return CreateRangeDataInserterOptions3D(parameter_dictionary.get());
-        parameter_dictionary.get());
   }
 
-  std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
+  std::unique_ptr<FastCorrelativeScanMatcher3D> GetFastCorrelativeScanMatcher(
-      const proto::FastCorrelativeScanMatcherOptions& options,
+      const proto::FastCorrelativeScanMatcherOptions3D& options,
       const transform::Rigid3f& pose) {
-    hybrid_grid_ = common::make_unique<mapping::HybridGrid>(0.05f);
+    hybrid_grid_ = common::make_unique<HybridGrid>(0.05f);
     range_data_inserter_.Insert(
         sensor::RangeData{pose.translation(),
                           sensor::TransformPointCloud(point_cloud_, pose),
@@ -104,44 +104,44 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
         hybrid_grid_.get());
     hybrid_grid_->FinishUpdate();
 
-    return common::make_unique<FastCorrelativeScanMatcher>(
+    return common::make_unique<FastCorrelativeScanMatcher3D>(
         *hybrid_grid_, hybrid_grid_.get(),
-        std::vector<mapping::TrajectoryNode>(
+        std::vector<TrajectoryNode>(
-            {{std::make_shared<const mapping::TrajectoryNode::Data>(
+            {{std::make_shared<const TrajectoryNode::Data>(
                   CreateConstantData(point_cloud_)),
               pose.cast<double>()}}),
         options);
   }
 
-  mapping::TrajectoryNode::Data CreateConstantData(
+  TrajectoryNode::Data CreateConstantData(
       const sensor::PointCloud& low_resolution_point_cloud) {
-    return mapping::TrajectoryNode::Data{common::FromUniversal(0),
+    return TrajectoryNode::Data{common::FromUniversal(0),
-                                         Eigen::Quaterniond::Identity(),
+                                Eigen::Quaterniond::Identity(),
-                                         {},
+                                {},
-                                         point_cloud_,
+                                point_cloud_,
-                                         low_resolution_point_cloud,
+                                low_resolution_point_cloud,
-                                         Eigen::VectorXf::Zero(10)};
+                                Eigen::VectorXf::Zero(10)};
   }
 
   std::mt19937 prng_ = std::mt19937(42);
   std::uniform_real_distribution<float> distribution_ =
       std::uniform_real_distribution<float>(-1.f, 1.f);
-  mapping::RangeDataInserter3D range_data_inserter_;
+  RangeDataInserter3D range_data_inserter_;
-  const proto::FastCorrelativeScanMatcherOptions options_;
+  const proto::FastCorrelativeScanMatcherOptions3D options_;
   sensor::PointCloud point_cloud_;
-  std::unique_ptr<mapping::HybridGrid> hybrid_grid_;
+  std::unique_ptr<HybridGrid> hybrid_grid_;
 };
 
 constexpr float kMinScore = 0.1f;
 
-TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
+TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
   for (int i = 0; i != 20; ++i) {
     const auto expected_pose = GetRandomPose();
 
-    std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
+    std::unique_ptr<FastCorrelativeScanMatcher3D> fast_correlative_scan_matcher(
         GetFastCorrelativeScanMatcher(options_, expected_pose));
 
-    const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
+    const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> result =
         fast_correlative_scan_matcher->Match(
             transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
             CreateConstantData(point_cloud_), kMinScore);
@@ -154,7 +154,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
         << "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
         << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
 
-    const std::unique_ptr<FastCorrelativeScanMatcher::Result>
+    const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
         low_resolution_result = fast_correlative_scan_matcher->Match(
             transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
             CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
@@ -163,13 +163,13 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
   }
 }
 
-TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
+TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
   const auto expected_pose = GetRandomPose();
 
-  std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
+  std::unique_ptr<FastCorrelativeScanMatcher3D> fast_correlative_scan_matcher(
       GetFastCorrelativeScanMatcher(options_, expected_pose));
 
-  const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
+  const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> result =
       fast_correlative_scan_matcher->MatchFullSubmap(
           Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
           CreateConstantData(point_cloud_), kMinScore);
@@ -182,7 +182,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
       << "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
       << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
 
-  const std::unique_ptr<FastCorrelativeScanMatcher::Result>
+  const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
       low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
           Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
           CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
@@ -192,5 +192,5 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/interpolated_grid.h

@@ -22,7 +22,7 @@
 #include "cartographer/mapping_3d/hybrid_grid.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 // Interpolates between HybridGrid probability voxels. We use the tricubic
@@ -34,7 +34,7 @@ namespace scan_matching {
 // continuously differentiable.
 class InterpolatedGrid {
  public:
-  explicit InterpolatedGrid(const mapping::HybridGrid& hybrid_grid)
+  explicit InterpolatedGrid(const HybridGrid& hybrid_grid)
       : hybrid_grid_(hybrid_grid) {}
 
   InterpolatedGrid(const InterpolatedGrid&) = delete;
@@ -145,11 +145,11 @@ class InterpolatedGrid {
     return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
   }
 
-  const mapping::HybridGrid& hybrid_grid_;
+  const HybridGrid& hybrid_grid_;
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
 #endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_

cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc

@@ -21,7 +21,7 @@
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
@@ -43,7 +43,7 @@ class InterpolatedGridTest : public ::testing::Test {
         hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
   }
 
-  mapping::HybridGrid hybrid_grid_;
+  HybridGrid hybrid_grid_;
   InterpolatedGrid interpolated_grid_;
 };
 
@@ -85,5 +85,5 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc

@@ -17,12 +17,11 @@
 #include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
-    const mapping::HybridGrid* low_resolution_grid,
+    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
-    const sensor::PointCloud* points) {
   return [=](const transform::Rigid3f& pose) {
     float score = 0.f;
     for (const Eigen::Vector3f& point :
@@ -36,5 +35,5 @@ std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
 }
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/low_resolution_matcher.h

@@ -24,15 +24,14 @@
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
-    const mapping::HybridGrid* low_resolution_grid,
+    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points);
-    const sensor::PointCloud* points);
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
 #endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_

cartographer/mapping_3d/scan_matching/precomputation_grid_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
+#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h"
 
 #include <algorithm>
 
@@ -24,9 +24,8 @@
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
-
 namespace {
 
 // C++11 defines that integer division rounds towards zero. For index math, we
@@ -47,15 +46,13 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
 
 }  // namespace
 
-PrecomputationGrid ConvertToPrecomputationGrid(
+PrecomputationGrid3D ConvertToPrecomputationGrid(
-    const mapping::HybridGrid& hybrid_grid) {
+    const HybridGrid& hybrid_grid) {
-  PrecomputationGrid result(hybrid_grid.resolution());
+  PrecomputationGrid3D result(hybrid_grid.resolution());
-  for (auto it = mapping::HybridGrid::Iterator(hybrid_grid); !it.Done();
+  for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
-       it.Next()) {
     const int cell_value = common::RoundToInt(
-        (mapping::ValueToProbability(it.GetValue()) -
+        (ValueToProbability(it.GetValue()) - kMinProbability) *
-         mapping::kMinProbability) *
+        (255.f / (kMaxProbability - kMinProbability)));
-        (255.f / (mapping::kMaxProbability - mapping::kMinProbability)));
     CHECK_GE(cell_value, 0);
     CHECK_LE(cell_value, 255);
     *result.mutable_value(it.GetCellIndex()) = cell_value;
@@ -63,18 +60,18 @@ PrecomputationGrid ConvertToPrecomputationGrid(
   return result;
 }
 
-PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
+PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid,
-                                  const bool half_resolution,
+                                    const bool half_resolution,
-                                  const Eigen::Array3i& shift) {
+                                    const Eigen::Array3i& shift) {
-  PrecomputationGrid result(grid.resolution());
+  PrecomputationGrid3D result(grid.resolution());
-  for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) {
+  for (auto it = PrecomputationGrid3D::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
       // position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}).
       // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid,
       // this results in precomputation grids analogous to the 2D case.
       const Eigen::Array3i cell_index =
-          it.GetCellIndex() - shift * PrecomputationGrid::GetOctant(i);
+          it.GetCellIndex() - shift * PrecomputationGrid3D::GetOctant(i);
       auto* const cell_value = result.mutable_value(
           half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index);
       *cell_value = std::max(it.GetValue(), *cell_value);
@@ -84,5 +81,5 @@ PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
 }
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h

@@ -14,32 +14,30 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_
 
 #include "cartographer/mapping_3d/hybrid_grid.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-class PrecomputationGrid : public mapping::HybridGridBase<uint8> {
+class PrecomputationGrid3D : public HybridGridBase<uint8> {
  public:
-  explicit PrecomputationGrid(const float resolution)
+  explicit PrecomputationGrid3D(const float resolution)
-      : mapping::HybridGridBase<uint8>(resolution) {}
+      : HybridGridBase<uint8>(resolution) {}
 
   // Maps values from [0, 255] to [kMinProbability, kMaxProbability].
   static float ToProbability(float value) {
-    return mapping::kMinProbability +
+    return kMinProbability +
-           value *
+           value * ((kMaxProbability - kMinProbability) / 255.f);
-               ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f);
   }
 };
 
-// Converts a HybridGrid to a PrecomputationGrid representing the same data,
+// Converts a HybridGrid to a PrecomputationGrid3D representing the same data,
 // but only using 8 bit instead of 2 x 16 bit.
-PrecomputationGrid ConvertToPrecomputationGrid(
+PrecomputationGrid3D ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid);
-    const mapping::HybridGrid& hybrid_grid);
 
 // Returns a grid of the same resolution containing the maximum value of
 // original voxels in 'grid'. This maximum is over the 8 voxels that have
@@ -47,12 +45,12 @@ PrecomputationGrid ConvertToPrecomputationGrid(
 // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this
 // is using the precomputed grid of one depth before, this results in
 // precomputation grids analogous to the 2D case.
-PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
+PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D& grid,
-                                  bool half_resolution,
+                                    bool half_resolution,
-                                  const Eigen::Array3i& shift);
+                                    const Eigen::Array3i& shift);
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_3D_H_

cartographer/mapping_3d/scan_matching/precomputation_grid_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
+#include "cartographer/mapping_3d/scan_matching/precomputation_grid_3d.h"
 
 #include <random>
 #include <tuple>
@@ -24,17 +24,17 @@
 #include "gmock/gmock.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
-TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
+TEST(PrecomputedGridGenerator3DTest, TestAgainstNaiveAlgorithm) {
-  mapping::HybridGrid hybrid_grid(2.f);
+  HybridGrid hybrid_grid(2.f);
 
   std::mt19937 rng(23847);
   std::uniform_int_distribution<int> coordinate_distribution(-50, 49);
-  std::uniform_real_distribution<float> value_distribution(
+  std::uniform_real_distribution<float> value_distribution(kMinProbability,
-      mapping::kMinProbability, mapping::kMaxProbability);
+                                                           kMaxProbability);
   for (int i = 0; i < 1000; ++i) {
     const auto x = coordinate_distribution(rng);
     const auto y = coordinate_distribution(rng);
@@ -43,7 +43,7 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
     hybrid_grid.SetProbability(cell_index, value_distribution(rng));
   }
 
-  std::vector<PrecomputationGrid> precomputed_grids;
+  std::vector<PrecomputationGrid3D> precomputed_grids;
   for (int depth = 0; depth <= 3; ++depth) {
     if (depth == 0) {
       precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid));
@@ -69,7 +69,7 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
       }
 
       EXPECT_NEAR(max_probability,
-                  PrecomputationGrid::ToProbability(
+                  PrecomputationGrid3D::ToProbability(
                       precomputed_grids.back().value(Eigen::Array3i(x, y, z))),
                   1e-2);
     }
@@ -78,5 +78,5 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options_3d.proto

@@ -14,12 +14,12 @@
 
 syntax = "proto3";
 
-package cartographer.mapping_3d.scan_matching.proto;
+package cartographer.mapping.scan_matching.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
 // NEXT ID: 7
-message CeresScanMatcherOptions {
+message CeresScanMatcherOptions3D {
   // Scaling parameters for each cost functor.
   repeated double occupied_space_weight = 1;
   double translation_weight = 2;

cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.proto

@@ -14,9 +14,9 @@
 
 syntax = "proto3";
 
-package cartographer.mapping_3d.scan_matching.proto;
+package cartographer.mapping.scan_matching.proto;
 
-message FastCorrelativeScanMatcherOptions {
+message FastCorrelativeScanMatcherOptions3D {
   // Number of precomputed grids to use.
   int32 branch_and_bound_depth = 2;
 

cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
 
 #include <cmath>
 
@@ -24,18 +24,16 @@
 #include "glog/logging.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher(
+RealTimeCorrelativeScanMatcher3D::RealTimeCorrelativeScanMatcher3D(
-    const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
+    const proto::RealTimeCorrelativeScanMatcherOptions& options)
-        options)
     : options_(options) {}
 
-float RealTimeCorrelativeScanMatcher::Match(
+float RealTimeCorrelativeScanMatcher3D::Match(
     const transform::Rigid3d& initial_pose_estimate,
-    const sensor::PointCloud& point_cloud,
+    const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid,
-    const mapping::HybridGrid& hybrid_grid,
     transform::Rigid3d* pose_estimate) const {
   CHECK_NOTNULL(pose_estimate);
   float best_score = -1.f;
@@ -55,7 +53,7 @@ float RealTimeCorrelativeScanMatcher::Match(
 }
 
 std::vector<transform::Rigid3f>
-RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
+RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms(
     const float resolution, const sensor::PointCloud& point_cloud) const {
   std::vector<transform::Rigid3f> result;
   const int linear_window_size =
@@ -96,8 +94,8 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
   return result;
 }
 
-float RealTimeCorrelativeScanMatcher::ScoreCandidate(
+float RealTimeCorrelativeScanMatcher3D::ScoreCandidate(
-    const mapping::HybridGrid& hybrid_grid,
+    const HybridGrid& hybrid_grid,
     const sensor::PointCloud& transformed_point_cloud,
     const transform::Rigid3f& transform) const {
   float score = 0.f;
@@ -115,5 +113,5 @@ float RealTimeCorrelativeScanMatcher::ScoreCandidate(
 }
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h

@@ -14,10 +14,8 @@
  * limitations under the License.
  */
 
-// A voxel accurate scan matcher, exhaustively evaluating the scan matching
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_
-// search space.
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_
-#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
-#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
 
 #include <vector>
 
@@ -27,41 +25,42 @@
 #include "cartographer/sensor/point_cloud.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
-class RealTimeCorrelativeScanMatcher {
+// A voxel accurate scan matcher, exhaustively evaluating the scan matching
+// search space.
+class RealTimeCorrelativeScanMatcher3D {
  public:
-  explicit RealTimeCorrelativeScanMatcher(
+  explicit RealTimeCorrelativeScanMatcher3D(
-      const mapping::scan_matching::proto::
+      const scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
-          RealTimeCorrelativeScanMatcherOptions& options);
+          options);
 
-  RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) =
+  RealTimeCorrelativeScanMatcher3D(const RealTimeCorrelativeScanMatcher3D&) =
       delete;
-  RealTimeCorrelativeScanMatcher& operator=(
+  RealTimeCorrelativeScanMatcher3D& operator=(
-      const RealTimeCorrelativeScanMatcher&) = delete;
+      const RealTimeCorrelativeScanMatcher3D&) = delete;
 
   // Aligns 'point_cloud' within the 'hybrid_grid' given an
   // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
   // returns the score.
   float Match(const transform::Rigid3d& initial_pose_estimate,
               const sensor::PointCloud& point_cloud,
-              const mapping::HybridGrid& hybrid_grid,
+              const HybridGrid& hybrid_grid,
               transform::Rigid3d* pose_estimate) const;
 
  private:
   std::vector<transform::Rigid3f> GenerateExhaustiveSearchTransforms(
       float resolution, const sensor::PointCloud& point_cloud) const;
-  float ScoreCandidate(const mapping::HybridGrid& hybrid_grid,
+  float ScoreCandidate(const HybridGrid& hybrid_grid,
                        const sensor::PointCloud& transformed_point_cloud,
                        const transform::Rigid3f& transform) const;
 
-  const mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions
+  const proto::RealTimeCorrelativeScanMatcherOptions options_;
-      options_;
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_3D_H_

cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
 
 #include <memory>
 
@@ -29,13 +29,13 @@
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
-class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
+class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test {
  protected:
-  RealTimeCorrelativeScanMatcherTest()
+  RealTimeCorrelativeScanMatcher3DTest()
       : hybrid_grid_(0.1f),
         expected_pose_(Eigen::Vector3d(-1., 0., 0.),
                        Eigen::Quaterniond::Identity()) {
@@ -57,8 +57,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
           rotation_delta_cost_weight = 1.,
         })text");
     real_time_correlative_scan_matcher_.reset(
-        new RealTimeCorrelativeScanMatcher(
+        new RealTimeCorrelativeScanMatcher3D(
-            mapping::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
+            CreateRealTimeCorrelativeScanMatcherOptions(
                 parameter_dictionary.get())));
   }
 
@@ -71,46 +71,46 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
     EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3));
   }
 
-  mapping::HybridGrid hybrid_grid_;
+  HybridGrid hybrid_grid_;
   transform::Rigid3d expected_pose_;
   sensor::PointCloud point_cloud_;
-  std::unique_ptr<RealTimeCorrelativeScanMatcher>
+  std::unique_ptr<RealTimeCorrelativeScanMatcher3D>
       real_time_correlative_scan_matcher_;
 };
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, PerfectEstimate) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
 }
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongX) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
 }
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongZ) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
 }
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongXYZ) {
   TestFromInitialPose(
       transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
 }
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundX) {
   TestFromInitialPose(transform::Rigid3d(
       Eigen::Vector3d(-1., 0., 0.),
       Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
 }
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundY) {
   TestFromInitialPose(transform::Rigid3d(
       Eigen::Vector3d(-1., 0., 0.),
       Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
 }
 
-TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
+TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundYZ) {
   TestFromInitialPose(transform::Rigid3d(
       Eigen::Vector3d(-1., 0., 0.),
       Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
@@ -118,5 +118,5 @@ TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_
 
 #include <cmath>
 
@@ -24,19 +24,19 @@
 #include "ceres/rotation.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 // Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
 // Cost increases with the solution's distance from 'target_rotation'.
-class RotationDeltaCostFunctor {
+class RotationDeltaCostFunctor3D {
  public:
   static ceres::CostFunction* CreateAutoDiffCostFunction(
       const double scaling_factor, const Eigen::Quaterniond& target_rotation) {
-    return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor,
+    return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor3D,
                                            3 /* residuals */,
                                            4 /* rotation variables */>(
-        new RotationDeltaCostFunctor(scaling_factor, target_rotation));
+        new RotationDeltaCostFunctor3D(scaling_factor, target_rotation));
   }
 
   template <typename T>
@@ -56,9 +56,10 @@ class RotationDeltaCostFunctor {
   }
 
  private:
-  // Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
+  // Constructs a new RotationDeltaCostFunctor3D from the given
-  explicit RotationDeltaCostFunctor(const double scaling_factor,
+  // 'target_rotation'.
-                                    const Eigen::Quaterniond& target_rotation)
+  explicit RotationDeltaCostFunctor3D(const double scaling_factor,
+                                      const Eigen::Quaterniond& target_rotation)
       : scaling_factor_(scaling_factor) {
     target_rotation_inverse_[0] = target_rotation.w();
     target_rotation_inverse_[1] = -target_rotation.x();
@@ -66,15 +67,16 @@ class RotationDeltaCostFunctor {
     target_rotation_inverse_[3] = -target_rotation.z();
   }
 
-  RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
+  RotationDeltaCostFunctor3D(const RotationDeltaCostFunctor3D&) = delete;
-  RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
+  RotationDeltaCostFunctor3D& operator=(const RotationDeltaCostFunctor3D&) =
+      delete;
 
   const double scaling_factor_;
   double target_rotation_inverse_[4];
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_

cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d_test.cc

@@ -14,12 +14,12 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h"
+#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor_3d.h"
 
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
@@ -29,8 +29,8 @@ double ComputeRotationDeltaSquaredCost(
     const Eigen::Quaterniond& rotation, const double scaling_factor,
     const Eigen::Quaterniond& target_rotation) {
   std::unique_ptr<ceres::CostFunction> cost_function(
-      RotationDeltaCostFunctor::CreateAutoDiffCostFunction(scaling_factor,
+      RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(scaling_factor,
-                                                           target_rotation));
+                                                             target_rotation));
   const std::array<double, 4> parameter_quaternion = {
       {rotation.w(), rotation.x(), rotation.y(), rotation.z()}};
   const std::vector<const double*> parameters = {parameter_quaternion.data()};
@@ -44,7 +44,7 @@ double ComputeRotationDeltaSquaredCost(
   return sum_of_squares;
 }
 
-TEST(RotationDeltaCostFunctorTest, SameRotationGivesZeroCost) {
+TEST(RotationDeltaCostFunctor3DTest, SameRotationGivesZeroCost) {
   EXPECT_NEAR(
       0.,
       ComputeRotationDeltaSquaredCost(Eigen::Quaterniond::Identity(), 1.0,
@@ -57,7 +57,7 @@ TEST(RotationDeltaCostFunctorTest, SameRotationGivesZeroCost) {
               kPrecision);
 }
 
-TEST(RotationDeltaCostFunctorTest, ComputesCorrectCost) {
+TEST(RotationDeltaCostFunctor3DTest, ComputesCorrectCost) {
   double scaling_factor = 1.2;
   double angle = 0.8;
   Eigen::Quaterniond rotation(
@@ -81,5 +81,5 @@ TEST(RotationDeltaCostFunctorTest, ComputesCorrectCost) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc

@@ -23,7 +23,7 @@
 #include "cartographer/common/port.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 namespace {
@@ -190,5 +190,5 @@ std::vector<float> RotationalScanMatcher::Match(
 }
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h

@@ -23,7 +23,7 @@
 #include "cartographer/sensor/point_cloud.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 class RotationalScanMatcher {
@@ -51,7 +51,7 @@ class RotationalScanMatcher {
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
 #endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_

cartographer/mapping_3d/scan_matching/rotational_scan_matcher_test.cc

@@ -21,11 +21,11 @@
 #include "gtest/gtest.h"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 namespace {
 
-TEST(RotationalScanMatcherTest, OnlySameHistogramIsScoreOne) {
+TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) {
   Eigen::VectorXf histogram(7);
   histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f;
   RotationalScanMatcher matcher({{histogram, 0.f}});
@@ -35,7 +35,7 @@ TEST(RotationalScanMatcherTest, OnlySameHistogramIsScoreOne) {
   EXPECT_GT(1.f, scores[1]);
 }
 
-TEST(RotationalScanMatcherTest, InterpolatesAsExpected) {
+TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) {
   constexpr int kNumBuckets = 10;
   constexpr float kAnglePerBucket = M_PI / kNumBuckets;
   constexpr float kNoInitialRotation = 0.f;
@@ -67,5 +67,5 @@ TEST(RotationalScanMatcherTest, InterpolatesAsExpected) {
 
 }  // namespace
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_3d/scan_matching/translation_delta_cost_functor_3d.h

@@ -14,25 +14,25 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_
-#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_
 
 #include "Eigen/Core"
 
 namespace cartographer {
-namespace mapping_3d {
+namespace mapping {
 namespace scan_matching {
 
 // Computes the cost of translating 'translation' to 'target_translation'.
 // Cost increases with the solution's distance from 'target_translation'.
-class TranslationDeltaCostFunctor {
+class TranslationDeltaCostFunctor3D {
  public:
   static ceres::CostFunction* CreateAutoDiffCostFunction(
       const double scaling_factor, const Eigen::Vector3d& target_translation) {
-    return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor,
+    return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor3D,
                                            3 /* residuals */,
                                            3 /* translation variables */>(
-        new TranslationDeltaCostFunctor(scaling_factor, target_translation));
+        new TranslationDeltaCostFunctor3D(scaling_factor, target_translation));
   }
 
   template <typename T>
@@ -44,18 +44,18 @@ class TranslationDeltaCostFunctor {
   }
 
  private:
-  // Constructs a new TranslationDeltaCostFunctor from the given
+  // Constructs a new TranslationDeltaCostFunctor3D from the given
   // 'target_translation'.
-  explicit TranslationDeltaCostFunctor(
+  explicit TranslationDeltaCostFunctor3D(
       const double scaling_factor, const Eigen::Vector3d& target_translation)
       : scaling_factor_(scaling_factor),
         x_(target_translation.x()),
         y_(target_translation.y()),
         z_(target_translation.z()) {}
 
-  TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
+  TranslationDeltaCostFunctor3D(const TranslationDeltaCostFunctor3D&) = delete;
-  TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
+  TranslationDeltaCostFunctor3D& operator=(
-      delete;
+      const TranslationDeltaCostFunctor3D&) = delete;
 
   const double scaling_factor_;
   const double x_;
@@ -64,7 +64,7 @@ class TranslationDeltaCostFunctor {
 };
 
 }  // namespace scan_matching
-}  // namespace mapping_3d
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_

cartographer/metrics/register.cc

@@ -17,14 +17,14 @@
 #include "cartographer/metrics/register.h"
 
 #include "cartographer/mapping_2d/pose_graph/constraint_builder_2d.h"
-#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
+#include "cartographer/mapping_3d/pose_graph/constraint_builder_3d.h"
 
 namespace cartographer {
 namespace metrics {
 
 void RegisterAllMetrics(FamilyFactory* registry) {
   mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry);
-  mapping_3d::pose_graph::ConstraintBuilder::RegisterMetrics(registry);
+  mapping::pose_graph::ConstraintBuilder3D::RegisterMetrics(registry);
 }
 
 }  // namespace metrics