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

Add filtered point clouds to TrajectoryNode::Data for 3D. (#482)

master
jie 2017-08-25 16:56:50 +02:00 committed by Wolfgang Hess
parent 0837d4b228
commit 42d8a8f005
13 changed files with 63 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

4
cartographer/mapping/trajectory_node.h
View File

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

5
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -106,7 +106,10 @@ void SparsePoseGraph::AddScan(
trajectory_id,
mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
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_;

10
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

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

2
cartographer/mapping_3d/global_trajectory_builder.cc
View File

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

13
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -162,7 +162,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
sensor::TransformPointCloud(filtered_range_data.returns,
pose_estimate.cast<float>())};
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::InsertionResult>
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<float>()),
extrapolator_->gravity_orientation());
return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation,
std::move(insertion_submaps)});
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, high_resolution_point_cloud,
low_resolution_point_cloud, pose_observation,
std::move(insertion_submaps)});
}
} // namespace mapping_3d

7
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -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<std::shared_ptr<const Submap>> insertion_submaps;
};
@ -66,6 +71,8 @@ class LocalTrajectoryBuilder {
std::unique_ptr<InsertionResult> 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_;

25
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -94,18 +94,22 @@ std::vector<mapping::SubmapId> 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<std::shared_ptr<const Submap>>& 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<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, sensor::Compress(range_data_in_tracking),
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);
@ -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);
}
}
}

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

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

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

@ -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<mapping::TrajectoryNode>& 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<mapping::TrajectoryNode>& 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<OptimizationProblem::Constraint>* 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";

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

@ -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<mapping::TrajectoryNode>& 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<mapping::TrajectoryNode>& 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>* 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.

10
configuration_files/sparse_pose_graph.lua
View File

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