12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use separate voxel filter options for 3D loop closure. (#465)

master
zjwoody 2017-08-21 18:01:23 +02:00 committed by Wolfgang Hess
parent edfd6ac8d7
commit ac693f3e04
4 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto
View File

@ -30,6 +30,7 @@ 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;
@ -60,7 +61,7 @@ message ConstraintBuilderOptions {
optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
fast_correlative_scan_matcher_options_3d = 10;
// Voxel filter used for high resolution, 3D loop closure refinement.
// Voxel filter used for high resolution, only used for 3D loop closure.
optional sensor.proto.AdaptiveVoxelFilterOptions
high_resolution_adaptive_voxel_filter_options = 15;

26
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -44,7 +44,10 @@ ConstraintBuilder::ConstraintBuilder(
: options_(options),
thread_pool_(thread_pool),
sampler_(options.sampling_ratio()),
adaptive_voxel_filter_(options.adaptive_voxel_filter_options()),
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() {
@ -175,11 +178,10 @@ void ConstraintBuilder::ComputeConstraint(
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id);
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(point_cloud);
const sensor::PointCloud high_resolution_point_cloud =
high_resolution_adaptive_voxel_filter_.Filter(point_cloud);
// The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'filtered_point_cloud' in scan j and
// - a 'high_resolution_point_cloud' in scan j and
// - the initial guess 'initial_pose' (submap i <- scan j).
float score = 0.f;
transform::Rigid3d pose_estimate;
@ -191,7 +193,7 @@ void ConstraintBuilder::ComputeConstraint(
// 3. Refine.
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
initial_pose.rotation(), filtered_point_cloud, point_cloud,
initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate,
&rotational_score)) {
CHECK_GT(score, options_.global_localization_min_score());
@ -204,7 +206,7 @@ void ConstraintBuilder::ComputeConstraint(
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, point_cloud,
initial_pose, high_resolution_point_cloud, point_cloud,
options_.min_score(), &score, &pose_estimate, &rotational_score)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
@ -221,14 +223,8 @@ void ConstraintBuilder::ComputeConstraint(
// Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud high_resolution_point_cloud =
adaptive_voxel_filter.Filter(point_cloud);
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
options_.low_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud low_resolution_point_cloud =
low_resolution_adaptive_voxel_filter.Filter(point_cloud);
low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform;
@ -248,7 +244,7 @@ void ConstraintBuilder::ComputeConstraint(
if (options_.log_matches()) {
std::ostringstream info;
info << "Node " << node_id << " with " << filtered_point_cloud.size()
info << "Node " << node_id << " with " << high_resolution_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";

3
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -183,7 +183,8 @@ class ConstraintBuilder {
submap_queued_work_items_ GUARDED_BY(mutex_);
common::FixedRatioSampler sampler_;
const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
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.

3
docs/source/configuration.rst
View File

@ -103,6 +103,7 @@ 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
@ -133,7 +134,7 @@ cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fa
Not yet documented.
cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options
Voxel filter used for high resolution, 3D loop closure refinement.
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.