From 42d8a8f00530a9ca2a794ac9ae0c239b61b9644a Mon Sep 17 00:00:00 2001 From: jie Date: Fri, 25 Aug 2017 16:56:50 +0200 Subject: [PATCH] Add filtered point clouds to TrajectoryNode::Data for 3D. (#482) --- .../sparse_pose_graph/constraint_builder.cc | 10 ----- .../proto/constraint_builder_options.proto | 8 ---- cartographer/mapping/trajectory_node.h | 4 ++ cartographer/mapping_2d/sparse_pose_graph.cc | 5 ++- .../mapping_2d/sparse_pose_graph_test.cc | 10 ----- .../mapping_3d/global_trajectory_builder.cc | 2 + .../mapping_3d/local_trajectory_builder.cc | 13 ++++-- .../mapping_3d/local_trajectory_builder.h | 7 ++++ cartographer/mapping_3d/sparse_pose_graph.cc | 25 ++++++----- cartographer/mapping_3d/sparse_pose_graph.h | 2 + .../sparse_pose_graph/constraint_builder.cc | 41 ++++++++----------- .../sparse_pose_graph/constraint_builder.h | 8 ++-- configuration_files/sparse_pose_graph.lua | 10 ----- 13 files changed, 63 insertions(+), 82 deletions(-) diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index 2a6d72e..8a04cef 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -55,16 +55,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( parameter_dictionary ->GetDictionary("fast_correlative_scan_matcher_3d") .get()); - *options.mutable_high_resolution_adaptive_voxel_filter_options() = - sensor::CreateAdaptiveVoxelFilterOptions( - parameter_dictionary - ->GetDictionary("high_resolution_adaptive_voxel_filter") - .get()); - *options.mutable_low_resolution_adaptive_voxel_filter_options() = - sensor::CreateAdaptiveVoxelFilterOptions( - parameter_dictionary - ->GetDictionary("low_resolution_adaptive_voxel_filter") - .get()); *options.mutable_ceres_scan_matcher_options_3d() = mapping_3d::scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get()); diff --git a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto index 8e293ad..d774311 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -61,14 +61,6 @@ message ConstraintBuilderOptions { optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d = 10; - // Voxel filter used for high resolution, only used for 3D loop closure. - optional sensor.proto.AdaptiveVoxelFilterOptions - high_resolution_adaptive_voxel_filter_options = 15; - - // Voxel filter used for low resolution, 3D loop closure refinement. - optional sensor.proto.AdaptiveVoxelFilterOptions - low_resolution_adaptive_voxel_filter_options = 16; - optional mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d = 12; } diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index a56ff57..b9e8914 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -35,6 +35,10 @@ struct TrajectoryNode { // Range data in 'pose' frame. sensor::CompressedRangeData range_data; + // Used for loop closure in 3D. + sensor::PointCloud high_resolution_point_cloud; + sensor::PointCloud low_resolution_point_cloud; + // Transform from the 3D 'tracking' frame to the 'pose' frame of the range // data, which contains roll, pitch and height for 2D. In 3D this is always // identity. diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index d62946c..e5425fc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -106,7 +106,10 @@ void SparsePoseGraph::AddScan( trajectory_id, mapping::TrajectoryNode{ std::make_shared( - mapping::TrajectoryNode::Data{time, Compress(range_data_in_pose), + mapping::TrajectoryNode::Data{time, + Compress(range_data_in_pose), + {}, + {}, tracking_to_pose}), optimized_pose}); ++num_trajectory_nodes_; diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index da18246..c7bece5 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -104,16 +104,6 @@ class SparsePoseGraphTest : public ::testing::Test { linear_z_search_window = 4., angular_search_window = 0.1, }, - high_resolution_adaptive_voxel_filter = { - max_length = 2., - min_num_points = 150, - max_range = 15., - }, - low_resolution_adaptive_voxel_filter = { - max_length = 4., - min_num_points = 200, - max_range = 60., - }, ceres_scan_matcher_3d = { occupied_space_weight_0 = 20., translation_weight = 10., diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index f15ef58..ce9c30a 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -43,6 +43,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData( } sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->range_data_in_tracking, + insertion_result->high_resolution_point_cloud, + insertion_result->low_resolution_point_cloud, insertion_result->pose_observation, trajectory_id_, insertion_result->insertion_submaps); } diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 9718636..89857f3 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -162,7 +162,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( sensor::TransformPointCloud(filtered_range_data.returns, pose_estimate.cast())}; - return InsertIntoSubmap(time, filtered_range_data, pose_estimate); + return InsertIntoSubmap( + time, filtered_range_data, filtered_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, pose_estimate); } void LocalTrajectoryBuilder::AddOdometerData( @@ -182,6 +184,8 @@ const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { std::unique_ptr LocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud, + const sensor::PointCloud& low_resolution_point_cloud, const transform::Rigid3d& pose_observation) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; @@ -196,9 +200,10 @@ LocalTrajectoryBuilder::InsertIntoSubmap( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), extrapolator_->gravity_orientation()); - return std::unique_ptr( - new InsertionResult{time, range_data_in_tracking, pose_observation, - std::move(insertion_submaps)}); + return std::unique_ptr(new InsertionResult{ + time, range_data_in_tracking, high_resolution_point_cloud, + low_resolution_point_cloud, pose_observation, + std::move(insertion_submaps)}); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index e20c0c0..338400f 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -41,6 +41,11 @@ class LocalTrajectoryBuilder { struct InsertionResult { common::Time time; sensor::RangeData range_data_in_tracking; + + // Used for loop closure in 3D. + sensor::PointCloud high_resolution_point_cloud; + sensor::PointCloud low_resolution_point_cloud; + transform::Rigid3d pose_observation; std::vector> insertion_submaps; }; @@ -66,6 +71,8 @@ class LocalTrajectoryBuilder { std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud, + const sensor::PointCloud& low_resolution_point_cloud, const transform::Rigid3d& pose_observation); const proto::LocalTrajectoryBuilderOptions options_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 9f9ef10..9f7aabc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -94,18 +94,22 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud, + const sensor::PointCloud& low_resolution_point_cloud, const transform::Rigid3d& pose, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); trajectory_nodes_.Append( - trajectory_id, mapping::TrajectoryNode{ - std::make_shared( - mapping::TrajectoryNode::Data{ - time, sensor::Compress(range_data_in_tracking), - transform::Rigid3d::Identity()}), - optimized_pose}); + trajectory_id, + mapping::TrajectoryNode{ + std::make_shared( + mapping::TrajectoryNode::Data{ + time, sensor::Compress(range_data_in_tracking), + high_resolution_point_cloud, low_resolution_point_cloud, + transform::Rigid3d::Identity()}), + optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); @@ -206,9 +210,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, - &trajectory_nodes_.at(node_id).constant_data->range_data.returns, - submap_nodes, initial_relative_pose.rotation(), - &trajectory_connectivity_); + trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, + initial_relative_pose.rotation(), &trajectory_connectivity_); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(node_id.trajectory_id) > 0 && @@ -219,8 +222,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, - &trajectory_nodes_.at(node_id).constant_data->range_data.returns, - submap_nodes, initial_relative_pose); + trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, + initial_relative_pose); } } } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index efdb268..060c4e9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -71,6 +71,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // this submap was inserted into for the last time. void AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, + const sensor::PointCloud& high_resolution_point_cloud, + const sensor::PointCloud& low_resolution_point_cloud, const transform::Rigid3d& pose, int trajectory_id, const std::vector>& insertion_submaps) EXCLUDES(mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 32a70c7..d165f24 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -45,10 +45,6 @@ ConstraintBuilder::ConstraintBuilder( : options_(options), thread_pool_(thread_pool), sampler_(options.sampling_ratio()), - high_resolution_adaptive_voxel_filter_( - options.high_resolution_adaptive_voxel_filter_options()), - low_resolution_adaptive_voxel_filter_( - options.low_resolution_adaptive_voxel_filter_options()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder::~ConstraintBuilder() { @@ -62,7 +58,7 @@ ConstraintBuilder::~ConstraintBuilder() { void ConstraintBuilder::MaybeAddConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, - const sensor::CompressedPointCloud* const compressed_point_cloud, + const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, const transform::Rigid3d& initial_pose) { if (initial_pose.translation().norm() > options_.max_constraint_distance()) { @@ -78,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint( submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ - compressed_point_cloud, initial_pose, constraint); + constant_data, initial_pose, constraint); FinishComputation(current_computation); }); } @@ -87,7 +83,7 @@ void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, - const sensor::CompressedPointCloud* const compressed_point_cloud, + const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, const Eigen::Quaterniond& gravity_alignment, mapping::TrajectoryConnectivity* const trajectory_connectivity) { @@ -99,7 +95,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ - trajectory_connectivity, compressed_point_cloud, + trajectory_connectivity, constant_data, transform::Rigid3d::Rotation(gravity_alignment), constraint); FinishComputation(current_computation); @@ -172,16 +168,13 @@ void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, - const sensor::CompressedPointCloud* const compressed_point_cloud, + const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid3d& initial_pose, std::unique_ptr* constraint) { const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); - const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress(); - const sensor::PointCloud high_resolution_point_cloud = - high_resolution_adaptive_voxel_filter_.Filter(point_cloud); - const sensor::PointCloud low_resolution_point_cloud = - low_resolution_adaptive_voxel_filter_.Filter(point_cloud); + const sensor::PointCloud point_cloud = + constant_data->range_data.returns.Decompress(); // The 'constraint_transform' (submap i <- scan j) is computed from: // - a 'high_resolution_point_cloud' in scan j and @@ -193,7 +186,7 @@ void ConstraintBuilder::ComputeConstraint( const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( submap_scan_matcher->low_resolution_hybrid_grid, - &low_resolution_point_cloud); + &constant_data->low_resolution_point_cloud); // Compute 'pose_estimate' in three stages: // 1. Fast estimate using the fast correlative scan matcher. @@ -201,9 +194,10 @@ void ConstraintBuilder::ComputeConstraint( // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( - initial_pose.rotation(), high_resolution_point_cloud, point_cloud, - options_.global_localization_min_score(), low_resolution_matcher, - &score, &pose_estimate, &rotational_score, &low_resolution_score)) { + initial_pose.rotation(), constant_data->high_resolution_point_cloud, + point_cloud, options_.global_localization_min_score(), + low_resolution_matcher, &score, &pose_estimate, &rotational_score, + &low_resolution_score)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); @@ -214,8 +208,8 @@ void ConstraintBuilder::ComputeConstraint( } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, high_resolution_point_cloud, point_cloud, - options_.min_score(), low_resolution_matcher, &score, + initial_pose, constant_data->high_resolution_point_cloud, + point_cloud, options_.min_score(), low_resolution_matcher, &score, &pose_estimate, &rotational_score, &low_resolution_score)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); @@ -236,9 +230,9 @@ void ConstraintBuilder::ComputeConstraint( ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; ceres_scan_matcher_.Match(pose_estimate, pose_estimate, - {{&high_resolution_point_cloud, + {{&constant_data->high_resolution_point_cloud, submap_scan_matcher->high_resolution_hybrid_grid}, - {&low_resolution_point_cloud, + {&constant_data->low_resolution_point_cloud, submap_scan_matcher->low_resolution_hybrid_grid}}, &constraint_transform, &unused_summary); @@ -251,7 +245,8 @@ void ConstraintBuilder::ComputeConstraint( if (options_.log_matches()) { std::ostringstream info; - info << "Node " << node_id << " with " << high_resolution_point_cloud.size() + info << "Node " << node_id << " with " + << constant_data->high_resolution_point_cloud.size() << " points on submap " << submap_id << std::fixed; if (match_full_submap) { info << " matches"; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 03e914a..4693f37 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -76,7 +76,7 @@ class ConstraintBuilder { void MaybeAddConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, - const sensor::CompressedPointCloud* compressed_point_cloud, + const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, const transform::Rigid3d& initial_pose); @@ -93,7 +93,7 @@ class ConstraintBuilder { void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, - const sensor::CompressedPointCloud* compressed_point_cloud, + const mapping::TrajectoryNode::Data* const constant_data, const std::vector& submap_nodes, const Eigen::Quaterniond& gravity_alignment, mapping::TrajectoryConnectivity* trajectory_connectivity); @@ -144,7 +144,7 @@ class ConstraintBuilder { const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, - const sensor::CompressedPointCloud* compressed_point_cloud, + const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid3d& initial_pose, std::unique_ptr* constraint) EXCLUDES(mutex_); @@ -183,8 +183,6 @@ class ConstraintBuilder { submap_queued_work_items_ GUARDED_BY(mutex_); common::FixedRatioSampler sampler_; - const sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter_; - const sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter_; scan_matching::CeresScanMatcher ceres_scan_matcher_; // Histograms of scan matcher scores. diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index e8e2a30..2bdcffc 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -52,16 +52,6 @@ SPARSE_POSE_GRAPH = { linear_z_search_window = 1., angular_search_window = math.rad(15.), }, - high_resolution_adaptive_voxel_filter = { - max_length = 2., - min_num_points = 150, - max_range = 15., - }, - low_resolution_adaptive_voxel_filter = { - max_length = 4., - min_num_points = 200, - max_range = 60., - }, ceres_scan_matcher_3d = { occupied_space_weight_0 = 5., occupied_space_weight_1 = 30.,