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

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

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

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.

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

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

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

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

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

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

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_);

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

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.

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