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

Add a histogram of low resolution scores for 3D. (#473)

master
Wolfgang Hess 2017-08-24 09:28:12 +02:00 committed by Damon Kohler
parent 4d11a226ff
commit f0e1dab031
5 changed files with 96 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -91,6 +91,39 @@ class PrecomputationGridStack {
std::vector<PrecomputationGrid> precomputation_grids_;
};
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 {
Candidate(const int scan_index, const Eigen::Array3i& offset)
: scan_index(scan_index), offset(offset) {}
static Candidate Unsuccessful() {
return Candidate(0, Eigen::Array3i::Zero());
}
// Index into the discrete scans vectors.
int scan_index;
// Linear offset from the initial pose in cell indices. For lower resolution
// candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth
// block of possibilities.
Eigen::Array3i offset;
// Score, higher is better.
float score = -std::numeric_limits<float>::infinity();
// Score of the low resolution matcher.
float low_resolution_score = 0.f;
bool operator<(const Candidate& other) const { return score < other.score; }
bool operator>(const Candidate& other) const { return score > other.score; }
};
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes,
@ -108,25 +141,26 @@ 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,
const MatchingFunction& matching_function, float* const score,
transform::Rigid3d* const pose_estimate,
float* const rotational_score) const {
const MatchingFunction& low_resolution_matcher, float* const score,
transform::Rigid3d* const pose_estimate, float* const rotational_score,
float* const low_resolution_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(), &matching_function};
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate, coarse_point_cloud,
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
options_.angular_search_window(), &low_resolution_matcher};
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
coarse_point_cloud, fine_point_cloud,
min_score, score, pose_estimate,
rotational_score, low_resolution_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,
const MatchingFunction& matching_function, float* const score,
transform::Rigid3d* const pose_estimate,
float* const rotational_score) const {
const MatchingFunction& low_resolution_matcher, float* const score,
transform::Rigid3d* const pose_estimate, float* const rotational_score,
float* const low_resolution_score) const {
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
gravity_alignment);
float max_point_distance = 0.f;
@ -137,10 +171,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
(width_in_voxels_ + 1) / 2 +
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
const SearchParameters search_parameters{
linear_window_size, linear_window_size, M_PI, &matching_function};
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate, coarse_point_cloud,
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
coarse_point_cloud, fine_point_cloud,
min_score, score, pose_estimate,
rotational_score, low_resolution_score);
}
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
@ -149,7 +184,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score) const {
float* const rotational_score, float* const low_resolution_score) const {
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);
@ -169,6 +204,7 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>();
*rotational_score =
discrete_scans[best_candidate.scan_index].rotational_score;
*low_resolution_score = best_candidate.low_resolution_score;
return true;
}
return false;
@ -347,26 +383,30 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
const std::vector<DiscreteScan>& discrete_scans,
const std::vector<Candidate>& candidates, const int candidate_depth,
float min_score) const {
Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero());
best_high_resolution_candidate.score = min_score;
if (candidate_depth == 0) {
for (const Candidate& candidate : candidates) {
if (candidate.score <= min_score) {
// Return if the candidate is bad because the following candidate will
// not have better score.
return best_high_resolution_candidate;
} else if ((*search_parameters.matching_function)(
GetPoseFromCandidate(discrete_scans, candidate)) >=
options_.min_low_resolution_score()) {
return Candidate::Unsuccessful();
}
const float low_resolution_score =
(*search_parameters.low_resolution_matcher)(
GetPoseFromCandidate(discrete_scans, candidate));
if (low_resolution_score >= options_.min_low_resolution_score()) {
// We found the best candidate that passes the matching function.
return candidate;
Candidate best_candidate = candidate;
best_candidate.low_resolution_score = low_resolution_score;
return best_candidate;
}
}
// All candidates have good scores but none passes the matching function.
return best_high_resolution_candidate;
return Candidate::Unsuccessful();
}
Candidate best_high_resolution_candidate = Candidate::Unsuccessful();
best_high_resolution_candidate.score = min_score;
for (const Candidate& candidate : candidates) {
if (candidate.score <= min_score) {
break;

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

@ -41,32 +41,8 @@ CreateFastCorrelativeScanMatcherOptions(
common::LuaParameterDictionary* parameter_dictionary);
class PrecomputationGridStack;
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 {
Candidate(const int scan_index, const Eigen::Array3i& offset)
: scan_index(scan_index), offset(offset) {}
// Index into the discrete scans vectors.
int scan_index;
// Linear offset from the initial pose in cell indices. For lower resolution
// candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth
// block of possibilities.
Eigen::Array3i offset;
// Score, higher is better.
float score = 0.f;
bool operator<(const Candidate& other) const { return score < other.score; }
bool operator>(const Candidate& other) const { return score > other.score; }
};
struct DiscreteScan;
struct Candidate;
// Used to compute scores between 0 and 1 how well the given pose matches.
using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
@ -85,35 +61,37 @@ 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', 'pose_estimate', and
// 'rotational_score' 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',
// 'rotational_score', and 'low_resolution_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,
const MatchingFunction& matching_function, float* score,
transform::Rigid3d* pose_estimate, float* rotational_score) const;
const MatchingFunction& low_resolution_matcher, float* score,
transform::Rigid3d* pose_estimate, float* rotational_score,
float* low_resolution_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', 'pose_estimate', and 'rotational_score' are updated with the
// result. 'fine_point_cloud' is used to compute the rotational scan matcher
// score.
// 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_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,
const MatchingFunction& matching_function, float* score,
transform::Rigid3d* pose_estimate,
float* rotational_score) const;
const MatchingFunction& low_resolution_matcher,
float* score, transform::Rigid3d* pose_estimate,
float* rotational_score,
float* low_resolution_score) const;
private:
struct SearchParameters {
const int linear_xy_window_size; // voxels
const int linear_z_window_size; // voxels
const double angular_search_window; // radians
const MatchingFunction* const matching_function;
const MatchingFunction* const low_resolution_matcher;
};
bool MatchWithSearchParameters(
@ -121,7 +99,8 @@ 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, float* rotational_score) const;
transform::Rigid3d* pose_estimate, float* rotational_score,
float* low_resolution_score) const;
DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose,

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

@ -128,12 +128,14 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
float score = 0.f;
transform::Rigid3d pose_estimate;
float rotational_score = 0.f;
float low_resolution_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
&score, &pose_estimate, &rotational_score, &low_resolution_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_LT(0.99f, low_resolution_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
@ -141,7 +143,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
EXPECT_FALSE(fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
&score, &pose_estimate, &rotational_score, &low_resolution_score));
}
}
@ -154,12 +156,14 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
float score = 0.f;
transform::Rigid3d pose_estimate;
float rotational_score = 0.f;
float low_resolution_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
&score, &pose_estimate, &rotational_score, &low_resolution_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_LT(0.99f, low_resolution_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
@ -167,7 +171,7 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
&score, &pose_estimate, &rotational_score, &low_resolution_score));
}
} // namespace

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

@ -189,6 +189,7 @@ void ConstraintBuilder::ComputeConstraint(
float score = 0.f;
transform::Rigid3d pose_estimate;
float rotational_score = 0.f;
float low_resolution_score = 0.f;
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
submap_scan_matcher->low_resolution_hybrid_grid,
@ -202,7 +203,7 @@ void ConstraintBuilder::ComputeConstraint(
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
options_.global_localization_min_score(), low_resolution_matcher,
&score, &pose_estimate, &rotational_score)) {
&score, &pose_estimate, &rotational_score, &low_resolution_score)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
@ -215,7 +216,7 @@ void ConstraintBuilder::ComputeConstraint(
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, high_resolution_point_cloud, point_cloud,
options_.min_score(), low_resolution_matcher, &score,
&pose_estimate, &rotational_score)) {
&pose_estimate, &rotational_score, &low_resolution_score)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
@ -226,6 +227,7 @@ void ConstraintBuilder::ComputeConstraint(
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
rotational_score_histogram_.Add(rotational_score);
low_resolution_score_histogram_.Add(low_resolution_score);
}
// Use the CSM estimate as both the initial and previous pose. This has the
@ -288,6 +290,8 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
LOG(INFO) << "Rotational score histogram:\n"
<< rotational_score_histogram_.ToString(10);
LOG(INFO) << "Low resolution score histogram:\n"
<< low_resolution_score_histogram_.ToString(10);
}
constraints_.clear();
callback = std::move(when_done_);

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

@ -190,6 +190,7 @@ class ConstraintBuilder {
// Histograms of scan matcher scores.
common::Histogram score_histogram_ GUARDED_BY(mutex_);
common::Histogram rotational_score_histogram_ GUARDED_BY(mutex_);
common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_);
};
} // namespace sparse_pose_graph