Rename the KalmanLocalTrajectoryBuilder. (#372)

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -16,8 +16,6 @@
 
 #include "cartographer/mapping_3d/global_trajectory_builder.h"
 
-#include "cartographer/mapping_3d/local_trajectory_builder.h"
-
 namespace cartographer {
 namespace mapping_3d {
 
@@ -26,14 +24,14 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
     const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
     : trajectory_id_(trajectory_id),
       sparse_pose_graph_(sparse_pose_graph),
-      local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {}
+      local_trajectory_builder_(options) {}
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
 void GlobalTrajectoryBuilder::AddImuData(
     const common::Time time, const Eigen::Vector3d& linear_acceleration,
     const Eigen::Vector3d& angular_velocity) {
-  local_trajectory_builder_->AddImuData(time, linear_acceleration,
+  local_trajectory_builder_.AddImuData(time, linear_acceleration,
                                        angular_velocity);
   sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
                                  angular_velocity);
@@ -43,7 +41,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
     const sensor::PointCloud& ranges) {
   auto insertion_result =
-      local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
+      local_trajectory_builder_.AddRangefinderData(time, origin, ranges);
   if (insertion_result == nullptr) {
     return;
   }
@@ -55,12 +53,12 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
                                               const transform::Rigid3d& pose) {
-  local_trajectory_builder_->AddOdometerData(time, pose);
+  local_trajectory_builder_.AddOdometerData(time, pose);
 }
 
 const GlobalTrajectoryBuilder::PoseEstimate&
 GlobalTrajectoryBuilder::pose_estimate() const {
-  return local_trajectory_builder_->pose_estimate();
+  return local_trajectory_builder_.pose_estimate();
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/global_trajectory_builder.h

@@ -17,10 +17,8 @@
 #ifndef CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
 #define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
 
-#include <memory>
-
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
+#include "cartographer/mapping_3d/local_trajectory_builder.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/sparse_pose_graph.h"
 
@@ -49,7 +47,7 @@ class GlobalTrajectoryBuilder
  private:
   const int trajectory_id_;
   mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
-  std::unique_ptr<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
+  LocalTrajectoryBuilder local_trajectory_builder_;
 };
 
 }  // namespace mapping_3d

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -1,225 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
-
-#include "cartographer/common/make_unique.h"
-#include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
-#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
-#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
-#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
-#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
-#include "glog/logging.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder(
-    const proto::LocalTrajectoryBuilderOptions& options)
-    : options_(options),
-      active_submaps_(options.submaps_options()),
-      scan_matcher_pose_estimate_(transform::Rigid3d::Identity()),
-      motion_filter_(options.motion_filter_options()),
-      real_time_correlative_scan_matcher_(
-          common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
-              options_.kalman_local_trajectory_builder_options()
-                  .real_time_correlative_scan_matcher_options())),
-      ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
-          options_.ceres_scan_matcher_options())),
-      num_accumulated_(0),
-      first_pose_prediction_(transform::Rigid3f::Identity()),
-      accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
-
-KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
-
-void KalmanLocalTrajectoryBuilder::AddImuData(
-    const common::Time time, const Eigen::Vector3d& linear_acceleration,
-    const Eigen::Vector3d& angular_velocity) {
-  if (!pose_tracker_) {
-    pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
-        options_.kalman_local_trajectory_builder_options()
-            .pose_tracker_options(),
-        time);
-  }
-
-  pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
-  pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
-}
-
-std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
-KalmanLocalTrajectoryBuilder::AddRangefinderData(
-    const common::Time time, const Eigen::Vector3f& origin,
-    const sensor::PointCloud& ranges) {
-  if (!pose_tracker_) {
-    LOG(INFO) << "PoseTracker not yet initialized.";
-    return nullptr;
-  }
-
-  const transform::Rigid3d pose_prediction =
-      pose_tracker_->GetPoseEstimateMean(time);
-  if (num_accumulated_ == 0) {
-    first_pose_prediction_ = pose_prediction.cast<float>();
-    accumulated_range_data_ =
-        sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
-  }
-
-  const transform::Rigid3f tracking_delta =
-      first_pose_prediction_.inverse() * pose_prediction.cast<float>();
-  const sensor::RangeData range_data_in_first_tracking =
-      sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
-                                 tracking_delta);
-  for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
-    const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
-    const float range = delta.norm();
-    if (range >= options_.min_range()) {
-      if (range <= options_.max_range()) {
-        accumulated_range_data_.returns.push_back(hit);
-      } else {
-        // We insert a ray cropped to 'max_range' as a miss for hits beyond the
-        // maximum range. This way the free space up to the maximum range will
-        // be updated.
-        accumulated_range_data_.misses.push_back(
-            range_data_in_first_tracking.origin +
-            options_.max_range() / range * delta);
-      }
-    }
-  }
-  ++num_accumulated_;
-
-  if (num_accumulated_ >= options_.scans_per_accumulation()) {
-    num_accumulated_ = 0;
-    return AddAccumulatedRangeData(
-        time, sensor::TransformRangeData(accumulated_range_data_,
-                                         tracking_delta.inverse()));
-  }
-  return nullptr;
-}
-
-std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
-KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
-    const common::Time time, const sensor::RangeData& range_data_in_tracking) {
-  const sensor::RangeData filtered_range_data = {
-      range_data_in_tracking.origin,
-      sensor::VoxelFiltered(range_data_in_tracking.returns,
-                            options_.voxel_filter_size()),
-      sensor::VoxelFiltered(range_data_in_tracking.misses,
-                            options_.voxel_filter_size())};
-
-  if (filtered_range_data.returns.empty()) {
-    LOG(WARNING) << "Dropped empty range data.";
-    return nullptr;
-  }
-
-  const transform::Rigid3d pose_prediction =
-      pose_tracker_->GetPoseEstimateMean(time);
-
-  std::shared_ptr<const Submap> matching_submap =
-      active_submaps_.submaps().front();
-  transform::Rigid3d initial_ceres_pose =
-      matching_submap->local_pose().inverse() * pose_prediction;
-  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
-      options_.high_resolution_adaptive_voxel_filter_options());
-  const sensor::PointCloud filtered_point_cloud_in_tracking =
-      adaptive_voxel_filter.Filter(filtered_range_data.returns);
-  if (options_.kalman_local_trajectory_builder_options()
-          .use_online_correlative_scan_matching()) {
-    // We take a copy since we use 'intial_ceres_pose' as an output argument.
-    const transform::Rigid3d initial_pose = initial_ceres_pose;
-    real_time_correlative_scan_matcher_->Match(
-        initial_pose, filtered_point_cloud_in_tracking,
-        matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
-  }
-
-  transform::Rigid3d pose_observation_in_submap;
-  ceres::Solver::Summary summary;
-
-  sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
-      options_.low_resolution_adaptive_voxel_filter_options());
-  const sensor::PointCloud low_resolution_point_cloud_in_tracking =
-      low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
-  ceres_scan_matcher_->Match(
-      matching_submap->local_pose().inverse() * scan_matcher_pose_estimate_,
-      initial_ceres_pose,
-      {{&filtered_point_cloud_in_tracking,
-        &matching_submap->high_resolution_hybrid_grid()},
-       {&low_resolution_point_cloud_in_tracking,
-        &matching_submap->low_resolution_hybrid_grid()}},
-      &pose_observation_in_submap, &summary);
-  const transform::Rigid3d pose_observation =
-      matching_submap->local_pose() * pose_observation_in_submap;
-  pose_tracker_->AddPoseObservation(
-      time, pose_observation,
-      options_.kalman_local_trajectory_builder_options()
-              .scan_matcher_variance() *
-          kalman_filter::PoseCovariance::Identity());
-
-  scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time);
-
-  last_pose_estimate_ = {
-      time, scan_matcher_pose_estimate_,
-      sensor::TransformPointCloud(filtered_range_data.returns,
-                                  pose_observation.cast<float>())};
-
-  return InsertIntoSubmap(time, filtered_range_data, pose_observation);
-}
-
-void KalmanLocalTrajectoryBuilder::AddOdometerData(
-    const common::Time time, const transform::Rigid3d& pose) {
-  if (!pose_tracker_) {
-    pose_tracker_.reset(new kalman_filter::PoseTracker(
-        options_.kalman_local_trajectory_builder_options()
-            .pose_tracker_options(),
-        time));
-  }
-  pose_tracker_->AddOdometerPoseObservation(
-      time, pose,
-      kalman_filter::BuildPoseCovariance(
-          options_.kalman_local_trajectory_builder_options()
-              .odometer_translational_variance(),
-          options_.kalman_local_trajectory_builder_options()
-              .odometer_rotational_variance()));
-}
-
-const KalmanLocalTrajectoryBuilder::PoseEstimate&
-KalmanLocalTrajectoryBuilder::pose_estimate() const {
-  return last_pose_estimate_;
-}
-
-std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
-KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
-    const common::Time time, const sensor::RangeData& range_data_in_tracking,
-    const transform::Rigid3d& pose_observation) {
-  if (motion_filter_.IsSimilar(time, pose_observation)) {
-    return nullptr;
-  }
-  // Querying the active submaps must be done here before calling
-  // InsertRangeData() since the queried values are valid for next insertion.
-  std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-  for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
-    insertion_submaps.push_back(submap);
-  }
-  active_submaps_.InsertRangeData(
-      sensor::TransformRangeData(range_data_in_tracking,
-                                 pose_observation.cast<float>()),
-      pose_tracker_->gravity_orientation());
-  return std::unique_ptr<InsertionResult>(
-      new InsertionResult{time, range_data_in_tracking, pose_observation,
-                          std::move(insertion_submaps)});
-}
-
-}  // namespace mapping_3d
-}  // namespace cartographer

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -1,97 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_
-#define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_
-
-#include <memory>
-
-#include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
-#include "cartographer/mapping/global_trajectory_builder_interface.h"
-#include "cartographer/mapping_3d/motion_filter.h"
-#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
-#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
-#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
-#include "cartographer/mapping_3d/submaps.h"
-#include "cartographer/sensor/range_data.h"
-#include "cartographer/sensor/voxel_filter.h"
-#include "cartographer/transform/rigid_transform.h"
-
-namespace cartographer {
-namespace mapping_3d {
-
-// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
-// closure.
-class KalmanLocalTrajectoryBuilder {
- public:
-  using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
-
-  struct InsertionResult {
-    common::Time time;
-    sensor::RangeData range_data_in_tracking;
-    transform::Rigid3d pose_observation;
-    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-  };
-
-  explicit KalmanLocalTrajectoryBuilder(
-      const proto::LocalTrajectoryBuilderOptions& options);
-  ~KalmanLocalTrajectoryBuilder();
-
-  KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete;
-  KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) =
-      delete;
-
-  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
-                  const Eigen::Vector3d& angular_velocity);
-  std::unique_ptr<InsertionResult> AddRangefinderData(
-      common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges);
-  void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
-  const PoseEstimate& pose_estimate() const;
-
- private:
-  std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
-      common::Time time, const sensor::RangeData& range_data_in_tracking);
-
-  std::unique_ptr<InsertionResult> InsertIntoSubmap(
-      common::Time time, const sensor::RangeData& range_data_in_tracking,
-      const transform::Rigid3d& pose_observation);
-
-  const proto::LocalTrajectoryBuilderOptions options_;
-  mapping_3d::ActiveSubmaps active_submaps_;
-
-  PoseEstimate last_pose_estimate_;
-
-  // Pose of the last computed scan match.
-  transform::Rigid3d scan_matcher_pose_estimate_;
-
-  MotionFilter motion_filter_;
-  std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>
-      real_time_correlative_scan_matcher_;
-  std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
-
-  std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
-
-  int num_accumulated_;
-  transform::Rigid3f first_pose_prediction_;
-  sensor::RangeData accumulated_range_data_;
-};
-
-}  // namespace mapping_3d
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -17,15 +17,208 @@
 #include "cartographer/mapping_3d/local_trajectory_builder.h"
 
 #include "cartographer/common/make_unique.h"
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
+#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
+#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
+#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
+#include "glog/logging.h"
 
 namespace cartographer {
 namespace mapping_3d {
 
-std::unique_ptr<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
+LocalTrajectoryBuilder::LocalTrajectoryBuilder(
-    const proto::LocalTrajectoryBuilderOptions&
+    const proto::LocalTrajectoryBuilderOptions& options)
-        local_trajectory_builder_options) {
+    : options_(options),
-  return common::make_unique<KalmanLocalTrajectoryBuilder>(
+      active_submaps_(options.submaps_options()),
-      local_trajectory_builder_options);
+      scan_matcher_pose_estimate_(transform::Rigid3d::Identity()),
+      motion_filter_(options.motion_filter_options()),
+      real_time_correlative_scan_matcher_(
+          common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
+              options_.kalman_local_trajectory_builder_options()
+                  .real_time_correlative_scan_matcher_options())),
+      ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
+          options_.ceres_scan_matcher_options())),
+      num_accumulated_(0),
+      first_pose_prediction_(transform::Rigid3f::Identity()),
+      accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
+
+LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
+
+void LocalTrajectoryBuilder::AddImuData(
+    const common::Time time, const Eigen::Vector3d& linear_acceleration,
+    const Eigen::Vector3d& angular_velocity) {
+  if (!pose_tracker_) {
+    pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
+        options_.kalman_local_trajectory_builder_options()
+            .pose_tracker_options(),
+        time);
+  }
+
+  pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
+  pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
+}
+
+std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
+                                           const Eigen::Vector3f& origin,
+                                           const sensor::PointCloud& ranges) {
+  if (!pose_tracker_) {
+    LOG(INFO) << "PoseTracker not yet initialized.";
+    return nullptr;
+  }
+
+  const transform::Rigid3d pose_prediction =
+      pose_tracker_->GetPoseEstimateMean(time);
+  if (num_accumulated_ == 0) {
+    first_pose_prediction_ = pose_prediction.cast<float>();
+    accumulated_range_data_ =
+        sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
+  }
+
+  const transform::Rigid3f tracking_delta =
+      first_pose_prediction_.inverse() * pose_prediction.cast<float>();
+  const sensor::RangeData range_data_in_first_tracking =
+      sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
+                                 tracking_delta);
+  for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
+    const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
+    const float range = delta.norm();
+    if (range >= options_.min_range()) {
+      if (range <= options_.max_range()) {
+        accumulated_range_data_.returns.push_back(hit);
+      } else {
+        // We insert a ray cropped to 'max_range' as a miss for hits beyond the
+        // maximum range. This way the free space up to the maximum range will
+        // be updated.
+        accumulated_range_data_.misses.push_back(
+            range_data_in_first_tracking.origin +
+            options_.max_range() / range * delta);
+      }
+    }
+  }
+  ++num_accumulated_;
+
+  if (num_accumulated_ >= options_.scans_per_accumulation()) {
+    num_accumulated_ = 0;
+    return AddAccumulatedRangeData(
+        time, sensor::TransformRangeData(accumulated_range_data_,
+                                         tracking_delta.inverse()));
+  }
+  return nullptr;
+}
+
+std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+LocalTrajectoryBuilder::AddAccumulatedRangeData(
+    const common::Time time, const sensor::RangeData& range_data_in_tracking) {
+  const sensor::RangeData filtered_range_data = {
+      range_data_in_tracking.origin,
+      sensor::VoxelFiltered(range_data_in_tracking.returns,
+                            options_.voxel_filter_size()),
+      sensor::VoxelFiltered(range_data_in_tracking.misses,
+                            options_.voxel_filter_size())};
+
+  if (filtered_range_data.returns.empty()) {
+    LOG(WARNING) << "Dropped empty range data.";
+    return nullptr;
+  }
+
+  const transform::Rigid3d pose_prediction =
+      pose_tracker_->GetPoseEstimateMean(time);
+
+  std::shared_ptr<const Submap> matching_submap =
+      active_submaps_.submaps().front();
+  transform::Rigid3d initial_ceres_pose =
+      matching_submap->local_pose().inverse() * pose_prediction;
+  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
+      options_.high_resolution_adaptive_voxel_filter_options());
+  const sensor::PointCloud filtered_point_cloud_in_tracking =
+      adaptive_voxel_filter.Filter(filtered_range_data.returns);
+  if (options_.kalman_local_trajectory_builder_options()
+          .use_online_correlative_scan_matching()) {
+    // We take a copy since we use 'intial_ceres_pose' as an output argument.
+    const transform::Rigid3d initial_pose = initial_ceres_pose;
+    real_time_correlative_scan_matcher_->Match(
+        initial_pose, filtered_point_cloud_in_tracking,
+        matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
+  }
+
+  transform::Rigid3d pose_observation_in_submap;
+  ceres::Solver::Summary summary;
+
+  sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
+      options_.low_resolution_adaptive_voxel_filter_options());
+  const sensor::PointCloud low_resolution_point_cloud_in_tracking =
+      low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
+  ceres_scan_matcher_->Match(
+      matching_submap->local_pose().inverse() * scan_matcher_pose_estimate_,
+      initial_ceres_pose,
+      {{&filtered_point_cloud_in_tracking,
+        &matching_submap->high_resolution_hybrid_grid()},
+       {&low_resolution_point_cloud_in_tracking,
+        &matching_submap->low_resolution_hybrid_grid()}},
+      &pose_observation_in_submap, &summary);
+  const transform::Rigid3d pose_observation =
+      matching_submap->local_pose() * pose_observation_in_submap;
+  pose_tracker_->AddPoseObservation(
+      time, pose_observation,
+      options_.kalman_local_trajectory_builder_options()
+              .scan_matcher_variance() *
+          kalman_filter::PoseCovariance::Identity());
+
+  scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time);
+
+  last_pose_estimate_ = {
+      time, scan_matcher_pose_estimate_,
+      sensor::TransformPointCloud(filtered_range_data.returns,
+                                  pose_observation.cast<float>())};
+
+  return InsertIntoSubmap(time, filtered_range_data, pose_observation);
+}
+
+void LocalTrajectoryBuilder::AddOdometerData(const common::Time time,
+                                             const transform::Rigid3d& pose) {
+  if (!pose_tracker_) {
+    pose_tracker_.reset(new kalman_filter::PoseTracker(
+        options_.kalman_local_trajectory_builder_options()
+            .pose_tracker_options(),
+        time));
+  }
+  pose_tracker_->AddOdometerPoseObservation(
+      time, pose,
+      kalman_filter::BuildPoseCovariance(
+          options_.kalman_local_trajectory_builder_options()
+              .odometer_translational_variance(),
+          options_.kalman_local_trajectory_builder_options()
+              .odometer_rotational_variance()));
+}
+
+const LocalTrajectoryBuilder::PoseEstimate&
+LocalTrajectoryBuilder::pose_estimate() const {
+  return last_pose_estimate_;
+}
+
+std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+LocalTrajectoryBuilder::InsertIntoSubmap(
+    const common::Time time, const sensor::RangeData& range_data_in_tracking,
+    const transform::Rigid3d& pose_observation) {
+  if (motion_filter_.IsSimilar(time, pose_observation)) {
+    return nullptr;
+  }
+  // Querying the active submaps must be done here before calling
+  // InsertRangeData() since the queried values are valid for next insertion.
+  std::vector<std::shared_ptr<const Submap>> insertion_submaps;
+  for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
+    insertion_submaps.push_back(submap);
+  }
+  active_submaps_.InsertRangeData(
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      pose_tracker_->gravity_orientation());
+  return std::unique_ptr<InsertionResult>(
+      new InsertionResult{time, range_data_in_tracking, pose_observation,
+                          std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/local_trajectory_builder.h

@@ -18,17 +18,77 @@
 #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
 
 #include <memory>
-#include <vector>
 
-#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
+#include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping_3d/motion_filter.h"
 #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/submaps.h"
+#include "cartographer/sensor/range_data.h"
+#include "cartographer/sensor/voxel_filter.h"
+#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping_3d {
 
-std::unique_ptr<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
+// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
-    const proto::LocalTrajectoryBuilderOptions&
+// closure.
-        local_trajectory_builder_options);
+class LocalTrajectoryBuilder {
+ public:
+  using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
+
+  struct InsertionResult {
+    common::Time time;
+    sensor::RangeData range_data_in_tracking;
+    transform::Rigid3d pose_observation;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
+  };
+
+  explicit LocalTrajectoryBuilder(
+      const proto::LocalTrajectoryBuilderOptions& options);
+  ~LocalTrajectoryBuilder();
+
+  LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
+  LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
+
+  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity);
+  std::unique_ptr<InsertionResult> AddRangefinderData(
+      common::Time time, const Eigen::Vector3f& origin,
+      const sensor::PointCloud& ranges);
+  void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
+  const PoseEstimate& pose_estimate() const;
+
+ private:
+  std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
+      common::Time time, const sensor::RangeData& range_data_in_tracking);
+
+  std::unique_ptr<InsertionResult> InsertIntoSubmap(
+      common::Time time, const sensor::RangeData& range_data_in_tracking,
+      const transform::Rigid3d& pose_observation);
+
+  const proto::LocalTrajectoryBuilderOptions options_;
+  mapping_3d::ActiveSubmaps active_submaps_;
+
+  PoseEstimate last_pose_estimate_;
+
+  // Pose of the last computed scan match.
+  transform::Rigid3d scan_matcher_pose_estimate_;
+
+  MotionFilter motion_filter_;
+  std::unique_ptr<scan_matching::RealTimeCorrelativeScanMatcher>
+      real_time_correlative_scan_matcher_;
+  std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
+
+  std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
+
+  int num_accumulated_;
+  transform::Rigid3f first_pose_prediction_;
+  sensor::RangeData accumulated_range_data_;
+};
 
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/local_trajectory_builder_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
+#include "cartographer/mapping_3d/local_trajectory_builder.h"
 
 #include <memory>
 #include <random>
@@ -35,7 +35,7 @@ namespace cartographer {
 namespace mapping_3d {
 namespace {
 
-class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
+class LocalTrajectoryBuilderTest : public ::testing::Test {
  protected:
   struct TrajectoryNode {
     common::Time time;
@@ -258,14 +258,13 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
     }
   }
 
-  std::unique_ptr<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
+  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
   std::vector<Eigen::Vector3f> bubbles_;
 };
 
-TEST_F(KalmanLocalTrajectoryBuilderTest,
+TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
-       MoveInsideCubeUsingOnlyCeresScanMatcher) {
   local_trajectory_builder_.reset(
-      new KalmanLocalTrajectoryBuilder(CreateTrajectoryBuilderOptions()));
+      new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions()));
   VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
 }