Expose low resolution matching scores. (#470)
This is needed in preparation of adding a low resolution score histogram.master
Wolfgang Hess
GitHub
parent
2dd2d6f448
commit
4d11a226ff
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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,8 +192,7 @@ 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(
|
||||
.at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
|
||||
node_id.trajectory_id))
|
||||
.point_cloud_pose;
|
||||
constraint_builder_.MaybeAddConstraint(
|
||||
|
|
@ -348,14 +346,16 @@ void SparsePoseGraph::WaitForAllComputations() {
|
|||
common::FromSeconds(1.))) {
|
||||
std::ostringstream progress_info;
|
||||
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
|
||||
<< 100. * (constraint_builder_.GetNumFinishedScans() -
|
||||
<< 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, ¬ification](
|
||||
constraint_builder_.WhenDone(
|
||||
[this, ¬ification](
|
||||
const sparse_pose_graph::ConstraintBuilder::Result& result) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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.),
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
Loading…
Reference in New Issue