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

FastCorrelativeScanMatcher outputs unique_ptr. (#643)

master
gaschler 2017-11-09 16:08:16 +01:00 committed by Wolfgang Hess
parent e21fc9f253
commit 6eaf0f344d
4 changed files with 95 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -156,12 +156,12 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
bool FastCorrelativeScanMatcher::Match(
std::unique_ptr<FastCorrelativeScanMatcher::Result>
FastCorrelativeScanMatcher::Match(
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const mapping::TrajectoryNode::Data& constant_data, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score, float* const low_resolution_score) const {
const mapping::TrajectoryNode::Data& constant_data,
const float min_score) const {
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
const SearchParameters search_parameters{
@ -173,16 +173,15 @@ bool FastCorrelativeScanMatcher::Match(
global_submap_pose.cast<float>(),
constant_data.high_resolution_point_cloud,
constant_data.rotational_scan_matcher_histogram,
constant_data.gravity_alignment, min_score, score, pose_estimate,
rotational_score, low_resolution_score);
constant_data.gravity_alignment, min_score);
}
bool FastCorrelativeScanMatcher::MatchFullSubmap(
std::unique_ptr<FastCorrelativeScanMatcher::Result>
FastCorrelativeScanMatcher::MatchFullSubmap(
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const mapping::TrajectoryNode::Data& constant_data, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score, float* const low_resolution_score) const {
const mapping::TrajectoryNode::Data& constant_data,
const float min_score) const {
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point :
constant_data.high_resolution_point_cloud) {
@ -201,22 +200,17 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
transform::Rigid3f::Rotation(global_submap_rotation.cast<float>()),
constant_data.high_resolution_point_cloud,
constant_data.rotational_scan_matcher_histogram,
constant_data.gravity_alignment, min_score, score, pose_estimate,
rotational_score, low_resolution_score);
constant_data.gravity_alignment, min_score);
}
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
std::unique_ptr<FastCorrelativeScanMatcher::Result>
FastCorrelativeScanMatcher::MatchWithSearchParameters(
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score, float* const low_resolution_score) const {
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);
const Eigen::Quaterniond& gravity_alignment, const float min_score) const {
const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
search_parameters, point_cloud, rotational_scan_matcher_histogram,
gravity_alignment, global_node_pose, global_submap_pose);
@ -228,15 +222,13 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
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 =
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 common::make_unique<Result>(Result{
best_candidate.score,
GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>(),
discrete_scans[best_candidate.scan_index].rotational_score,
best_candidate.low_resolution_score});
}
return false;
return nullptr;
}
DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(

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

@ -49,6 +49,13 @@ using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
class FastCorrelativeScanMatcher {
public:
struct Result {
float score;
transform::Rigid3d pose_estimate;
float rotational_score;
float low_resolution_score;
};
FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid,
@ -61,28 +68,23 @@ class FastCorrelativeScanMatcher {
delete;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given 'global_node_pose' and 'global_submap_pose'. If a score above
// 'min_score' (excluding equality) is possible, true is returned, and
// 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score'
// are updated with the result.
bool Match(const transform::Rigid3d& global_node_pose,
// given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
// returned if a score above 'min_score' (excluding equality) is possible.
std::unique_ptr<Result> Match(
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose,
const mapping::TrajectoryNode::Data& constant_data,
float min_score, float* score, transform::Rigid3d* pose_estimate,
float* rotational_score, float* low_resolution_score) const;
float min_score) const;
// Aligns the node with the given 'constant_data' within the 'hybrid_grid'
// given rotations which are expected to be approximately gravity aligned.
// If a score above 'min_score' (excluding equality) is possible, true is
// returned, and 'score', 'pose_estimate', 'rotational_score', and
// 'low_resolution_score' are updated with the result.
bool MatchFullSubmap(const Eigen::Quaterniond& global_node_rotation,
// 'Result' is only returned if a score above 'min_score' (excluding equality)
// is possible.
std::unique_ptr<Result> MatchFullSubmap(
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation,
const mapping::TrajectoryNode::Data& constant_data,
float min_score, float* score,
transform::Rigid3d* pose_estimate,
float* rotational_score,
float* low_resolution_score) const;
float min_score) const;
private:
struct SearchParameters {
@ -92,15 +94,13 @@ class FastCorrelativeScanMatcher {
const MatchingFunction* const low_resolution_matcher;
};
bool MatchWithSearchParameters(
std::unique_ptr<Result> MatchWithSearchParameters(
const SearchParameters& search_parameters,
const transform::Rigid3f& global_node_pose,
const transform::Rigid3f& global_submap_pose,
const sensor::PointCloud& point_cloud,
const Eigen::VectorXf& rotational_scan_matcher_histogram,
const Eigen::Quaterniond& gravity_alignment, float min_score,
float* score, transform::Rigid3d* pose_estimate, float* rotational_score,
float* low_resolution_score) const;
const Eigen::Quaterniond& gravity_alignment, float min_score) const;
DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
const sensor::PointCloud& point_cloud,
const transform::Rigid3f& pose,

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

@ -140,26 +140,25 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
GetFastCorrelativeScanMatcher(options_, expected_pose));
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(
const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
&rotational_score, &low_resolution_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_LT(0.14f, low_resolution_score);
CreateConstantData(point_cloud_), kMinScore);
EXPECT_THAT(result, testing::NotNull());
EXPECT_LT(kMinScore, result->score);
EXPECT_LT(0.09f, result->rotational_score);
EXPECT_LT(0.14f, result->low_resolution_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
transform::IsNearly(result->pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
EXPECT_FALSE(fast_correlative_scan_matcher->Match(
const std::unique_ptr<FastCorrelativeScanMatcher::Result>
low_resolution_result = fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
&score, &pose_estimate, &rotational_score, &low_resolution_score))
<< low_resolution_score;
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
EXPECT_THAT(low_resolution_result, testing::IsNull())
<< low_resolution_result->low_resolution_score;
}
}
@ -169,26 +168,25 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
GetFastCorrelativeScanMatcher(options_, expected_pose));
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(
const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
&rotational_score, &low_resolution_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_LT(0.14f, low_resolution_score);
CreateConstantData(point_cloud_), kMinScore);
EXPECT_THAT(result, testing::NotNull());
EXPECT_LT(kMinScore, result->score);
EXPECT_LT(0.09f, result->rotational_score);
EXPECT_LT(0.14f, result->low_resolution_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
transform::IsNearly(result->pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
const std::unique_ptr<FastCorrelativeScanMatcher::Result>
low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
&score, &pose_estimate, &rotational_score, &low_resolution_score))
<< low_resolution_score;
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
EXPECT_THAT(low_resolution_result, testing::IsNull())
<< low_resolution_result->low_resolution_score;
}
} // namespace

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

@ -179,43 +179,41 @@ void ConstraintBuilder::ComputeConstraint(
// The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'high_resolution_point_cloud' in scan j and
// - the initial guess 'initial_pose' (submap i <- scan j).
float score = 0.f;
transform::Rigid3d pose_estimate;
float rotational_score = 0.f;
float low_resolution_score = 0.f;
// TODO(gaschler): Match methods should return unique_ptr<struct>.
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher::Result>
match_result;
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
match_result =
submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
global_node_pose.rotation(), global_submap_pose.rotation(),
*constant_data, options_.global_localization_min_score(), &score,
&pose_estimate, &rotational_score, &low_resolution_score)) {
CHECK_GT(score, options_.global_localization_min_score());
*constant_data, options_.global_localization_min_score());
if (match_result != nullptr) {
CHECK_GT(match_result->score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
} else {
return;
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
match_result = submap_scan_matcher->fast_correlative_scan_matcher->Match(
global_node_pose, global_submap_pose, *constant_data,
options_.min_score(), &score, &pose_estimate, &rotational_score,
&low_resolution_score)) {
options_.min_score());
if (match_result != nullptr) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
CHECK_GT(match_result->score, options_.min_score());
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
rotational_score_histogram_.Add(rotational_score);
low_resolution_score_histogram_.Add(low_resolution_score);
score_histogram_.Add(match_result->score);
rotational_score_histogram_.Add(match_result->rotational_score);
low_resolution_score_histogram_.Add(match_result->low_resolution_score);
}
// Use the CSM estimate as both the initial and previous pose. This has the
@ -223,7 +221,8 @@ void ConstraintBuilder::ComputeConstraint(
// CSM estimate.
ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform;
ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
ceres_scan_matcher_.Match(match_result->pose_estimate,
match_result->pose_estimate,
{{&constant_data->high_resolution_point_cloud,
submap_scan_matcher->high_resolution_hybrid_grid},
{&constant_data->low_resolution_point_cloud,
@ -252,7 +251,8 @@ void ConstraintBuilder::ComputeConstraint(
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference);
}
info << " with score " << std::setprecision(1) << 100. * score << "%.";
info << " with score " << std::setprecision(1) << 100. * match_result->score
<< "%.";
LOG(INFO) << info.str();
}
}