Extrapolator Result for motion compensation (#1731)

This makes use of the ExtrapolationResult returned by the pose extrapolator. ExtrapolationResult contains poses corresponding to the timestamps of range data measurements to be used for motion compensation. When using the PoseExtrapolator implementation there is no functional difference. This PR prepares the integration of ImuBasedPoseExtrapolator, which requires using ExtrapolationResult returned by ExtrapolatePosesWithGravity to run efficiently.

Signed-off-by: mschworer <mschworer@lyft.com>

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

@@ -25,6 +25,7 @@
 #include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h"
 #include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h"
 #include "cartographer/mapping/proto/scan_matching//real_time_correlative_scan_matcher_options.pb.h"
+#include "cartographer/transform/timestamped_transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -55,7 +56,6 @@ LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D(
               options_.real_time_correlative_scan_matcher_options())),
       ceres_scan_matcher_(absl::make_unique<scan_matching::CeresScanMatcher3D>(
           options_.ceres_scan_matcher_options())),
-      accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}},
       range_data_collator_(expected_range_sensor_ids) {}
 
 LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {}
@@ -116,14 +116,13 @@ std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
 LocalTrajectoryBuilder3D::AddRangeData(
     const std::string& sensor_id,
     const sensor::TimedPointCloudData& unsynchronized_data) {
-  const auto synchronized_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& current_sensor_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.
@@ -134,111 +133,134 @@ LocalTrajectoryBuilder3D::AddRangeData(
   CHECK(!synchronized_data.ranges.empty());
   CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
   const common::Time time_first_point =
-      current_sensor_time +
+      synchronized_data.time +
       common::FromSeconds(synchronized_data.ranges.front().point_time.time);
   if (time_first_point < extrapolator_->GetLastPoseTime()) {
     LOG(INFO) << "Extrapolator is still initializing.";
     return nullptr;
   }
 
-  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
+  if (num_accumulated_ == 0) {
+    accumulated_point_cloud_origin_data_.clear();
+  }
+
+  synchronized_data.ranges =
       sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
           .Filter(synchronized_data.ranges);
-
+  accumulated_point_cloud_origin_data_.emplace_back(
-  std::vector<transform::Rigid3f> hits_poses;
+      std::move(synchronized_data));
-  hits_poses.reserve(hits.size());
-  bool warned = false;
-
-  for (const auto& hit : hits) {
-    common::Time time_point =
-        current_sensor_time + common::FromSeconds(hit.point_time.time);
-    if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
-      if (!warned) {
-        LOG(ERROR)
-            << "Timestamp of individual range data point jumps backwards from "
-            << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
-        warned = true;
-      }
-      time_point = extrapolator_->GetLastExtrapolatedTime();
-    }
-    hits_poses.push_back(
-        extrapolator_->ExtrapolatePose(time_point).cast<float>());
-  }
-
-  if (num_accumulated_ == 0) {
-    // 'accumulated_range_data_.origin' is not used.
-    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
-  }
-
-  for (size_t i = 0; i < hits.size(); ++i) {
-    sensor::RangefinderPoint hit_in_local =
-        hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time);
-    const Eigen::Vector3f origin_in_local =
-        hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
-    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
-    const float range = delta.norm();
-    if (range >= options_.min_range()) {
-      if (range <= options_.max_range()) {
-        accumulated_range_data_.returns.push_back(hit_in_local);
-      } 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.
-        hit_in_local.position =
-            origin_in_local + options_.max_range() / range * delta;
-        accumulated_range_data_.misses.push_back(hit_in_local);
-      }
-    }
-  }
   ++num_accumulated_;
 
-  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
+  if (num_accumulated_ < options_.num_accumulated_range_data()) {
-    absl::optional<common::Duration> sensor_duration;
+    return nullptr;
-    if (last_sensor_time_.has_value()) {
-      sensor_duration = current_sensor_time - last_sensor_time_.value();
-    }
-    last_sensor_time_ = current_sensor_time;
-    num_accumulated_ = 0;
-
-    transform::Rigid3f current_pose =
-        extrapolator_->ExtrapolatePose(current_sensor_time).cast<float>();
-
-    const auto voxel_filter_start = std::chrono::steady_clock::now();
-    const sensor::RangeData filtered_range_data = {
-        current_pose.translation(),
-        sensor::VoxelFilter(options_.voxel_filter_size())
-            .Filter(accumulated_range_data_.returns),
-        sensor::VoxelFilter(options_.voxel_filter_size())
-            .Filter(accumulated_range_data_.misses)};
-    const auto voxel_filter_stop = std::chrono::steady_clock::now();
-    const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
-
-    if (sensor_duration.has_value()) {
-      const double voxel_filter_fraction =
-          common::ToSeconds(voxel_filter_duration) /
-          common::ToSeconds(sensor_duration.value());
-      kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction);
-    }
-
-    return AddAccumulatedRangeData(
-        current_sensor_time,
-        sensor::TransformRangeData(filtered_range_data, current_pose.inverse()),
-        sensor_duration);
   }
-  return nullptr;
+  num_accumulated_ = 0;
+
+  bool warned = false;
+  std::vector<common::Time> hit_times;
+  common::Time prev_time_point = extrapolator_->GetLastExtrapolatedTime();
+  for (const auto& point_cloud_origin_data :
+       accumulated_point_cloud_origin_data_) {
+    for (const auto& hit : point_cloud_origin_data.ranges) {
+      common::Time time_point = point_cloud_origin_data.time +
+                                common::FromSeconds(hit.point_time.time);
+      if (time_point < prev_time_point) {
+        if (!warned) {
+          LOG(ERROR) << "Timestamp of individual range data point jumps "
+                        "backwards from "
+                     << prev_time_point << " to " << time_point;
+          warned = true;
+        }
+        time_point = prev_time_point;
+      }
+
+      hit_times.push_back(time_point);
+      prev_time_point = time_point;
+    }
+  }
+  hit_times.push_back(accumulated_point_cloud_origin_data_.back().time);
+
+  const PoseExtrapolatorInterface::ExtrapolationResult extrapolation_result =
+      extrapolator_->ExtrapolatePosesWithGravity(hit_times);
+  std::vector<transform::Rigid3f> hits_poses(
+      std::move(extrapolation_result.previous_poses));
+  hits_poses.push_back(extrapolation_result.current_pose.cast<float>());
+  CHECK_EQ(hits_poses.size(), hit_times.size());
+
+  sensor::RangeData accumulated_range_data;
+  std::vector<transform::Rigid3f>::const_iterator hits_poses_it =
+      hits_poses.begin();
+  for (const auto& point_cloud_origin_data :
+       accumulated_point_cloud_origin_data_) {
+    for (const auto& hit : point_cloud_origin_data.ranges) {
+      const Eigen::Vector3f hit_in_local =
+          *hits_poses_it * hit.point_time.position;
+      const Eigen::Vector3f origin_in_local =
+          *hits_poses_it * point_cloud_origin_data.origins.at(hit.origin_index);
+      const Eigen::Vector3f delta = hit_in_local - origin_in_local;
+      const float range = delta.norm();
+      if (range >= options_.min_range()) {
+        if (range <= options_.max_range()) {
+          accumulated_range_data.returns.push_back(
+              sensor::RangefinderPoint{hit_in_local});
+        } 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(sensor::RangefinderPoint{
+              origin_in_local + options_.max_range() / range * delta});
+        }
+      }
+      ++hits_poses_it;
+    }
+  }
+  CHECK(std::next(hits_poses_it) == hits_poses.end());
+
+  const common::Time current_sensor_time = synchronized_data.time;
+  absl::optional<common::Duration> sensor_duration;
+  if (last_sensor_time_.has_value()) {
+    sensor_duration = current_sensor_time - last_sensor_time_.value();
+  }
+  last_sensor_time_ = current_sensor_time;
+
+  const common::Time current_time = hit_times.back();
+  const auto voxel_filter_start = std::chrono::steady_clock::now();
+  const sensor::RangeData filtered_range_data = {
+      extrapolation_result.current_pose.translation().cast<float>(),
+      sensor::VoxelFilter(options_.voxel_filter_size())
+          .Filter(accumulated_range_data.returns),
+      sensor::VoxelFilter(options_.voxel_filter_size())
+          .Filter(accumulated_range_data.misses)};
+  const auto voxel_filter_stop = std::chrono::steady_clock::now();
+  const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
+
+  if (sensor_duration.has_value()) {
+    const double voxel_filter_fraction =
+        common::ToSeconds(voxel_filter_duration) /
+        common::ToSeconds(sensor_duration.value());
+    kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction);
+  }
+
+  return AddAccumulatedRangeData(
+      current_time,
+      sensor::TransformRangeData(
+          filtered_range_data,
+          extrapolation_result.current_pose.inverse().cast<float>()),
+      sensor_duration, extrapolation_result.current_pose,
+      extrapolation_result.gravity_from_tracking);
 }
 
 std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
 LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     const common::Time time,
     const sensor::RangeData& filtered_range_data_in_tracking,
-    const absl::optional<common::Duration>& sensor_duration) {
+    const absl::optional<common::Duration>& sensor_duration,
+    const transform::Rigid3d& pose_prediction,
+    const Eigen::Quaterniond& gravity_alignment) {
   if (filtered_range_data_in_tracking.returns.empty()) {
     LOG(WARNING) << "Dropped empty range data.";
     return nullptr;
   }
-  const transform::Rigid3d pose_prediction =
-      extrapolator_->ExtrapolatePose(time);
 
   const auto scan_matcher_start = std::chrono::steady_clock::now();
 
@@ -278,8 +300,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction);
   }
 
-  const Eigen::Quaterniond gravity_alignment =
-      extrapolator_->EstimateGravityOrientation(time);
   sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
       filtered_range_data_in_tracking, pose_estimate->cast<float>());
 

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

@@ -33,6 +33,7 @@
 #include "cartographer/sensor/internal/voxel_filter.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/range_data.h"
+#include "cartographer/sensor/timed_point_cloud_data.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
@@ -78,7 +79,9 @@ class LocalTrajectoryBuilder3D {
   std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
       common::Time time,
       const sensor::RangeData& filtered_range_data_in_tracking,
-      const absl::optional<common::Duration>& sensor_duration);
+      const absl::optional<common::Duration>& sensor_duration,
+      const transform::Rigid3d& pose_prediction,
+      const Eigen::Quaterniond& gravity_alignment);
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       common::Time time, const sensor::RangeData& filtered_range_data_in_local,
@@ -106,7 +109,8 @@ class LocalTrajectoryBuilder3D {
   std::unique_ptr<mapping::PoseExtrapolatorInterface> extrapolator_;
 
   int num_accumulated_ = 0;
-  sensor::RangeData accumulated_range_data_;
+  std::vector<sensor::TimedPointCloudOriginData>
+      accumulated_point_cloud_origin_data_;
   absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
 
   absl::optional<double> last_thread_cpu_time_seconds_;