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

Removed redundant 2d voxel filtering (#1243) 

Removed redundant adaptive voxel fitering in trajectory builder 2d.

Adaptive voxel filtering of the lidar point cloud was performed in InsertIntoSubmap and ScanMatch. Both methods are called from AddAccumulatedRangeData. Now, adaptive voxel filtering is done only once in AddAccumulatedRangeData and the filtered point cloud is then forwarded to InsertIntoSubmap and ScanMatch.
master
Martin Schwörer 2018-07-09 20:45:55 +02:00 committed by Wally B. Feed
parent 9e54a981c6
commit aa3ac7e837
2 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -62,19 +62,13 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data) {
const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
std::shared_ptr<const Submap2D> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
transform::Rigid2d initial_ceres_pose = pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}
if (options_.use_online_correlative_scan_matching()) {
CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
proto::GridOptions2D_GridType_PROBABILITY_GRID);
@ -222,9 +216,16 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const transform::Rigid2d pose_prediction = transform::Project2D(
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
.Filter(gravity_aligned_range_data.returns);
if (filtered_gravity_aligned_point_cloud.empty()) {
return nullptr;
}
// local map frame <- gravity-aligned frame
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
ScanMatch(time, pose_prediction, gravity_aligned_range_data);
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
if (pose_estimate_2d == nullptr) {
LOG(WARNING) << "Scan matching failed.";
return nullptr;
@ -236,9 +237,9 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
sensor::RangeData range_data_in_local =
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d->cast<float>()));
std::unique_ptr<InsertionResult> insertion_result =
InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
pose_estimate, gravity_alignment.rotation());
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
auto duration = std::chrono::steady_clock::now() - accumulation_started_;
kLocalSlamLatencyMetric->Set(
std::chrono::duration_cast<std::chrono::duration<double>>(duration)
@ -251,7 +252,7 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_local,
const sensor::RangeData& gravity_aligned_range_data,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment) {
if (motion_filter_.IsSimilar(time, pose_estimate)) {
@ -266,11 +267,6 @@ LocalTrajectoryBuilder2D::InsertIntoSubmap(
}
active_submaps_.InsertRangeData(range_data_in_local);
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.loop_closure_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
return common::make_unique<InsertionResult>(InsertionResult{
std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
time,

9
cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
View File

@ -83,18 +83,17 @@ class LocalTrajectoryBuilder2D {
sensor::RangeData TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const;
std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_local,
const sensor::RangeData& gravity_aligned_range_data,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment);
// Scan matches 'gravity_aligned_range_data' and returns the observed pose,
// or nullptr on failure.
// Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
// observed pose, or nullptr on failure.
std::unique_ptr<transform::Rigid2d> ScanMatch(
common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data);
const sensor::PointCloud& filtered_gravity_aligned_point_cloud);
// Lazily constructs a PoseExtrapolator.
void InitializeExtrapolator(common::Time time);