diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index 8a04cef..ec57f1a 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -32,9 +32,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( options.set_sampling_ratio(parameter_dictionary->GetDouble("sampling_ratio")); options.set_max_constraint_distance( parameter_dictionary->GetDouble("max_constraint_distance")); - *options.mutable_adaptive_voxel_filter_options() = - sensor::CreateAdaptiveVoxelFilterOptions( - parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); options.set_min_score(parameter_dictionary->GetDouble("min_score")); options.set_global_localization_min_score( parameter_dictionary->GetDouble("global_localization_min_score")); 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 d774311..f09ff50 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -16,7 +16,6 @@ syntax = "proto2"; package cartographer.mapping.sparse_pose_graph.proto; -import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; @@ -30,11 +29,6 @@ message ConstraintBuilderOptions { // Threshold for poses to be considered near a submap. optional double max_constraint_distance = 2; - // This is only used for 2D. - // Voxel filter used to compute a sparser point cloud for matching. - optional sensor.proto.AdaptiveVoxelFilterOptions - adaptive_voxel_filter_options = 3; - // Threshold for the scan match score below which a match is not considered. // Low scores indicate that the scan and map do not look similar. optional double min_score = 4; diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index b9e8914..d63415f 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -35,6 +35,9 @@ struct TrajectoryNode { // Range data in 'pose' frame. sensor::CompressedRangeData range_data; + // Used for loop closure in 2D: voxel filtered returns in 'pose' frame. + sensor::PointCloud filtered_point_cloud; + // Used for loop closure in 3D. sensor::PointCloud high_resolution_point_cloud; sensor::PointCloud low_resolution_point_cloud; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index e1a996a..3d588cc 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -40,6 +40,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData( sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, insertion_result->range_data_in_tracking_2d, + insertion_result->filtered_point_cloud_in_tracking_2d, insertion_result->pose_estimate_2d, trajectory_id_, insertion_result->insertion_submaps); } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index d4980a9..94da92b 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -181,9 +181,15 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( TransformRangeData(range_data_in_tracking_2d, transform::Embed3D(pose_estimate_2d.cast()))); - return common::make_unique(InsertionResult{ - time, std::move(insertion_submaps), tracking_to_tracking_2d, - range_data_in_tracking_2d, pose_estimate_2d}); + sensor::AdaptiveVoxelFilter adaptive_voxel_filter( + options_.loop_closure_adaptive_voxel_filter_options()); + const sensor::PointCloud filtered_point_cloud_in_tracking_2d = + adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns); + + return common::make_unique( + InsertionResult{time, std::move(insertion_submaps), + tracking_to_tracking_2d, range_data_in_tracking_2d, + pose_estimate_2d, filtered_point_cloud_in_tracking_2d}); } const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 78048a3..50177f9 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -43,6 +43,7 @@ class LocalTrajectoryBuilder { transform::Rigid3d tracking_to_tracking_2d; sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; + sensor::PointCloud filtered_point_cloud_in_tracking_2d; }; explicit LocalTrajectoryBuilder( diff --git a/cartographer/mapping_2d/local_trajectory_builder_options.cc b/cartographer/mapping_2d/local_trajectory_builder_options.cc index 1dbf09b..fe05082 100644 --- a/cartographer/mapping_2d/local_trajectory_builder_options.cc +++ b/cartographer/mapping_2d/local_trajectory_builder_options.cc @@ -43,6 +43,11 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( *options.mutable_adaptive_voxel_filter_options() = sensor::CreateAdaptiveVoxelFilterOptions( parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); + *options.mutable_loop_closure_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("loop_closure_adaptive_voxel_filter") + .get()); *options.mutable_real_time_correlative_scan_matcher_options() = scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index 5c3e884..eda7e9f 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto @@ -44,6 +44,11 @@ message LocalTrajectoryBuilderOptions { optional sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 6; + // Voxel filter used to compute a sparser point cloud for finding loop + // closures. + optional sensor.proto.AdaptiveVoxelFilterOptions + loop_closure_adaptive_voxel_filter_options = 20; + // Whether to solve the online scan matching first using the correlative scan // matcher to generate a good starting point for Ceres. optional bool use_online_correlative_scan_matching = 5; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index e5425fc..eed759f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -95,8 +95,9 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, - const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, - const int trajectory_id, + const sensor::RangeData& range_data_in_pose, + const sensor::PointCloud& filtered_point_cloud, + const transform::Rigid2d& pose, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); @@ -108,6 +109,7 @@ void SparsePoseGraph::AddScan( std::make_shared( mapping::TrajectoryNode::Data{time, Compress(range_data_in_pose), + filtered_point_cloud, {}, {}, tracking_to_pose}), @@ -184,7 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, global_localization_samplers_[node_id.trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, - &trajectory_nodes_.at(node_id).constant_data->range_data.returns, + trajectory_nodes_.at(node_id).constant_data.get(), &trajectory_connectivity_); } else { const bool scan_and_submap_trajectories_connected = @@ -206,7 +208,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .point_cloud_pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, - &trajectory_nodes_.at(node_id).constant_data->range_data.returns, + trajectory_nodes_.at(node_id).constant_data.get(), initial_relative_pose); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2717b3e..b940666 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -73,6 +73,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, + const sensor::PointCloud& filtered_point_cloud, const transform::Rigid2d& pose, int trajectory_id, const std::vector>& insertion_submaps) EXCLUDES(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 0d54ee9..3175e85 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -48,7 +48,6 @@ ConstraintBuilder::ConstraintBuilder( : options_(options), thread_pool_(thread_pool), sampler_(options.sampling_ratio()), - adaptive_voxel_filter_(options.adaptive_voxel_filter_options()), ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ConstraintBuilder::~ConstraintBuilder() { @@ -62,7 +61,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 transform::Rigid2d& initial_relative_pose) { if (initial_relative_pose.translation().norm() > options_.max_constraint_distance()) { @@ -76,10 +75,10 @@ void ConstraintBuilder::MaybeAddConstraint( const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { - ComputeConstraint( - submap_id, submap, node_id, false, /* match_full_submap */ - nullptr, /* trajectory_connectivity */ - compressed_point_cloud, initial_relative_pose, constraint); + ComputeConstraint(submap_id, submap, node_id, + false, /* match_full_submap */ + nullptr, /* trajectory_connectivity */ + constant_data, initial_relative_pose, constraint); FinishComputation(current_computation); }); } @@ -88,7 +87,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, mapping::TrajectoryConnectivity* const trajectory_connectivity) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); @@ -99,7 +98,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ - trajectory_connectivity, compressed_point_cloud, + trajectory_connectivity, constant_data, transform::Rigid2d::Identity(), constraint); FinishComputation(current_computation); }); @@ -164,15 +163,13 @@ void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, 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::Rigid2d& initial_relative_pose, std::unique_ptr* constraint) { const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose; const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); - const sensor::PointCloud filtered_point_cloud = - adaptive_voxel_filter_.Filter(compressed_point_cloud->Decompress()); // The 'constraint_transform' (submap i <- scan j) is computed from: // - a 'filtered_point_cloud' in scan j, @@ -188,8 +185,8 @@ void ConstraintBuilder::ComputeConstraint( // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( - filtered_point_cloud, options_.global_localization_min_score(), - &score, &pose_estimate)) { + constant_data->filtered_point_cloud, + options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); @@ -200,8 +197,8 @@ void ConstraintBuilder::ComputeConstraint( } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, filtered_point_cloud, options_.min_score(), &score, - &pose_estimate)) { + initial_pose, constant_data->filtered_point_cloud, + options_.min_score(), &score, &pose_estimate)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { @@ -217,9 +214,9 @@ void ConstraintBuilder::ComputeConstraint( // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; - ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud, - *submap_scan_matcher->probability_grid, - &pose_estimate, &unused_summary); + ceres_scan_matcher_.Match( + pose_estimate, pose_estimate, constant_data->filtered_point_cloud, + *submap_scan_matcher->probability_grid, &pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; @@ -232,8 +229,9 @@ void ConstraintBuilder::ComputeConstraint( if (options_.log_matches()) { std::ostringstream info; - info << "Node " << node_id << " with " << filtered_point_cloud.size() - << " points on submap " << submap_id << std::fixed; + info << "Node " << node_id << " with " + << constant_data->filtered_point_cloud.size() << " points on submap " + << submap_id << std::fixed; if (match_full_submap) { info << " matches"; } else { diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index 6f9e1c0..e9cb33f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -80,7 +80,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 transform::Rigid2d& initial_relative_pose); // Schedules exploring a new constraint between 'submap' identified by @@ -94,7 +94,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, mapping::TrajectoryConnectivity* trajectory_connectivity); // Must be called after all computations related to one node have been added. @@ -142,7 +142,7 @@ class ConstraintBuilder { const mapping::SubmapId& submap_id, const Submap* submap, 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::Rigid2d& initial_relative_pose, std::unique_ptr* constraint) EXCLUDES(mutex_); @@ -181,7 +181,6 @@ class ConstraintBuilder { submap_queued_work_items_ GUARDED_BY(mutex_); common::FixedRatioSampler sampler_; - const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; scan_matching::CeresScanMatcher ceres_scan_matcher_; // Histogram of scan matcher scores. diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index c7bece5..f92cd47 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -69,11 +69,6 @@ class SparsePoseGraphTest : public ::testing::Test { constraint_builder = { sampling_ratio = 1., max_constraint_distance = 6., - adaptive_voxel_filter = { - max_length = 1e-2, - min_num_points = 1000, - max_range = 50., - }, min_score = 0.5, global_localization_min_score = 0.6, loop_closure_translation_weight = 1., @@ -164,7 +159,7 @@ class SparsePoseGraphTest : public ::testing::Test { sparse_pose_graph_->AddScan( common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, - pose_estimate, kTrajectoryId, insertion_submaps); + range_data.returns, pose_estimate, kTrajectoryId, insertion_submaps); } void MoveRelative(const transform::Rigid2d& movement) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 9f7aabc..a1ec50c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -102,14 +102,16 @@ void SparsePoseGraph::AddScan( 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), - high_resolution_point_cloud, low_resolution_point_cloud, - 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); diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 2bdcffc..0de0012 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -17,11 +17,6 @@ SPARSE_POSE_GRAPH = { constraint_builder = { sampling_ratio = 0.3, max_constraint_distance = 15., - adaptive_voxel_filter = { - max_length = 0.9, - min_num_points = 100, - max_range = 50., - }, min_score = 0.55, global_localization_min_score = 0.6, loop_closure_translation_weight = 1.1e4, diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 6c16b40..d62d0b6 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -28,6 +28,12 @@ TRAJECTORY_BUILDER_2D = { max_range = 50., }, + loop_closure_adaptive_voxel_filter = { + max_length = 0.9, + min_num_points = 100, + max_range = 50., + }, + use_online_correlative_scan_matching = false, real_time_correlative_scan_matcher = { linear_search_window = 0.1, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index dbfb469..bc24457 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -105,10 +105,6 @@ double sampling_ratio double max_constraint_distance Threshold for poses to be considered near a submap. -cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options - This is only used for 2D. - Voxel filter used to compute a sparser point cloud for matching. - double min_score Threshold for the scan match score below which a match is not considered. Low scores indicate that the scan and map do not look similar. @@ -136,12 +132,6 @@ cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_m cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d Not yet documented. -cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options - Voxel filter used for high resolution, only used for 3D loop closure. - -cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options - Voxel filter used for low resolution, 3D loop closure refinement. - cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d Not yet documented. @@ -206,6 +196,10 @@ float voxel_filter_size cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options Voxel filter used to compute a sparser point cloud for matching. +cartographer.sensor.proto.AdaptiveVoxelFilterOptions loop_closure_adaptive_voxel_filter_options + Voxel filter used to compute a sparser point cloud for finding loop + closures. + bool use_online_correlative_scan_matching Whether to solve the online scan matching first using the correlative scan matcher to generate a good starting point for Ceres.