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 parameter_dictionary
->GetDictionary("fast_correlative_scan_matcher_3d") ->GetDictionary("fast_correlative_scan_matcher_3d")
.get()); .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() = *options.mutable_ceres_scan_matcher_options_3d() =
mapping_3d::scan_matching::CreateCeresScanMatcherOptions( mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get()); parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get());

View File

@ -61,14 +61,6 @@ message ConstraintBuilderOptions {
optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
fast_correlative_scan_matcher_options_3d = 10; 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 optional mapping_3d.scan_matching.proto.CeresScanMatcherOptions
ceres_scan_matcher_options_3d = 12; ceres_scan_matcher_options_3d = 12;
} }

View File

@ -35,6 +35,10 @@ struct TrajectoryNode {
// Range data in 'pose' frame. // Range data in 'pose' frame.
sensor::CompressedRangeData range_data; 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 // 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 // data, which contains roll, pitch and height for 2D. In 3D this is always
// identity. // identity.

View File

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

View File

@ -104,16 +104,6 @@ class SparsePoseGraphTest : public ::testing::Test {
linear_z_search_window = 4., linear_z_search_window = 4.,
angular_search_window = 0.1, 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 = { ceres_scan_matcher_3d = {
occupied_space_weight_0 = 20., occupied_space_weight_0 = 20.,
translation_weight = 10., translation_weight = 10.,

View File

@ -43,6 +43,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
} }
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking, 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->pose_observation, trajectory_id_,
insertion_result->insertion_submaps); insertion_result->insertion_submaps);
} }

View File

@ -162,7 +162,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
sensor::TransformPointCloud(filtered_range_data.returns, sensor::TransformPointCloud(filtered_range_data.returns,
pose_estimate.cast<float>())}; 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( void LocalTrajectoryBuilder::AddOdometerData(
@ -182,6 +184,8 @@ const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::InsertIntoSubmap( LocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking, 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) { const transform::Rigid3d& pose_observation) {
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr; return nullptr;
@ -196,9 +200,10 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
sensor::TransformRangeData(range_data_in_tracking, sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()), pose_observation.cast<float>()),
extrapolator_->gravity_orientation()); extrapolator_->gravity_orientation());
return std::unique_ptr<InsertionResult>( return std::unique_ptr<InsertionResult>(new InsertionResult{
new InsertionResult{time, range_data_in_tracking, pose_observation, time, range_data_in_tracking, high_resolution_point_cloud,
std::move(insertion_submaps)}); low_resolution_point_cloud, pose_observation,
std::move(insertion_submaps)});
} }
} // namespace mapping_3d } // namespace mapping_3d

View File

@ -41,6 +41,11 @@ class LocalTrajectoryBuilder {
struct InsertionResult { struct InsertionResult {
common::Time time; common::Time time;
sensor::RangeData range_data_in_tracking; 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; transform::Rigid3d pose_observation;
std::vector<std::shared_ptr<const Submap>> insertion_submaps; std::vector<std::shared_ptr<const Submap>> insertion_submaps;
}; };
@ -66,6 +71,8 @@ class LocalTrajectoryBuilder {
std::unique_ptr<InsertionResult> InsertIntoSubmap( std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_tracking, 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 transform::Rigid3d& pose_observation);
const proto::LocalTrajectoryBuilderOptions options_; const proto::LocalTrajectoryBuilderOptions options_;

View File

@ -94,18 +94,22 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan( void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking, 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 transform::Rigid3d& pose, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose( const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose); GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append( trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{ trajectory_id,
std::make_shared<const mapping::TrajectoryNode::Data>( mapping::TrajectoryNode{
mapping::TrajectoryNode::Data{ std::make_shared<const mapping::TrajectoryNode::Data>(
time, sensor::Compress(range_data_in_tracking), mapping::TrajectoryNode::Data{
transform::Rigid3d::Identity()}), time, sensor::Compress(range_data_in_tracking),
optimized_pose}); high_resolution_point_cloud, low_resolution_point_cloud,
transform::Rigid3d::Identity()}),
optimized_pose});
++num_trajectory_nodes_; ++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id); 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. // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, 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(), submap_nodes,
submap_nodes, initial_relative_pose.rotation(), initial_relative_pose.rotation(), &trajectory_connectivity_);
&trajectory_connectivity_);
} else { } else {
const bool scan_and_submap_trajectories_connected = const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 && 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) { scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, 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(), submap_nodes,
submap_nodes, initial_relative_pose); initial_relative_pose);
} }
} }
} }

View File

@ -71,6 +71,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// this submap was inserted into for the last time. // this submap was inserted into for the last time.
void AddScan( void AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking, 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 transform::Rigid3d& pose, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_); EXCLUDES(mutex_);

View File

@ -45,10 +45,6 @@ ConstraintBuilder::ConstraintBuilder(
: options_(options), : options_(options),
thread_pool_(thread_pool), thread_pool_(thread_pool),
sampler_(options.sampling_ratio()), 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()) {} ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {}
ConstraintBuilder::~ConstraintBuilder() { ConstraintBuilder::~ConstraintBuilder() {
@ -62,7 +58,7 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, 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 std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_pose) { const transform::Rigid3d& initial_pose) {
if (initial_pose.translation().norm() > options_.max_constraint_distance()) { if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
@ -78,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */ ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */ nullptr, /* trajectory_connectivity */
compressed_point_cloud, initial_pose, constraint); constant_data, initial_pose, constraint);
FinishComputation(current_computation); FinishComputation(current_computation);
}); });
} }
@ -87,7 +83,7 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, 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 std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment, const Eigen::Quaterniond& gravity_alignment,
mapping::TrajectoryConnectivity* const trajectory_connectivity) { mapping::TrajectoryConnectivity* const trajectory_connectivity) {
@ -99,7 +95,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */ ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
trajectory_connectivity, compressed_point_cloud, trajectory_connectivity, constant_data,
transform::Rigid3d::Rotation(gravity_alignment), transform::Rigid3d::Rotation(gravity_alignment),
constraint); constraint);
FinishComputation(current_computation); FinishComputation(current_computation);
@ -172,16 +168,13 @@ void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud, const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& initial_pose, const transform::Rigid3d& initial_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) { std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
const SubmapScanMatcher* const submap_scan_matcher = const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id); GetSubmapScanMatcher(submap_id);
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress(); const sensor::PointCloud point_cloud =
const sensor::PointCloud high_resolution_point_cloud = constant_data->range_data.returns.Decompress();
high_resolution_adaptive_voxel_filter_.Filter(point_cloud);
const sensor::PointCloud low_resolution_point_cloud =
low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
// The 'constraint_transform' (submap i <- scan j) is computed from: // The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'high_resolution_point_cloud' in scan j and // - a 'high_resolution_point_cloud' in scan j and
@ -193,7 +186,7 @@ void ConstraintBuilder::ComputeConstraint(
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher( const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
submap_scan_matcher->low_resolution_hybrid_grid, submap_scan_matcher->low_resolution_hybrid_grid,
&low_resolution_point_cloud); &constant_data->low_resolution_point_cloud);
// Compute 'pose_estimate' in three stages: // Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher. // 1. Fast estimate using the fast correlative scan matcher.
@ -201,9 +194,10 @@ void ConstraintBuilder::ComputeConstraint(
// 3. Refine. // 3. Refine.
if (match_full_submap) { if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
initial_pose.rotation(), high_resolution_point_cloud, point_cloud, initial_pose.rotation(), constant_data->high_resolution_point_cloud,
options_.global_localization_min_score(), low_resolution_matcher, point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate, &rotational_score, &low_resolution_score)) { low_resolution_matcher, &score, &pose_estimate, &rotational_score,
&low_resolution_score)) {
CHECK_GT(score, options_.global_localization_min_score()); CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0);
@ -214,8 +208,8 @@ void ConstraintBuilder::ComputeConstraint(
} }
} else { } else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match( if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, high_resolution_point_cloud, point_cloud, initial_pose, constant_data->high_resolution_point_cloud,
options_.min_score(), low_resolution_matcher, &score, point_cloud, options_.min_score(), low_resolution_matcher, &score,
&pose_estimate, &rotational_score, &low_resolution_score)) { &pose_estimate, &rotational_score, &low_resolution_score)) {
// We've reported a successful local match. // We've reported a successful local match.
CHECK_GT(score, options_.min_score()); CHECK_GT(score, options_.min_score());
@ -236,9 +230,9 @@ void ConstraintBuilder::ComputeConstraint(
ceres::Solver::Summary unused_summary; ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform; transform::Rigid3d constraint_transform;
ceres_scan_matcher_.Match(pose_estimate, pose_estimate, 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}, submap_scan_matcher->high_resolution_hybrid_grid},
{&low_resolution_point_cloud, {&constant_data->low_resolution_point_cloud,
submap_scan_matcher->low_resolution_hybrid_grid}}, submap_scan_matcher->low_resolution_hybrid_grid}},
&constraint_transform, &unused_summary); &constraint_transform, &unused_summary);
@ -251,7 +245,8 @@ void ConstraintBuilder::ComputeConstraint(
if (options_.log_matches()) { if (options_.log_matches()) {
std::ostringstream info; 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; << " points on submap " << submap_id << std::fixed;
if (match_full_submap) { if (match_full_submap) {
info << " matches"; info << " matches";

View File

@ -76,7 +76,7 @@ class ConstraintBuilder {
void MaybeAddConstraint( void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, 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 std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_pose); const transform::Rigid3d& initial_pose);
@ -93,7 +93,7 @@ class ConstraintBuilder {
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, 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 std::vector<mapping::TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& gravity_alignment, const Eigen::Quaterniond& gravity_alignment,
mapping::TrajectoryConnectivity* trajectory_connectivity); mapping::TrajectoryConnectivity* trajectory_connectivity);
@ -144,7 +144,7 @@ class ConstraintBuilder {
const mapping::SubmapId& submap_id, const mapping::NodeId& node_id, const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
bool match_full_submap, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* compressed_point_cloud, const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& initial_pose, const transform::Rigid3d& initial_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_); std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
@ -183,8 +183,6 @@ class ConstraintBuilder {
submap_queued_work_items_ GUARDED_BY(mutex_); submap_queued_work_items_ GUARDED_BY(mutex_);
common::FixedRatioSampler sampler_; 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_; scan_matching::CeresScanMatcher ceres_scan_matcher_;
// Histograms of scan matcher scores. // Histograms of scan matcher scores.

View File

@ -52,16 +52,6 @@ SPARSE_POSE_GRAPH = {
linear_z_search_window = 1., linear_z_search_window = 1.,
angular_search_window = math.rad(15.), 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 = { ceres_scan_matcher_3d = {
occupied_space_weight_0 = 5., occupied_space_weight_0 = 5.,
occupied_space_weight_1 = 30., occupied_space_weight_1 = 30.,