From 3c5a7aa2d864e2fc9b6cd9ff889ce7c583df1aac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 2 Nov 2017 16:01:23 +0100 Subject: [PATCH] Refactoring of LocalTrajectoryBuilder::AddAccumulatedRangeData (#618) - introduce InsertIntoSubmap for 2D - clarify some variable names in 3D - move rotational_scan_matcher_histogram calculation to InsertIntoSubmap for 3D - refactor last version of range data before insertion into range_data_in_local (filtered_range_data_in_local for 3D) --- .../mapping_2d/local_trajectory_builder.cc | 30 ++++---- .../mapping_2d/local_trajectory_builder.h | 6 ++ .../mapping_3d/local_trajectory_builder.cc | 72 +++++++++---------- .../mapping_3d/local_trajectory_builder.h | 12 ++-- 4 files changed, 65 insertions(+), 55 deletions(-) diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index e385db8..85965f1 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -130,8 +130,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& range_data) { - // Transforms 'range_data' into a frame where gravity direction is - // approximately +z. + // Transforms 'range_data' from the tracking frame into a frame where gravity + // direction is approximately +z. const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( extrapolator_->EstimateGravityOrientation(time)); const sensor::RangeData gravity_aligned_range_data = @@ -147,19 +147,27 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( const transform::Rigid2d pose_prediction = transform::Project2D( non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); - transform::Rigid2d pose_estimate_2d; + transform::Rigid2d pose_estimate_2d; // local frame <- gravity-aligned frame ScanMatch(time, pose_prediction, gravity_aligned_range_data, &pose_estimate_2d); const transform::Rigid3d pose_estimate = transform::Embed3D(pose_estimate_2d) * gravity_alignment; extrapolator_->AddPose(time, pose_estimate); - last_pose_estimate_ = { - time, pose_estimate, - sensor::TransformPointCloud( - gravity_aligned_range_data.returns, - transform::Embed3D(pose_estimate_2d.cast()))}; + sensor::RangeData range_data_in_local = + TransformRangeData(gravity_aligned_range_data, + transform::Embed3D(pose_estimate_2d.cast())); + last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns}; + return InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data, + pose_estimate, gravity_alignment.rotation()); +} +std::unique_ptr +LocalTrajectoryBuilder::InsertIntoSubmap( + const common::Time time, const sensor::RangeData& range_data_in_local, + const sensor::RangeData& gravity_aligned_range_data, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment) { if (motion_filter_.IsSimilar(time, pose_estimate)) { return nullptr; } @@ -170,9 +178,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( for (const std::shared_ptr& submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); } - active_submaps_.InsertRangeData( - TransformRangeData(gravity_aligned_range_data, - transform::Embed3D(pose_estimate_2d.cast()))); + active_submaps_.InsertRangeData(range_data_in_local); sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.loop_closure_adaptive_voxel_filter_options()); @@ -183,7 +189,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( std::make_shared( mapping::TrajectoryNode::Data{ time, - gravity_alignment.rotation(), + gravity_alignment, filtered_gravity_aligned_point_cloud, {}, // 'high_resolution_point_cloud' is only used in 3D. {}, // 'low_resolution_point_cloud' is only used in 3D. diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 36a2040..fcf7567 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -68,6 +68,12 @@ class LocalTrajectoryBuilder { const transform::Rigid3f& gravity_alignment, 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 transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment); + // Scan matches 'gravity_aligned_range_data' and fill in the // 'pose_observation' with the result. void ScanMatch(common::Time time, const transform::Rigid2d& pose_prediction, diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index a53223a..3582318 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -110,14 +110,14 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& range_data_in_tracking) { - const sensor::RangeData filtered_range_data = { + const sensor::RangeData filtered_range_data_in_tracking = { range_data_in_tracking.origin, sensor::VoxelFiltered(range_data_in_tracking.returns, options_.voxel_filter_size()), sensor::VoxelFiltered(range_data_in_tracking.misses, options_.voxel_filter_size())}; - if (filtered_range_data.returns.empty()) { + if (filtered_range_data_in_tracking.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; } @@ -131,13 +131,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( matching_submap->local_pose().inverse() * pose_prediction; sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.high_resolution_adaptive_voxel_filter_options()); - const sensor::PointCloud filtered_point_cloud_in_tracking = - adaptive_voxel_filter.Filter(filtered_range_data.returns); + const sensor::PointCloud high_resolution_point_cloud_in_tracking = + adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns); if (options_.use_online_correlative_scan_matching()) { // We take a copy since we use 'initial_ceres_pose' as an output argument. const transform::Rigid3d initial_pose = initial_ceres_pose; real_time_correlative_scan_matcher_->Match( - initial_pose, filtered_point_cloud_in_tracking, + initial_pose, high_resolution_point_cloud_in_tracking, matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose); } @@ -147,11 +147,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = - low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns); + low_resolution_adaptive_voxel_filter.Filter( + filtered_range_data_in_tracking.returns); ceres_scan_matcher_->Match( matching_submap->local_pose().inverse() * pose_prediction, initial_ceres_pose, - {{&filtered_point_cloud_in_tracking, + {{&high_resolution_point_cloud_in_tracking, &matching_submap->high_resolution_hybrid_grid()}, {&low_resolution_point_cloud_in_tracking, &matching_submap->low_resolution_hybrid_grid()}}, @@ -161,22 +162,15 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( extrapolator_->AddPose(time, pose_estimate); const Eigen::Quaterniond gravity_alignment = extrapolator_->EstimateGravityOrientation(time); - const auto rotational_scan_matcher_histogram = - scan_matching::RotationalScanMatcher::ComputeHistogram( - sensor::TransformPointCloud( - filtered_range_data.returns, - transform::Rigid3f::Rotation(gravity_alignment.cast())), - options_.rotational_histogram_size()); - last_pose_estimate_ = { - time, pose_estimate, - sensor::TransformPointCloud(filtered_range_data.returns, - pose_estimate.cast())}; - - return InsertIntoSubmap(time, filtered_range_data, gravity_alignment, - filtered_point_cloud_in_tracking, - low_resolution_point_cloud_in_tracking, - rotational_scan_matcher_histogram, pose_estimate); + sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData( + filtered_range_data_in_tracking, pose_estimate.cast()); + last_pose_estimate_ = {time, pose_estimate, + filtered_range_data_in_local.returns}; + return InsertIntoSubmap( + time, filtered_range_data_in_local, filtered_range_data_in_tracking, + high_resolution_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment); } void LocalTrajectoryBuilder::AddOdometerData( @@ -195,13 +189,14 @@ const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { std::unique_ptr LocalTrajectoryBuilder::InsertIntoSubmap( - const common::Time time, const sensor::RangeData& range_data_in_tracking, - const Eigen::Quaterniond& gravity_alignment, - const sensor::PointCloud& high_resolution_point_cloud, - const sensor::PointCloud& low_resolution_point_cloud, - const Eigen::VectorXf& rotational_scan_matcher_histogram, - const transform::Rigid3d& pose_observation) { - if (motion_filter_.IsSimilar(time, pose_observation)) { + const common::Time time, + const sensor::RangeData& filtered_range_data_in_local, + const sensor::RangeData& filtered_range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud_in_tracking, + const sensor::PointCloud& low_resolution_point_cloud_in_tracking, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment) { + if (motion_filter_.IsSimilar(time, pose_estimate)) { return nullptr; } // Querying the active submaps must be done here before calling @@ -210,21 +205,24 @@ LocalTrajectoryBuilder::InsertIntoSubmap( for (const std::shared_ptr& submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); } - active_submaps_.InsertRangeData( - sensor::TransformRangeData(range_data_in_tracking, - pose_observation.cast()), - gravity_alignment); + active_submaps_.InsertRangeData(filtered_range_data_in_local, + gravity_alignment); + const auto rotational_scan_matcher_histogram = + scan_matching::RotationalScanMatcher::ComputeHistogram( + sensor::TransformPointCloud( + filtered_range_data_in_tracking.returns, + transform::Rigid3f::Rotation(gravity_alignment.cast())), + options_.rotational_histogram_size()); return std::unique_ptr(new InsertionResult{ - std::make_shared( mapping::TrajectoryNode::Data{ time, gravity_alignment, {}, // 'filtered_point_cloud' is only used in 2D. - high_resolution_point_cloud, - low_resolution_point_cloud, + high_resolution_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, rotational_scan_matcher_histogram, - pose_observation}), + pose_estimate}), std::move(insertion_submaps)}); } diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 7ec9d8e..e6ab106 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -63,12 +63,12 @@ class LocalTrajectoryBuilder { common::Time time, const sensor::RangeData& range_data_in_tracking); std::unique_ptr InsertIntoSubmap( - common::Time time, const sensor::RangeData& range_data_in_tracking, - const Eigen::Quaterniond& gravity_alignment, - const sensor::PointCloud& high_resolution_point_cloud, - const sensor::PointCloud& low_resolution_point_cloud, - const Eigen::VectorXf& rotational_scan_matcher_histogram, - const transform::Rigid3d& pose_observation); + common::Time time, const sensor::RangeData& filtered_range_data_in_local, + const sensor::RangeData& filtered_range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud_in_tracking, + const sensor::PointCloud& low_resolution_point_cloud_in_tracking, + const transform::Rigid3d& pose_estimate, + const Eigen::Quaterniond& gravity_alignment); const proto::LocalTrajectoryBuilderOptions options_; ActiveSubmaps active_submaps_;