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

Add filtered point cloud to TrajectoryNode::Data for 2D. (#483)

master
Wolfgang Hess 2017-08-28 11:17:53 +02:00 committed by GitHub
parent 42d8a8f005
commit a7fe8bd2ab
17 changed files with 73 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/sparse_pose_graph/constraint_builder.cc
View File

@ -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"));

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

@ -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;

3
cartographer/mapping/trajectory_node.h
View File

@ -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;

1
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -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);
}

12
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -181,9 +181,15 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
TransformRangeData(range_data_in_tracking_2d,
transform::Embed3D(pose_estimate_2d.cast<float>())));
return common::make_unique<InsertionResult>(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>(
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 {

1
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -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(

5
cartographer/mapping_2d/local_trajectory_builder_options.cc
View File

@ -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

5
cartographer/mapping_2d/proto/local_trajectory_builder_options.proto
View File

@ -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;

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -95,8 +95,9 @@ std::vector<mapping::SubmapId> 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<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@ -108,6 +109,7 @@ void SparsePoseGraph::AddScan(
std::make_shared<const mapping::TrajectoryNode::Data>(
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);
}
}

1
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -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<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);

38
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -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<ConstraintBuilder::Constraint>* 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 {

7
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -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>* 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.

7
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -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) {

18
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -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<const mapping::TrajectoryNode::Data>(
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<const mapping::TrajectoryNode::Data>(
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);

5
configuration_files/sparse_pose_graph.lua
View File

@ -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,

6
configuration_files/trajectory_builder_2d.lua
View File

@ -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,

14
docs/source/configuration.rst
View File

@ -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.