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

Add full submap scan matching in 3D. (#145)

master
Wolfgang Hess 2016-11-28 11:09:53 +01:00 committed by GitHub
parent 88e4ea2866
commit 318607ccd1
2 changed files with 111 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -95,10 +95,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const proto::FastCorrelativeScanMatcherOptions& options)
: options_(options),
resolution_(hybrid_grid.resolution()),
linear_xy_window_size_(
common::RoundToInt(options_.linear_xy_search_window() / resolution_)),
linear_z_window_size_(
common::RoundToInt(options_.linear_z_search_window() / resolution_)),
origin_(hybrid_grid.origin()),
width_in_voxels_(hybrid_grid.grid_size()),
precomputation_grid_stack_(
common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {}
@ -109,20 +107,55 @@ 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* score, transform::Rigid3d* pose_estimate) const {
float* const score, transform::Rigid3d* const pose_estimate) 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);
}
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 {
const transform::Rigid3d initial_pose_estimate(origin_.cast<double>(),
gravity_alignment);
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point : coarse_point_cloud) {
max_point_distance = std::max(max_point_distance, point.norm());
}
const int linear_window_size =
(width_in_voxels_ + 1) / 2 + std::ceil(max_point_distance);
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);
}
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
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 {
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);
const std::vector<DiscreteScan> discrete_scans =
GenerateDiscreteScans(coarse_point_cloud, fine_point_cloud,
initial_pose_estimate.cast<float>());
const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
search_parameters, coarse_point_cloud, fine_point_cloud,
initial_pose_estimate.cast<float>());
const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans);
ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
const Candidate best_candidate =
BranchAndBound(discrete_scans, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
const Candidate best_candidate = BranchAndBound(
search_parameters, discrete_scans, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
if (best_candidate.score > min_score) {
*score = best_candidate.score;
*pose_estimate =
@ -137,6 +170,7 @@ bool FastCorrelativeScanMatcher::Match(
}
DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose) const {
std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
@ -156,7 +190,9 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
options_.branch_and_bound_depth() - full_resolution_depth;
CHECK_GE(low_resolution_depth, 0);
const Eigen::Array3i search_window_start(
-linear_xy_window_size_, -linear_xy_window_size_, -linear_z_window_size_);
-search_parameters.linear_xy_window_size,
-search_parameters.linear_xy_window_size,
-search_parameters.linear_z_window_size);
for (int i = 0; i != low_resolution_depth; ++i) {
const int reduction_exponent = i + 1;
const Eigen::Array3i low_resolution_search_window_start(
@ -178,6 +214,7 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
}
std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud,
const transform::Rigid3f& initial_pose) const {
@ -194,8 +231,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
kSafetyMargin * std::acos(1.f -
common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range)));
const int angular_window_size =
common::RoundToInt(options_.angular_search_window() / angular_step_size);
const int angular_window_size = common::RoundToInt(
search_parameters.angular_search_window / angular_step_size);
// TODO(whess): Should there be a small search window for rotations around
// x and y?
std::vector<float> angles;
@ -216,19 +253,23 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
Eigen::Translation3f(initial_pose.translation()) *
transform::AngleAxisVectorToRotationQuaternion(angle_axis) *
Eigen::Quaternionf(initial_pose.rotation()));
result.push_back(DiscretizeScan(coarse_point_cloud, pose));
result.push_back(
DiscretizeScan(search_parameters, coarse_point_cloud, pose));
}
return result;
}
std::vector<Candidate>
FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const int num_discrete_scans) const {
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
const int num_lowest_resolution_linear_xy_candidates =
(2 * linear_xy_window_size_ + linear_step_size) / linear_step_size;
(2 * search_parameters.linear_xy_window_size + linear_step_size) /
linear_step_size;
const int num_lowest_resolution_linear_z_candidates =
(2 * linear_z_window_size_ + linear_step_size) / linear_step_size;
(2 * search_parameters.linear_z_window_size + linear_step_size) /
linear_step_size;
const int num_candidates =
num_discrete_scans *
common::Power(num_lowest_resolution_linear_xy_candidates, 2) *
@ -236,11 +277,13 @@ FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates(
std::vector<Candidate> candidates;
candidates.reserve(num_candidates);
for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) {
for (int z = -linear_z_window_size_; z <= linear_z_window_size_;
z += linear_step_size) {
for (int y = -linear_xy_window_size_; y <= linear_xy_window_size_;
for (int z = -search_parameters.linear_z_window_size;
z <= search_parameters.linear_z_window_size; z += linear_step_size) {
for (int y = -search_parameters.linear_xy_window_size;
y <= search_parameters.linear_xy_window_size;
y += linear_step_size) {
for (int x = -linear_xy_window_size_; x <= linear_xy_window_size_;
for (int x = -search_parameters.linear_xy_window_size;
x <= search_parameters.linear_xy_window_size;
x += linear_step_size) {
candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z));
}
@ -277,15 +320,18 @@ void FastCorrelativeScanMatcher::ScoreCandidates(
std::vector<Candidate>
FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans) const {
std::vector<Candidate> lowest_resolution_candidates =
GenerateLowestResolutionCandidates(discrete_scans.size());
GenerateLowestResolutionCandidates(search_parameters,
discrete_scans.size());
ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans,
&lowest_resolution_candidates);
return lowest_resolution_candidates;
}
Candidate FastCorrelativeScanMatcher::BranchAndBound(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<Candidate>& candidates, const int candidate_depth,
float min_score) const {
@ -303,15 +349,17 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
std::vector<Candidate> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
for (int z : {0, half_width}) {
if (candidate.offset.z() + z > linear_z_window_size_) {
if (candidate.offset.z() + z > search_parameters.linear_z_window_size) {
break;
}
for (int y : {0, half_width}) {
if (candidate.offset.y() + y > linear_xy_window_size_) {
if (candidate.offset.y() + y >
search_parameters.linear_xy_window_size) {
break;
}
for (int x : {0, half_width}) {
if (candidate.offset.x() + x > linear_xy_window_size_) {
if (candidate.offset.x() + x >
search_parameters.linear_xy_window_size) {
break;
}
higher_resolution_candidates.emplace_back(
@ -321,11 +369,11 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
}
ScoreCandidates(candidate_depth - 1, discrete_scans,
&higher_resolution_candidates);
best_high_resolution_candidate =
std::max(best_high_resolution_candidate,
BranchAndBound(discrete_scans, higher_resolution_candidates,
candidate_depth - 1,
best_high_resolution_candidate.score));
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(search_parameters, discrete_scans,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}

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

@ -89,28 +89,55 @@ class FastCorrelativeScanMatcher {
const sensor::PointCloud& fine_point_cloud, float min_score,
float* score, transform::Rigid3d* pose_estimate) 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.
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;
private:
DiscreteScan DiscretizeScan(const sensor::PointCloud& point_cloud,
struct SearchParameters {
const int linear_xy_window_size; // voxels
const int linear_z_window_size; // voxels
const double angular_search_window; // radians
};
bool MatchWithSearchParameters(
const SearchParameters& search_parameters,
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;
DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose) const;
std::vector<DiscreteScan> GenerateDiscreteScans(
const SearchParameters& search_parameters,
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud,
const transform::Rigid3f& initial_pose) const;
std::vector<Candidate> GenerateLowestResolutionCandidates(
int num_discrete_scans) const;
const SearchParameters& search_parameters, int num_discrete_scans) const;
void ScoreCandidates(int depth,
const std::vector<DiscreteScan>& discrete_scans,
std::vector<Candidate>* const candidates) const;
std::vector<Candidate> ComputeLowestResolutionCandidates(
const SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans) const;
Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
Candidate BranchAndBound(const SearchParameters& search_parameters,
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<Candidate>& candidates,
int candidate_depth, float min_score) const;
const proto::FastCorrelativeScanMatcherOptions options_;
const float resolution_;
const int linear_xy_window_size_;
const int linear_z_window_size_;
const Eigen::Vector3f origin_;
const int width_in_voxels_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
RotationalScanMatcher rotational_scan_matcher_;
};