From aa3ac7e837cbe0938e4cf0a524c577a42f8ad7bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Mon, 9 Jul 2018 20:45:55 +0200 Subject: [PATCH] Removed redundant 2d voxel filtering (#1243) Removed redundant adaptive voxel fitering in trajectory builder 2d. Adaptive voxel filtering of the lidar point cloud was performed in InsertIntoSubmap and ScanMatch. Both methods are called from AddAccumulatedRangeData. Now, adaptive voxel filtering is done only once in AddAccumulatedRangeData and the filtered point cloud is then forwarded to InsertIntoSubmap and ScanMatch. --- .../2d/local_trajectory_builder_2d.cc | 32 ++++++++----------- .../internal/2d/local_trajectory_builder_2d.h | 9 +++--- 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index feda1fa..5286238 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -62,19 +62,13 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( const common::Time time, const transform::Rigid2d& pose_prediction, - const sensor::RangeData& gravity_aligned_range_data) { + const sensor::PointCloud& filtered_gravity_aligned_point_cloud) { std::shared_ptr matching_submap = active_submaps_.submaps().front(); // The online correlative scan matcher will refine the initial estimate for // the Ceres scan matcher. transform::Rigid2d initial_ceres_pose = pose_prediction; - sensor::AdaptiveVoxelFilter adaptive_voxel_filter( - options_.adaptive_voxel_filter_options()); - const sensor::PointCloud filtered_gravity_aligned_point_cloud = - adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns); - if (filtered_gravity_aligned_point_cloud.empty()) { - return nullptr; - } + if (options_.use_online_correlative_scan_matching()) { CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(), proto::GridOptions2D_GridType_PROBABILITY_GRID); @@ -222,9 +216,16 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( const transform::Rigid2d pose_prediction = transform::Project2D( non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); + const sensor::PointCloud& filtered_gravity_aligned_point_cloud = + sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options()) + .Filter(gravity_aligned_range_data.returns); + if (filtered_gravity_aligned_point_cloud.empty()) { + return nullptr; + } + // local map frame <- gravity-aligned frame std::unique_ptr pose_estimate_2d = - ScanMatch(time, pose_prediction, gravity_aligned_range_data); + ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud); if (pose_estimate_2d == nullptr) { LOG(WARNING) << "Scan matching failed."; return nullptr; @@ -236,9 +237,9 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( sensor::RangeData range_data_in_local = TransformRangeData(gravity_aligned_range_data, transform::Embed3D(pose_estimate_2d->cast())); - std::unique_ptr insertion_result = - InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data, - pose_estimate, gravity_alignment.rotation()); + std::unique_ptr insertion_result = InsertIntoSubmap( + time, range_data_in_local, filtered_gravity_aligned_point_cloud, + pose_estimate, gravity_alignment.rotation()); auto duration = std::chrono::steady_clock::now() - accumulation_started_; kLocalSlamLatencyMetric->Set( std::chrono::duration_cast>(duration) @@ -251,7 +252,7 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( std::unique_ptr LocalTrajectoryBuilder2D::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_local, - const sensor::RangeData& gravity_aligned_range_data, + const sensor::PointCloud& filtered_gravity_aligned_point_cloud, const transform::Rigid3d& pose_estimate, const Eigen::Quaterniond& gravity_alignment) { if (motion_filter_.IsSimilar(time, pose_estimate)) { @@ -266,11 +267,6 @@ LocalTrajectoryBuilder2D::InsertIntoSubmap( } active_submaps_.InsertRangeData(range_data_in_local); - sensor::AdaptiveVoxelFilter adaptive_voxel_filter( - options_.loop_closure_adaptive_voxel_filter_options()); - const sensor::PointCloud filtered_gravity_aligned_point_cloud = - adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns); - return common::make_unique(InsertionResult{ std::make_shared(TrajectoryNode::Data{ time, diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h index bfc47b3..068a4a2 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h @@ -83,18 +83,17 @@ class LocalTrajectoryBuilder2D { sensor::RangeData TransformToGravityAlignedFrameAndFilter( const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const; - std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& range_data_in_local, - const sensor::RangeData& gravity_aligned_range_data, + const sensor::PointCloud& filtered_gravity_aligned_point_cloud, const transform::Rigid3d& pose_estimate, const Eigen::Quaterniond& gravity_alignment); - // Scan matches 'gravity_aligned_range_data' and returns the observed pose, - // or nullptr on failure. + // Scan matches 'filtered_gravity_aligned_point_cloud' and returns the + // observed pose, or nullptr on failure. std::unique_ptr ScanMatch( common::Time time, const transform::Rigid2d& pose_prediction, - const sensor::RangeData& gravity_aligned_range_data); + const sensor::PointCloud& filtered_gravity_aligned_point_cloud); // Lazily constructs a PoseExtrapolator. void InitializeExtrapolator(common::Time time);