diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.cc b/cartographer/internal/mapping_2d/local_trajectory_builder.cc index 50baa2b..076f497 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.cc +++ b/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(); - accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}}; + + CHECK(!range_data.returns.empty()); + 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 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()); + } + + 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(); - // 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) { - const Eigen::Vector3f hit_in_first_tracking = - tracking_delta * hit.head<3>(); - const Eigen::Vector3f delta = - hit_in_first_tracking - origin_in_first_tracking; + for (size_t i = 0; i < range_data.returns.size(); ++i) { + const Eigen::Vector4f& hit = range_data.returns[i]; + const Eigen::Vector3f origin_in_local = + range_data_poses[i] * range_data.origin; + 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() * tracking_delta.inverse(), + gravity_alignment.cast() * 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( ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), options_.imu_gravity_time_constant()); diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.h b/cartographer/internal/mapping_2d/local_trajectory_builder.h index 7e1f19c..670df42 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.h +++ b/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 AddRangeData( common::Time, const sensor::TimedRangeData& range_data); void AddImuData(const sensor::ImuData& imu_data); @@ -101,7 +103,6 @@ class LocalTrajectoryBuilder { std::unique_ptr extrapolator_; int num_accumulated_ = 0; - transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity(); sensor::RangeData accumulated_range_data_; }; diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder.cc index 22ad471..c821f2f 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/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(); - accumulated_range_data_ = sensor::RangeData{range_data.origin, {}, {}}; + + CHECK(!range_data.returns.empty()); + 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. - const transform::Rigid3f tracking_delta = - first_pose_estimate_.inverse() * - extrapolator_->ExtrapolatePose(time).cast(); - const Eigen::Vector3f origin_in_first_tracking = - tracking_delta * range_data.origin; - for (const Eigen::Vector4f& hit : range_data.returns) { - const Eigen::Vector3f hit_in_first_tracking = - tracking_delta * hit.head<3>(); - const Eigen::Vector3f delta = - hit_in_first_tracking - origin_in_first_tracking; + sensor::TimedPointCloud hits = + sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) + .Filter(range_data.returns); + + std::vector hits_poses; + hits_poses.reserve(hits.size()); + for (const Eigen::Vector4f& hit : hits) { + const common::Time time_point = time + common::FromSeconds(hit[3]); + hits_poses.push_back( + extrapolator_->ExtrapolatePose(time_point).cast()); + } + + 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(); 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; } diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.h b/cartographer/internal/mapping_3d/local_trajectory_builder.h index 9a683cb..cf78a7c 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.h +++ b/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 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 extrapolator_; int num_accumulated_ = 0; - transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity(); sensor::RangeData accumulated_range_data_; }; diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index 4519c07..6898103 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/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::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 = - ExtrapolateTranslation(time) + newest_timed_pose.pose.translation(); - const Eigen::Quaterniond rotation = - newest_timed_pose.pose.rotation() * - ExtrapolateRotation(time, extrapolation_imu_tracker_.get()); - return transform::Rigid3d(translation, rotation); + if (cached_extrapolated_pose_.time != time) { + const Eigen::Vector3d translation = + ExtrapolateTranslation(time) + newest_timed_pose.pose.translation(); + const Eigen::Quaterniond rotation = + newest_timed_pose.pose.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( diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index 290704b..b1eb652 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/cartographer/mapping/pose_extrapolator.h @@ -80,6 +80,7 @@ class PoseExtrapolator { std::unique_ptr imu_tracker_; std::unique_ptr odometry_imu_tracker_; std::unique_ptr extrapolation_imu_tracker_; + TimedPose cached_extrapolated_pose_; std::deque odometry_data_; Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();