LocalTrajectoryBuilder uses RangeDataCollator (#996)

Local SLAM accepts and unwarps time-overlapping range data from multiple sensors.

[RFC=0017](https://github.com/googlecartographer/rfcs/blob/master/text/0017-synchronize-points.md)
FIXES=#910

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -34,13 +34,15 @@ static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
 static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
 
 LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
-    const proto::LocalTrajectoryBuilderOptions2D& options)
+    const proto::LocalTrajectoryBuilderOptions2D& options,
+    const std::vector<std::string>& expected_range_sensor_ids)
     : options_(options),
       active_submaps_(options.submaps_options()),
       motion_filter_(options_.motion_filter_options()),
       real_time_correlative_scan_matcher_(
           options_.real_time_correlative_scan_matcher_options()),
-      ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
+      ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
+      range_data_collator_(expected_range_sensor_ids) {}
 
 LocalTrajectoryBuilder2D::~LocalTrajectoryBuilder2D() {}
 
@@ -101,7 +103,16 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
 
 std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
 LocalTrajectoryBuilder2D::AddRangeData(
-    const common::Time time, const sensor::TimedRangeData& range_data) {
+    const std::string& sensor_id,
+    const sensor::TimedPointCloudData& unsynchronized_data) {
+  auto synchronized_data =
+      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
+  if (synchronized_data.ranges.empty()) {
+    LOG(INFO) << "Range data collator filling buffer.";
+    return nullptr;
+  }
+
+  const common::Time& time = synchronized_data.time;
   // Initialize extrapolator now if we do not ever use an IMU.
   if (!options_.use_imu_data()) {
     InitializeExtrapolator(time);
@@ -114,10 +125,12 @@ LocalTrajectoryBuilder2D::AddRangeData(
     return nullptr;
   }
 
-  CHECK(!range_data.returns.empty());
+  CHECK(!synchronized_data.ranges.empty());
-  CHECK_EQ(range_data.returns.back()[3], 0);
+  // TODO(gaschler): Check if this can strictly be 0.
+  CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
   const common::Time time_first_point =
-      time + common::FromSeconds(range_data.returns.front()[3]);
+      time +
+      common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
   if (time_first_point < extrapolator_->GetLastPoseTime()) {
     LOG(INFO) << "Extrapolator is still initializing.";
     return nullptr;
@@ -128,10 +141,10 @@ LocalTrajectoryBuilder2D::AddRangeData(
   }
 
   std::vector<transform::Rigid3f> range_data_poses;
-  range_data_poses.reserve(range_data.returns.size());
+  range_data_poses.reserve(synchronized_data.ranges.size());
   bool warned = false;
-  for (const Eigen::Vector4f& hit : range_data.returns) {
+  for (const auto& range : synchronized_data.ranges) {
-    common::Time time_point = time + common::FromSeconds(hit[3]);
+    common::Time time_point = time + common::FromSeconds(range.point_time[3]);
     if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
       if (!warned) {
         LOG(ERROR)
@@ -153,10 +166,11 @@ LocalTrajectoryBuilder2D::AddRangeData(
 
   // Drop any returns below the minimum range and convert returns beyond the
   // maximum range into misses.
-  for (size_t i = 0; i < range_data.returns.size(); ++i) {
+  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
-    const Eigen::Vector4f& hit = range_data.returns[i];
+    const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;
     const Eigen::Vector3f origin_in_local =
-        range_data_poses[i] * range_data.origin;
+        range_data_poses[i] *
+        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
     const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
     const Eigen::Vector3f delta = hit_in_local - origin_in_local;
     const float range = delta.norm();
@@ -176,8 +190,9 @@ LocalTrajectoryBuilder2D::AddRangeData(
     num_accumulated_ = 0;
     const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
         extrapolator_->EstimateGravityOrientation(time));
-    accumulated_range_data_.origin =
+    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
-        range_data_poses.back() * range_data.origin;
+    // 'time'.
+    accumulated_range_data_.origin = range_data_poses.back().translation();
     return AddAccumulatedRangeData(
         time,
         TransformToGravityAlignedFrameAndFilter(

cartographer/mapping/internal/2d/local_trajectory_builder_2d.h

@@ -26,6 +26,7 @@
 #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
 #include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
+#include "cartographer/mapping/internal/range_data_collator.h"
 #include "cartographer/mapping/pose_extrapolator.h"
 #include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/imu_data.h"
@@ -55,7 +56,8 @@ class LocalTrajectoryBuilder2D {
   };
 
   explicit LocalTrajectoryBuilder2D(
-      const proto::LocalTrajectoryBuilderOptions2D& options);
+      const proto::LocalTrajectoryBuilderOptions2D& options,
+      const std::vector<std::string>& expected_range_sensor_ids);
   ~LocalTrajectoryBuilder2D();
 
   LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
@@ -63,11 +65,12 @@ class LocalTrajectoryBuilder2D {
 
   // Returns 'MatchingResult' when range data accumulation completed,
   // otherwise 'nullptr'. Range data must be approximately horizontal
-  // for 2D SLAM. `time` is when the last point in `range_data` was
+  // for 2D SLAM. `TimedPointCloudData::time` is when the last point in
-  // acquired, `range_data` contains the relative time of point with
+  // `range_data` was acquired, `TimedPointCloudData::ranges` contains the
-  // respect to `time`.
+  // relative time of point with respect to `TimedPointCloudData::time`.
   std::unique_ptr<MatchingResult> AddRangeData(
-      common::Time, const sensor::TimedRangeData& range_data);
+      const std::string& sensor_id,
+      const sensor::TimedPointCloudData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
   void AddOdometryData(const sensor::OdometryData& odometry_data);
 
@@ -109,6 +112,8 @@ class LocalTrajectoryBuilder2D {
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
   std::chrono::steady_clock::time_point accumulation_started_;
+
+  RangeDataCollator range_data_collator_;
 };
 
 }  // namespace mapping

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -38,7 +38,8 @@ static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null();
 static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null();
 
 LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
-    const mapping::proto::LocalTrajectoryBuilderOptions3D& options)
+    const mapping::proto::LocalTrajectoryBuilderOptions3D& options,
+    const std::vector<std::string>& expected_range_sensor_ids)
     : options_(options),
       active_submaps_(options.submaps_options()),
       motion_filter_(options.motion_filter_options()),
@@ -48,7 +49,8 @@ LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
       ceres_scan_matcher_(
           common::make_unique<scan_matching::CeresScanMatcher3D>(
               options_.ceres_scan_matcher_options())),
-      accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
+      accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}},
+      range_data_collator_(expected_range_sensor_ids) {}
 
 LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {}
 
@@ -68,7 +70,16 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
 
 std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
 LocalTrajectoryBuilder3D::AddRangeData(
-    const common::Time time, const sensor::TimedRangeData& range_data) {
+    const std::string& sensor_id,
+    const sensor::TimedPointCloudData& unsynchronized_data) {
+  auto synchronized_data =
+      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
+  if (synchronized_data.ranges.empty()) {
+    LOG(INFO) << "Range data collator filling buffer.";
+    return nullptr;
+  }
+
+  const common::Time& time = synchronized_data.time;
   if (extrapolator_ == nullptr) {
     // Until we've initialized the extrapolator with our first IMU message, we
     // cannot compute the orientation of the rangefinder.
@@ -76,10 +87,11 @@ LocalTrajectoryBuilder3D::AddRangeData(
     return nullptr;
   }
 
-  CHECK(!range_data.returns.empty());
+  CHECK(!synchronized_data.ranges.empty());
-  CHECK_EQ(range_data.returns.back()[3], 0);
+  CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
   const common::Time time_first_point =
-      time + common::FromSeconds(range_data.returns.front()[3]);
+      time +
+      common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
   if (time_first_point < extrapolator_->GetLastPoseTime()) {
     LOG(INFO) << "Extrapolator is still initializing.";
     return nullptr;
@@ -89,15 +101,15 @@ LocalTrajectoryBuilder3D::AddRangeData(
     accumulation_started_ = std::chrono::steady_clock::now();
   }
 
-  sensor::TimedPointCloud hits =
+  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
       sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
-          .Filter(range_data.returns);
+          .Filter(synchronized_data.ranges);
 
   std::vector<transform::Rigid3f> hits_poses;
   hits_poses.reserve(hits.size());
   bool warned = false;
-  for (const Eigen::Vector4f& hit : hits) {
+  for (const auto& hit : hits) {
-    common::Time time_point = time + common::FromSeconds(hit[3]);
+    common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
     if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
       if (!warned) {
         LOG(ERROR)
@@ -117,8 +129,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
   }
 
   for (size_t i = 0; i < hits.size(); ++i) {
-    const Eigen::Vector3f hit_in_local = hits_poses[i] * hits[i].head<3>();
+    const Eigen::Vector3f hit_in_local =
-    const Eigen::Vector3f origin_in_local = hits_poses[i] * range_data.origin;
+        hits_poses[i] * hits[i].point_time.head<3>();
+    const Eigen::Vector3f origin_in_local =
+        hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
     const Eigen::Vector3f delta = hit_in_local - origin_in_local;
     const float range = delta.norm();
     if (range >= options_.min_range()) {
@@ -140,7 +154,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
     transform::Rigid3f current_pose =
         extrapolator_->ExtrapolatePose(time).cast<float>();
     const sensor::RangeData filtered_range_data = {
-        current_pose * range_data.origin,
+        current_pose.translation(),
         sensor::VoxelFilter(options_.voxel_filter_size())
             .Filter(accumulated_range_data_.returns),
         sensor::VoxelFilter(options_.voxel_filter_size())

cartographer/mapping/internal/3d/local_trajectory_builder_3d.h

@@ -26,6 +26,7 @@
 #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
 #include "cartographer/mapping/internal/motion_filter.h"
+#include "cartographer/mapping/internal/range_data_collator.h"
 #include "cartographer/mapping/pose_extrapolator.h"
 #include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/imu_data.h"
@@ -54,7 +55,8 @@ class LocalTrajectoryBuilder3D {
   };
 
   explicit LocalTrajectoryBuilder3D(
-      const mapping::proto::LocalTrajectoryBuilderOptions3D& options);
+      const mapping::proto::LocalTrajectoryBuilderOptions3D& options,
+      const std::vector<std::string>& expected_range_sensor_ids);
   ~LocalTrajectoryBuilder3D();
 
   LocalTrajectoryBuilder3D(const LocalTrajectoryBuilder3D&) = delete;
@@ -62,11 +64,12 @@ class LocalTrajectoryBuilder3D {
 
   void AddImuData(const sensor::ImuData& imu_data);
   // Returns 'MatchingResult' when range data accumulation completed,
-  // otherwise 'nullptr'. `time` is when the last point in `range_data`
+  // otherwise 'nullptr'.  `TimedPointCloudData::time` is when the last point in
-  // was acquired, `range_data` contains the relative time of point with
+  // `range_data` was acquired, `TimedPointCloudData::ranges` contains the
-  // respect to `time`.
+  // relative time of point with respect to `TimedPointCloudData::time`.
   std::unique_ptr<MatchingResult> AddRangeData(
-      common::Time time, const sensor::TimedRangeData& range_data);
+      const std::string& sensor_id,
+      const sensor::TimedPointCloudData& range_data);
   void AddOdometryData(const sensor::OdometryData& odometry_data);
 
   static void RegisterMetrics(metrics::FamilyFactory* family_factory);
@@ -97,6 +100,8 @@ class LocalTrajectoryBuilder3D {
   int num_accumulated_ = 0;
   sensor::RangeData accumulated_range_data_;
   std::chrono::steady_clock::time_point accumulation_started_;
+
+  RangeDataCollator range_data_collator_;
 };
 
 }  // namespace mapping

cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc

@@ -35,6 +35,8 @@ namespace cartographer {
 namespace mapping {
 namespace {
 
+constexpr char kSensorId[] = "sensor_id";
+
 class LocalTrajectoryBuilderTest : public ::testing::Test {
  protected:
   struct TrajectoryNode {
@@ -255,8 +257,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
       const auto range_data = GenerateRangeData(node.pose);
       const std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
           matching_result = local_trajectory_builder_->AddRangeData(
-              node.time, sensor::TimedRangeData{
+              kSensorId, sensor::TimedPointCloudData{
-                             range_data.origin, range_data.returns, {}});
+                             node.time, range_data.origin, range_data.returns});
       if (matching_result != nullptr) {
         EXPECT_THAT(matching_result->local_pose,
                     transform::IsNearly(node.pose, 1e-1));
@@ -271,8 +273,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
 };
 
 TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
-  local_trajectory_builder_.reset(
+  local_trajectory_builder_.reset(new LocalTrajectoryBuilder3D(
-      new LocalTrajectoryBuilder3D(CreateTrajectoryBuilderOptions3D()));
+      CreateTrajectoryBuilderOptions3D(), {kSensorId}));
   VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
 }
 

cartographer/mapping/internal/global_trajectory_builder.cc

@@ -56,10 +56,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
         << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
     std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
         matching_result = local_trajectory_builder_->AddRangeData(
-            timed_point_cloud_data.time,
+            sensor_id, timed_point_cloud_data);
-            sensor::TimedRangeData{timed_point_cloud_data.origin,
-                                   timed_point_cloud_data.ranges,
-                                   {}});
     if (matching_result == nullptr) {
       // The range data has not been fully accumulated yet.
       return;

cartographer/mapping/map_builder.cc

@@ -49,6 +49,17 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
   return options;
 }
 
+std::vector<std::string> SelectRangeSensorIds(
+    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
+  std::vector<std::string> range_sensor_ids;
+  for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
+    if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
+      range_sensor_ids.push_back(sensor_id.id);
+    }
+  }
+  return range_sensor_ids;
+}
+
 MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
     : options_(options), thread_pool_(options.num_background_threads()) {
   if (options.use_trajectory_builder_2d()) {
@@ -81,7 +92,8 @@ int MapBuilder::AddTrajectoryBuilder(
     std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
     if (trajectory_options.has_trajectory_builder_3d_options()) {
       local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(
-          trajectory_options.trajectory_builder_3d_options());
+          trajectory_options.trajectory_builder_3d_options(),
+          SelectRangeSensorIds(expected_sensor_ids));
     }
     DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
     trajectory_builders_.push_back(
@@ -95,7 +107,8 @@ int MapBuilder::AddTrajectoryBuilder(
     std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
     if (trajectory_options.has_trajectory_builder_2d_options()) {
       local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
-          trajectory_options.trajectory_builder_2d_options());
+          trajectory_options.trajectory_builder_2d_options(),
+          SelectRangeSensorIds(expected_sensor_ids));
     }
     DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
     trajectory_builders_.push_back(