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

Add rotational score histograms in 3D. (#403)

master
Wolfgang Hess 2017-07-11 11:59:42 +02:00 committed by GitHub
parent 167047173c
commit 10a19d0a3f
5 changed files with 48 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -106,21 +106,23 @@ bool FastCorrelativeScanMatcher::Match(
const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate) const {
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score) const {
const SearchParameters search_parameters{
common::RoundToInt(options_.linear_xy_search_window() / resolution_),
common::RoundToInt(options_.linear_z_search_window() / resolution_),
options_.angular_search_window()};
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
coarse_point_cloud, fine_point_cloud,
min_score, score, pose_estimate);
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate, coarse_point_cloud,
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
}
bool FastCorrelativeScanMatcher::MatchFullSubmap(
const Eigen::Quaterniond& gravity_alignment,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate) const {
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score) const {
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
gravity_alignment);
float max_point_distance = 0.f;
@ -132,9 +134,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
const SearchParameters search_parameters{linear_window_size,
linear_window_size, M_PI};
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
coarse_point_cloud, fine_point_cloud,
min_score, score, pose_estimate);
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate, coarse_point_cloud,
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
}
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
@ -142,7 +144,8 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate) const {
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score) const {
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);
@ -158,12 +161,14 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
*score = best_candidate.score;
const auto& discrete_scan = discrete_scans[best_candidate.scan_index];
*pose_estimate =
(transform::Rigid3f(
resolution_ * best_candidate.offset.matrix().cast<float>(),
Eigen::Quaternionf::Identity()) *
discrete_scans[best_candidate.scan_index].pose)
discrete_scan.pose)
.cast<double>();
*rotational_score = discrete_scan.rotational_score;
return true;
}
return false;
@ -171,8 +176,8 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose) const {
const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose,
const float rotational_score) const {
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0);
std::vector<Eigen::Array3i> full_resolution_cell_indices;
@ -210,7 +215,7 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
low_resolution_cell_at_start - low_resolution_search_window_start);
}
}
return DiscreteScan{pose, cell_indices_per_depth};
return DiscreteScan{pose, cell_indices_per_depth, rotational_score};
}
std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
@ -253,7 +258,7 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
initial_pose.rotation());
result.push_back(
DiscretizeScan(search_parameters, coarse_point_cloud, pose));
DiscretizeScan(search_parameters, coarse_point_cloud, pose, scores[i]));
}
return result;
}

25
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -46,6 +46,7 @@ struct DiscreteScan {
transform::Rigid3f pose;
// Contains a vector of discretized scans for each 'depth'.
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
float rotational_score;
};
struct Candidate {
@ -81,24 +82,27 @@ class FastCorrelativeScanMatcher {
// Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an
// 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
// is possible, true is returned, and 'score' and 'pose_estimate' are updated
// with the result. 'fine_point_cloud' is used to compute the rotational scan
// matcher score.
// is possible, true is returned, and 'score', 'pose_estimate', and
// 'rotational_score' are updated with the result. 'fine_point_cloud' is used
// to compute the rotational scan matcher score.
bool Match(const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, float min_score,
float* score, transform::Rigid3d* pose_estimate) const;
float* score, transform::Rigid3d* pose_estimate,
float* rotational_score) const;
// Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which
// is expected to be approximately gravity aligned. If a score above
// 'min_score' (excluding equality) is possible, true is returned, and 'score'
// and 'pose_estimate' are updated with the result. 'fine_point_cloud' is used
// to compute the rotational scan matcher score.
// 'min_score' (excluding equality) is possible, true is returned, and
// 'score', 'pose_estimate', and 'rotational_score' are updated with the
// result. 'fine_point_cloud' is used to compute the rotational scan matcher
// score.
bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud,
float min_score, float* score,
transform::Rigid3d* pose_estimate) const;
transform::Rigid3d* pose_estimate,
float* rotational_score) const;
private:
struct SearchParameters {
@ -112,10 +116,11 @@ class FastCorrelativeScanMatcher {
const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, float min_score, float* score,
transform::Rigid3d* pose_estimate) const;
transform::Rigid3d* pose_estimate, float* rotational_score) const;
DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose) const;
const transform::Rigid3f& pose,
float rotational_score) const;
std::vector<DiscreteScan> GenerateDiscreteScans(
const SearchParameters& search_parameters,
const sensor::PointCloud& coarse_point_cloud,

6
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -98,12 +98,14 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
options);
float score = 0.f;
transform::Rigid3d pose_estimate;
float score;
float rotational_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher.Match(
transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore,
&score, &pose_estimate));
&score, &pose_estimate, &rotational_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()

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

@ -181,8 +181,9 @@ void ConstraintBuilder::ComputeConstraint(
// The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'filtered_point_cloud' in scan j and
// - the initial guess 'initial_pose' (submap i <- scan j).
float score = 0.;
float score = 0.f;
transform::Rigid3d pose_estimate;
float rotational_score = 0.f;
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
@ -191,7 +192,8 @@ void ConstraintBuilder::ComputeConstraint(
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
initial_pose.rotation(), filtered_point_cloud, point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
options_.global_localization_min_score(), &score, &pose_estimate,
&rotational_score)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
@ -203,7 +205,7 @@ void ConstraintBuilder::ComputeConstraint(
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, point_cloud,
options_.min_score(), &score, &pose_estimate)) {
options_.min_score(), &score, &pose_estimate, &rotational_score)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
@ -213,6 +215,7 @@ void ConstraintBuilder::ComputeConstraint(
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
rotational_score_histogram_.Add(rotational_score);
}
// Use the CSM estimate as both the initial and previous pose. This has the
@ -282,6 +285,8 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
LOG(INFO) << constraints_.size() << " computations resulted in "
<< result.size() << " additional constraints.";
LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
LOG(INFO) << "Rotational score histogram:\n"
<< rotational_score_histogram_.ToString(10);
}
constraints_.clear();
callback = std::move(when_done_);

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

@ -185,8 +185,9 @@ class ConstraintBuilder {
const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
scan_matching::CeresScanMatcher ceres_scan_matcher_;
// Histogram of scan matcher scores.
// Histograms of scan matcher scores.
common::Histogram score_histogram_ GUARDED_BY(mutex_);
common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_);
};
} // namespace sparse_pose_graph