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

Expose low resolution matching scores. (#470) 

This is needed in preparation of adding a low resolution score histogram.
master
Wolfgang Hess 2017-08-23 17:48:49 +02:00 committed by GitHub
parent 2dd2d6f448
commit 4d11a226ff
14 changed files with 55 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/sparse_pose_graph/constraint_builder.cc
View File

@ -36,8 +36,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
sensor::CreateAdaptiveVoxelFilterOptions(
parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
options.set_min_score(parameter_dictionary->GetDouble("min_score"));
options.set_min_low_resolution_score(
parameter_dictionary->GetDouble("min_low_resolution_score"));
options.set_global_localization_min_score(
parameter_dictionary->GetDouble("global_localization_min_score"));
options.set_loop_closure_translation_weight(

5
cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto
View File

@ -23,7 +23,6 @@ import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.p
import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
message ConstraintBuilderOptions {
// Next ID: 18
// A constraint will be added if the proportion of added constraints to
// potential constraints drops below this number.
optional double sampling_ratio = 1;
@ -40,10 +39,6 @@ message ConstraintBuilderOptions {
// Low scores indicate that the scan and map do not look similar.
optional double min_score = 4;
// Threshold for the score of the low resolution grid below which a match is
// not considered. Only used for 3D.
optional double min_low_resolution_score = 17;
// Threshold below which global localizations are not trusted.
optional double global_localization_min_score = 5;

26
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -71,8 +71,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
}
CHECK_EQ(2, insertion_submaps.size());
const mapping::SubmapId last_submap_id{
trajectory_id,
submap_data.at(trajectory_id).rbegin()->first};
trajectory_id, submap_data.at(trajectory_id).rbegin()->first};
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
// and 'insertions_submaps.back()' is new.
@ -193,9 +192,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index -
optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id))
.at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id))
.point_cloud_pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -348,19 +346,21 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(num_trajectory_nodes_ - num_finished_scans_at_start)
<< "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone([this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
locker.Await([&notification]() { return notification; });
}

4
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -154,8 +154,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
const int submap_index = index_submap_data.first;
const SubmapData& submap_data = index_submap_data.second;
C_submaps[trajectory_id].emplace(
submap_index, FromPose(submap_data.pose));
C_submaps[trajectory_id].emplace(submap_index,
FromPose(submap_data.pose));
problem.AddParameterBlock(
C_submaps[trajectory_id].at(submap_index).data(), 3);
if (first_submap || frozen) {

2
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -75,7 +75,6 @@ class SparsePoseGraphTest : public ::testing::Test {
max_range = 50.,
},
min_score = 0.5,
min_low_resolution_score = 0.5,
global_localization_min_score = 0.6,
loop_closure_translation_weight = 1.,
loop_closure_rotation_weight = 1.,
@ -100,6 +99,7 @@ class SparsePoseGraphTest : public ::testing::Test {
full_resolution_depth = 3,
rotational_histogram_size = 30,
min_rotational_score = 0.1,
min_low_resolution_score = 0.5,
linear_xy_search_window = 4.,
linear_z_search_window = 4.,
angular_search_window = 0.1,

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

@ -45,6 +45,8 @@ CreateFastCorrelativeScanMatcherOptions(
parameter_dictionary->GetInt("rotational_histogram_size"));
options.set_min_rotational_score(
parameter_dictionary->GetDouble("min_rotational_score"));
options.set_min_low_resolution_score(
parameter_dictionary->GetDouble("min_low_resolution_score"));
options.set_linear_xy_search_window(
parameter_dictionary->GetDouble("linear_xy_search_window"));
options.set_linear_z_search_window(
@ -354,7 +356,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
// not have better score.
return best_high_resolution_candidate;
} else if ((*search_parameters.matching_function)(
GetPoseFromCandidate(discrete_scans, candidate))) {
GetPoseFromCandidate(discrete_scans, candidate)) >=
options_.min_low_resolution_score()) {
// We found the best candidate that passes the matching function.
return candidate;
}

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

@ -68,7 +68,8 @@ struct Candidate {
bool operator>(const Candidate& other) const { return score > other.score; }
};
using MatchingFunction = std::function<bool(const transform::Rigid3f&)>;
// Used to compute scores between 0 and 1 how well the given pose matches.
using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
class FastCorrelativeScanMatcher {
public:

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

@ -72,6 +72,7 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
", "
"rotational_histogram_size = 30, "
"min_rotational_score = 0.1, "
"min_low_resolution_score = 0.5, "
"linear_xy_search_window = 0.8, "
"linear_z_search_window = 0.8, "
"angular_search_window = 0.3, "
@ -109,12 +110,13 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
std::uniform_real_distribution<float> distribution_ =
std::uniform_real_distribution<float>(-1.f, 1.f);
RangeDataInserter range_data_inserter_;
static constexpr float kMinScore = 0.1f;
const proto::FastCorrelativeScanMatcherOptions options_;
sensor::PointCloud point_cloud_;
};
constexpr float FastCorrelativeScanMatcherTest::kMinScore;
constexpr float kMinScore = 0.1f;
constexpr float kPassingLowResolutionScore = 1.f;
constexpr float kFailingLowResolutionScore = 0.f;
TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
for (int i = 0; i != 20; ++i) {
@ -128,8 +130,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
float rotational_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return true; }, &score, &pose_estimate,
&rotational_score));
[](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_THAT(expected_pose,
@ -138,8 +140,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
EXPECT_FALSE(fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return false; }, &score, &pose_estimate,
&rotational_score));
[](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
}
}
@ -154,8 +156,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
float rotational_score = 0.f;
EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return true; }, &score, &pose_estimate,
&rotational_score));
[](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
EXPECT_LT(kMinScore, score);
EXPECT_LT(0.09f, rotational_score);
EXPECT_THAT(expected_pose,
@ -164,8 +166,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
[](const transform::Rigid3f&) { return false; }, &score, &pose_estimate,
&rotational_score));
[](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
&score, &pose_estimate, &rotational_score));
}
} // namespace

31
cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc
View File

@ -20,29 +20,16 @@ namespace cartographer {
namespace mapping_3d {
namespace scan_matching {
namespace {
// TODO(zhengj, whess): Interpolate the Grid to get better score.
float EvaluateLowResolutionScore(const HybridGrid& low_resolution_grid,
const sensor::PointCloud& points) {
float score = 0.f;
for (const auto& point : points) {
score += low_resolution_grid.GetProbability(
low_resolution_grid.GetCellIndex(point));
}
return score / points.size();
}
} // namespace
std::function<bool(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points,
const float min_low_resolution_score) {
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
return [=](const transform::Rigid3f& pose) {
return EvaluateLowResolutionScore(
*low_resolution_grid,
sensor::TransformPointCloud(*points, pose)) >=
min_low_resolution_score;
float score = 0.f;
for (const auto& point : sensor::TransformPointCloud(*points, pose)) {
// TODO(zhengj, whess): Interpolate the Grid to get better score.
score += low_resolution_grid->GetProbability(
low_resolution_grid->GetCellIndex(point));
}
return score / points->size();
};
}

5
cartographer/mapping_3d/scan_matching/low_resolution_matcher.h
View File

@ -27,9 +27,8 @@ namespace cartographer {
namespace mapping_3d {
namespace scan_matching {
std::function<bool(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points,
float min_low_resolution_score);
std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points);
} // namespace scan_matching
} // namespace mapping_3d

4
cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto
View File

@ -30,6 +30,10 @@ message FastCorrelativeScanMatcherOptions {
// Minimum score for the rotational scan matcher.
optional double min_rotational_score = 4;
// Threshold for the score of the low resolution grid below which a match is
// not considered. Only used for 3D.
optional double min_low_resolution_score = 9;
// Linear search window in the plane orthogonal to gravity in which the best
// possible scan alignment will be found.
optional double linear_xy_search_window = 5;

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

@ -192,7 +192,7 @@ void ConstraintBuilder::ComputeConstraint(
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
submap_scan_matcher->low_resolution_hybrid_grid,
&low_resolution_point_cloud, options_.min_low_resolution_score());
&low_resolution_point_cloud);
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.

2
configuration_files/sparse_pose_graph.lua
View File

@ -23,7 +23,6 @@ SPARSE_POSE_GRAPH = {
max_range = 50.,
},
min_score = 0.55,
min_low_resolution_score = 0.55,
global_localization_min_score = 0.6,
loop_closure_translation_weight = 1.1e4,
loop_closure_rotation_weight = 1e5,
@ -48,6 +47,7 @@ SPARSE_POSE_GRAPH = {
full_resolution_depth = 3,
rotational_histogram_size = 120,
min_rotational_score = 0.77,
min_low_resolution_score = 0.55,
linear_xy_search_window = 5.,
linear_z_search_window = 1.,
angular_search_window = math.rad(15.),

9
docs/source/configuration.rst
View File

@ -96,7 +96,6 @@ cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
=====================================================================
double sampling_ratio
Next ID: 18
A constraint will be added if the proportion of added constraints to
potential constraints drops below this number.
@ -111,10 +110,6 @@ double min_score
Threshold for the scan match score below which a match is not considered.
Low scores indicate that the scan and map do not look similar.
double min_low_resolution_score
Threshold for the score of the low resolution grid below which a match is
not considered. Only used for 3D.
double global_localization_min_score
Threshold below which global localizations are not trusted.
@ -445,6 +440,10 @@ int32 rotational_histogram_size
double min_rotational_score
Minimum score for the rotational scan matcher.
double min_low_resolution_score
Threshold for the score of the low resolution grid below which a match is
not considered. Only used for 3D.
double linear_xy_search_window
Linear search window in the plane orthogonal to gravity in which the best
possible scan alignment will be found.