Check if filtered point clouds are empty. (#664)
The point cloud will be filtered by MaxRangeFilter before doing scan matching. For some special cases, the filtered point cloud maybe empty, it will cause crash: real_time_correlative_scan_matcher.cc:159 Check failed: candidate.score > 0.f (nan vs. 0) This PR fixed this bug, check the point cloud size before scan matching, return false for empty point cloud.master
parent
b2e948d8a2
commit
a08a370ef3
cartographer
mapping_3d
|
@ -48,10 +48,9 @@ sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
|
|||
sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())};
|
||||
}
|
||||
|
||||
void LocalTrajectoryBuilder::ScanMatch(
|
||||
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
|
||||
const common::Time time, const transform::Rigid2d& pose_prediction,
|
||||
const sensor::RangeData& gravity_aligned_range_data,
|
||||
transform::Rigid2d* const pose_observation) {
|
||||
const sensor::RangeData& gravity_aligned_range_data) {
|
||||
std::shared_ptr<const Submap> matching_submap =
|
||||
active_submaps_.submaps().front();
|
||||
// The online correlative scan matcher will refine the initial estimate for
|
||||
|
@ -61,16 +60,21 @@ void LocalTrajectoryBuilder::ScanMatch(
|
|||
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()) {
|
||||
real_time_correlative_scan_matcher_.Match(
|
||||
pose_prediction, filtered_gravity_aligned_point_cloud,
|
||||
matching_submap->probability_grid(), &initial_ceres_pose);
|
||||
}
|
||||
|
||||
auto pose_observation = common::make_unique<transform::Rigid2d>();
|
||||
ceres::Solver::Summary summary;
|
||||
ceres_scan_matcher_.Match(
|
||||
pose_prediction, initial_ceres_pose, filtered_gravity_aligned_point_cloud,
|
||||
matching_submap->probability_grid(), pose_observation, &summary);
|
||||
matching_submap->probability_grid(), pose_observation.get(), &summary);
|
||||
return pose_observation;
|
||||
}
|
||||
|
||||
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
||||
|
@ -149,16 +153,19 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
|||
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
|
||||
|
||||
// local map frame <- gravity-aligned frame
|
||||
transform::Rigid2d pose_estimate_2d;
|
||||
ScanMatch(time, pose_prediction, gravity_aligned_range_data,
|
||||
&pose_estimate_2d);
|
||||
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
|
||||
ScanMatch(time, pose_prediction, gravity_aligned_range_data);
|
||||
if (pose_estimate_2d == nullptr) {
|
||||
LOG(WARNING) << "Scan matching failed.";
|
||||
return nullptr;
|
||||
}
|
||||
const transform::Rigid3d pose_estimate =
|
||||
transform::Embed3D(pose_estimate_2d) * gravity_alignment;
|
||||
transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
|
||||
extrapolator_->AddPose(time, pose_estimate);
|
||||
|
||||
sensor::RangeData range_data_in_local =
|
||||
TransformRangeData(gravity_aligned_range_data,
|
||||
transform::Embed3D(pose_estimate_2d.cast<float>()));
|
||||
transform::Embed3D(pose_estimate_2d->cast<float>()));
|
||||
last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
|
||||
std::unique_ptr<InsertionResult> insertion_result =
|
||||
InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
|
||||
|
|
|
@ -83,11 +83,11 @@ class LocalTrajectoryBuilder {
|
|||
const transform::Rigid3d& pose_estimate,
|
||||
const Eigen::Quaterniond& gravity_alignment);
|
||||
|
||||
// Scan matches 'gravity_aligned_range_data' and fill in the
|
||||
// 'pose_observation' with the result.
|
||||
void ScanMatch(common::Time time, const transform::Rigid2d& pose_prediction,
|
||||
const sensor::RangeData& gravity_aligned_range_data,
|
||||
transform::Rigid2d* pose_observation);
|
||||
// Scan matches 'gravity_aligned_range_data' 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);
|
||||
|
||||
// Lazily constructs a PoseExtrapolator.
|
||||
void InitializeExtrapolator(common::Time time);
|
||||
|
|
|
@ -133,6 +133,10 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
|||
options_.high_resolution_adaptive_voxel_filter_options());
|
||||
const sensor::PointCloud high_resolution_point_cloud_in_tracking =
|
||||
adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
|
||||
if (high_resolution_point_cloud_in_tracking.empty()) {
|
||||
LOG(WARNING) << "Dropped empty high resolution point cloud data.";
|
||||
return nullptr;
|
||||
}
|
||||
if (options_.use_online_correlative_scan_matching()) {
|
||||
// We take a copy since we use 'initial_ceres_pose' as an output argument.
|
||||
const transform::Rigid3d initial_pose = initial_ceres_pose;
|
||||
|
@ -149,6 +153,10 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
|||
const sensor::PointCloud low_resolution_point_cloud_in_tracking =
|
||||
low_resolution_adaptive_voxel_filter.Filter(
|
||||
filtered_range_data_in_tracking.returns);
|
||||
if (low_resolution_point_cloud_in_tracking.empty()) {
|
||||
LOG(WARNING) << "Dropped empty low resolution point cloud data.";
|
||||
return nullptr;
|
||||
}
|
||||
ceres_scan_matcher_->Match(
|
||||
matching_submap->local_pose().inverse() * pose_prediction,
|
||||
initial_ceres_pose,
|
||||
|
|
Loading…
Reference in New Issue