Unwarp by point in LocalTrajectoryBuilder. (#636)

cartographer/internal/mapping_2d/local_trajectory_builder.cc

@@ -94,32 +94,45 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
     LOG(INFO) << "Extrapolator not yet initialized.";
     return nullptr;
   }
-  if (num_accumulated_ == 0) {
+
-    first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>();
+  CHECK(!range_data.returns.empty());
-    accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}};
+  CHECK_EQ(range_data.returns.back()[3], 0);
+  const common::Time time_first_point =
+      time + common::FromSeconds(range_data.returns.front()[3]);
+  if (time_first_point < extrapolator_->GetLastPoseTime()) {
+    LOG(INFO) << "Extrapolator is still initializing.";
+    return nullptr;
+  }
+
+  std::vector<transform::Rigid3f> range_data_poses;
+  range_data_poses.reserve(range_data.returns.size());
+  for (const Eigen::Vector4f& hit : range_data.returns) {
+    const common::Time time_point = time + common::FromSeconds(hit[3]);
+    range_data_poses.push_back(
+        extrapolator_->ExtrapolatePose(time_point).cast<float>());
+  }
+
+  if (num_accumulated_ == 0) {
+    // 'accumulated_range_data_.origin' is uninitialized until the last
+    // accumulation.
+    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
   }
 
-  // TODO(gaschler): Take time delta of individual points into account.
-  const transform::Rigid3f tracking_delta =
-      first_pose_estimate_.inverse() *
-      extrapolator_->ExtrapolatePose(time).cast<float>();
-  // TODO(gaschler): Try to move this common behavior with 3D into helper.
-  const Eigen::Vector3f origin_in_first_tracking =
-      tracking_delta * range_data.origin;
   // Drop any returns below the minimum range and convert returns beyond the
   // maximum range into misses.
-  for (const Eigen::Vector4f& hit : range_data.returns) {
+  for (size_t i = 0; i < range_data.returns.size(); ++i) {
-    const Eigen::Vector3f hit_in_first_tracking =
+    const Eigen::Vector4f& hit = range_data.returns[i];
-        tracking_delta * hit.head<3>();
+    const Eigen::Vector3f origin_in_local =
-    const Eigen::Vector3f delta =
+        range_data_poses[i] * range_data.origin;
-        hit_in_first_tracking - origin_in_first_tracking;
+    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();
     if (range >= options_.min_range()) {
       if (range <= options_.max_range()) {
-        accumulated_range_data_.returns.push_back(hit_in_first_tracking);
+        accumulated_range_data_.returns.push_back(hit_in_local);
       } else {
         accumulated_range_data_.misses.push_back(
-            origin_in_first_tracking +
+            origin_in_local +
             options_.missing_data_ray_length() / range * delta);
       }
     }
@@ -130,10 +143,12 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
     num_accumulated_ = 0;
     const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
         extrapolator_->EstimateGravityOrientation(time));
+    accumulated_range_data_.origin =
+        range_data_poses.back() * range_data.origin;
     return AddAccumulatedRangeData(
         time,
         TransformToGravityAlignedFrameAndFilter(
-            gravity_alignment.cast<float>() * tracking_delta.inverse(),
+            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
             accumulated_range_data_),
         gravity_alignment);
   }
@@ -238,6 +253,7 @@ void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) {
   // stability. Usually poses known to the extrapolator will be further apart
   // in time and thus the last two are used.
   constexpr double kExtrapolationEstimationTimeSec = 0.001;
+  // TODO(gaschler): Consider using InitializeWithImu as 3D does.
   extrapolator_ = common::make_unique<mapping::PoseExtrapolator>(
       ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
       options_.imu_gravity_time_constant());

cartographer/internal/mapping_2d/local_trajectory_builder.h

@@ -61,7 +61,9 @@ class LocalTrajectoryBuilder {
 
   // Returns 'MatchingResult' when range data accumulation completed,
   // otherwise 'nullptr'. Range data must be approximately horizontal
-  // for 2D SLAM.
+  // for 2D SLAM. `time` is when the last point in `range_data` was
+  // acquired, `range_data` contains the relative time of point with
+  // respect to `time`.
   std::unique_ptr<MatchingResult> AddRangeData(
       common::Time, const sensor::TimedRangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
@@ -101,7 +103,6 @@ class LocalTrajectoryBuilder {
   std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
 
   int num_accumulated_ = 0;
-  transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
   sensor::RangeData accumulated_range_data_;
 };
 

cartographer/internal/mapping_3d/local_trajectory_builder.cc

@@ -67,32 +67,47 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
     LOG(INFO) << "IMU not yet initialized.";
     return nullptr;
   }
-  if (num_accumulated_ == 0) {
+
-    first_pose_estimate_ = extrapolator_->ExtrapolatePose(time).cast<float>();
+  CHECK(!range_data.returns.empty());
-    accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}};
+  CHECK_EQ(range_data.returns.back()[3], 0);
+  const common::Time time_first_point =
+      time + common::FromSeconds(range_data.returns.front()[3]);
+  if (time_first_point < extrapolator_->GetLastPoseTime()) {
+    LOG(INFO) << "Extrapolator is still initializing.";
+    return nullptr;
   }
 
-  // TODO(gaschler): Take time delta of individual points into account.
+  sensor::TimedPointCloud hits =
-  const transform::Rigid3f tracking_delta =
+      sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
-      first_pose_estimate_.inverse() *
+          .Filter(range_data.returns);
-      extrapolator_->ExtrapolatePose(time).cast<float>();
+
-  const Eigen::Vector3f origin_in_first_tracking =
+  std::vector<transform::Rigid3f> hits_poses;
-      tracking_delta * range_data.origin;
+  hits_poses.reserve(hits.size());
-  for (const Eigen::Vector4f& hit : range_data.returns) {
+  for (const Eigen::Vector4f& hit : hits) {
-    const Eigen::Vector3f hit_in_first_tracking =
+    const common::Time time_point = time + common::FromSeconds(hit[3]);
-        tracking_delta * hit.head<3>();
+    hits_poses.push_back(
-    const Eigen::Vector3f delta =
+        extrapolator_->ExtrapolatePose(time_point).cast<float>());
-        hit_in_first_tracking - origin_in_first_tracking;
+  }
+
+  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) {
+    const Eigen::Vector3f hit_in_local = hits_poses[i] * hits[i].head<3>();
+    const Eigen::Vector3f origin_in_local = hits_poses[i] * range_data.origin;
+    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(hit_in_first_tracking);
+        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.
         accumulated_range_data_.misses.push_back(
-            origin_in_first_tracking + options_.max_range() / range * delta);
+            origin_in_local + options_.max_range() / range * delta);
       }
     }
   }
@@ -100,15 +115,17 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
 
   if (num_accumulated_ >= options_.num_accumulated_range_data()) {
     num_accumulated_ = 0;
+    transform::Rigid3f current_pose =
+        extrapolator_->ExtrapolatePose(time).cast<float>();
     const sensor::RangeData filtered_range_data = {
-        accumulated_range_data_.origin,
+        current_pose * range_data.origin,
         sensor::VoxelFilter(options_.voxel_filter_size())
             .Filter(accumulated_range_data_.returns),
         sensor::VoxelFilter(options_.voxel_filter_size())
             .Filter(accumulated_range_data_.misses)};
     return AddAccumulatedRangeData(
         time, sensor::TransformRangeData(filtered_range_data,
-                                         tracking_delta.inverse()));
+                                         current_pose.inverse()));
   }
   return nullptr;
 }

cartographer/internal/mapping_3d/local_trajectory_builder.h

@@ -60,7 +60,9 @@ class LocalTrajectoryBuilder {
 
   void AddImuData(const sensor::ImuData& imu_data);
   // Returns 'MatchingResult' when range data accumulation completed,
-  // otherwise 'nullptr'.
+  // otherwise 'nullptr'. `time` is when the last point in `range_data`
+  // was acquired, `range_data` contains the relative time of point with
+  // respect to `time`.
   std::unique_ptr<MatchingResult> AddRangeData(
       common::Time time, const sensor::TimedRangeData& range_data);
   void AddOdometryData(const sensor::OdometryData& odometry_data);
@@ -89,7 +91,6 @@ class LocalTrajectoryBuilder {
   std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
 
   int num_accumulated_ = 0;
-  transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity();
   sensor::RangeData accumulated_range_data_;
 };
 

cartographer/mapping/pose_extrapolator.cc

@@ -28,7 +28,9 @@ namespace mapping {
 PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
                                    double imu_gravity_time_constant)
     : pose_queue_duration_(pose_queue_duration),
-      gravity_time_constant_(imu_gravity_time_constant) {}
+      gravity_time_constant_(imu_gravity_time_constant),
+      cached_extrapolated_pose_{common::Time::min(),
+                                transform::Rigid3d::Identity()} {}
 
 std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
     const common::Duration pose_queue_duration,
@@ -130,15 +132,18 @@ void PoseExtrapolator::AddOdometryData(
 }
 
 transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
-  // TODO(whess): Keep the last extrapolated pose.
   const TimedPose& newest_timed_pose = timed_pose_queue_.back();
   CHECK_GE(time, newest_timed_pose.time);
-  const Eigen::Vector3d translation =
+  if (cached_extrapolated_pose_.time != time) {
-      ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
+    const Eigen::Vector3d translation =
-  const Eigen::Quaterniond rotation =
+        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
-      newest_timed_pose.pose.rotation() *
+    const Eigen::Quaterniond rotation =
-      ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
+        newest_timed_pose.pose.rotation() *
-  return transform::Rigid3d(translation, rotation);
+        ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
+    cached_extrapolated_pose_ =
+        TimedPose{time, transform::Rigid3d{translation, rotation}};
+  }
+  return cached_extrapolated_pose_.pose;
 }
 
 Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(

cartographer/mapping/pose_extrapolator.h

@@ -80,6 +80,7 @@ class PoseExtrapolator {
   std::unique_ptr<ImuTracker> imu_tracker_;
   std::unique_ptr<ImuTracker> odometry_imu_tracker_;
   std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
+  TimedPose cached_extrapolated_pose_;
 
   std::deque<sensor::OdometryData> odometry_data_;
   Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();