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

Use TrajectoryNode::Data in FastCorrelativeMatcher. (#493) 

* Use TrajectoryNode::Data in FastCorrelativeMatcher.
master
Christoph Schütte 2017-08-31 15:10:48 +02:00 committed by GitHub
parent e2c67a7bde
commit 982f2bd2e0
4 changed files with 79 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,6 +24,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/transform/transform.h"
@ -126,6 +127,7 @@ struct Candidate {
FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options)
: options_(options),
@ -133,49 +135,53 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
width_in_voxels_(hybrid_grid.grid_size()),
precomputation_grid_stack_(
common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {}
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
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& low_resolution_matcher, 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,
float* const score, transform::Rigid3d* const pose_estimate,
float* const rotational_score, float* const low_resolution_score) const {
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
const SearchParameters search_parameters{
common::RoundToInt(options_.linear_xy_search_window() / resolution_),
common::RoundToInt(options_.linear_z_search_window() / resolution_),
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);
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate,
constant_data.high_resolution_point_cloud,
constant_data.range_data.returns.Decompress(), 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& low_resolution_matcher, 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,
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;
for (const Eigen::Vector3f& point : coarse_point_cloud) {
for (const Eigen::Vector3f& point :
constant_data.high_resolution_point_cloud) {
max_point_distance = std::max(max_point_distance, point.norm());
}
const int linear_window_size =
(width_in_voxels_ + 1) / 2 +
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
const SearchParameters search_parameters{
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);
return MatchWithSearchParameters(
search_parameters, initial_pose_estimate,
constant_data.high_resolution_point_cloud,
constant_data.range_data.returns.Decompress(), min_score, score,
pose_estimate, rotational_score, low_resolution_score);
}
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
@ -269,8 +275,9 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
}
const float kSafetyMargin = 1.f - 1e-2f;
const float angular_step_size =
kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range)));
kSafetyMargin * std::acos(1.f -
common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range)));
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

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

@ -51,6 +51,7 @@ class FastCorrelativeScanMatcher {
public:
FastCorrelativeScanMatcher(
const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid,
const std::vector<mapping::TrajectoryNode>& nodes,
const proto::FastCorrelativeScanMatcherOptions& options);
~FastCorrelativeScanMatcher();
@ -65,11 +66,9 @@ class FastCorrelativeScanMatcher {
// '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& low_resolution_matcher, float* score,
transform::Rigid3d* pose_estimate, float* rotational_score,
float* low_resolution_score) const;
const mapping::TrajectoryNode::Data& constant_data,
float min_score, 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
@ -78,11 +77,9 @@ class FastCorrelativeScanMatcher {
// 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& low_resolution_matcher,
float* score, transform::Rigid3d* pose_estimate,
const mapping::TrajectoryNode::Data& constant_data,
float min_score, float* score,
transform::Rigid3d* pose_estimate,
float* rotational_score,
float* low_resolution_score) const;
@ -130,6 +127,7 @@ class FastCorrelativeScanMatcher {
const float resolution_;
const int width_in_voxels_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
const HybridGrid* const low_resolution_hybrid_grid_;
RotationalScanMatcher rotational_scan_matcher_;
};

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

@ -72,7 +72,9 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
", "
"rotational_histogram_size = 30, "
"min_rotational_score = 0.1, "
"min_low_resolution_score = 0.5, "
// Unknown space has kMinProbability = 0.1, so we need to make sure here
// to pick a larger number otherwise we always find matches.
"min_low_resolution_score = 0.15, "
"linear_xy_search_window = 0.8, "
"linear_z_search_window = 0.8, "
"angular_search_window = 0.3, "
@ -94,16 +96,28 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
const proto::FastCorrelativeScanMatcherOptions& options,
const transform::Rigid3f& pose) {
HybridGrid hybrid_grid(0.05f);
hybrid_grid_ = common::make_unique<HybridGrid>(0.05f);
range_data_inserter_.Insert(
sensor::RangeData{pose.translation(),
sensor::TransformPointCloud(point_cloud_, pose),
{}},
&hybrid_grid);
hybrid_grid.FinishUpdate();
hybrid_grid_.get());
hybrid_grid_->FinishUpdate();
return common::make_unique<FastCorrelativeScanMatcher>(
hybrid_grid, std::vector<mapping::TrajectoryNode>(), options);
*hybrid_grid_, hybrid_grid_.get(),
std::vector<mapping::TrajectoryNode>(), options);
}
mapping::TrajectoryNode::Data CreateConstantData(
const sensor::PointCloud& low_resolution_point_cloud) {
return mapping::TrajectoryNode::Data{
common::FromUniversal(0),
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}),
{},
point_cloud_,
low_resolution_point_cloud,
transform::Rigid3d::Identity()};
}
std::mt19937 prng_ = std::mt19937(42);
@ -112,11 +126,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
RangeDataInserter range_data_inserter_;
const proto::FastCorrelativeScanMatcherOptions options_;
sensor::PointCloud point_cloud_;
std::unique_ptr<HybridGrid> hybrid_grid_;
};
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) {
@ -130,20 +143,21 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
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, &low_resolution_score));
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.99f, low_resolution_score);
EXPECT_LT(0.14f, low_resolution_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
<< "\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 kFailingLowResolutionScore; },
&score, &pose_estimate, &rotational_score, &low_resolution_score));
transform::Rigid3d::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
&score, &pose_estimate, &rotational_score, &low_resolution_score))
<< low_resolution_score;
}
}
@ -158,20 +172,21 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
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, &low_resolution_score));
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.99f, low_resolution_score);
EXPECT_LT(0.14f, low_resolution_score);
EXPECT_THAT(expected_pose,
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
<< "\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 kFailingLowResolutionScore; },
&score, &pose_estimate, &rotational_score, &low_resolution_score));
Eigen::Quaterniond::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
&score, &pose_estimate, &rotational_score, &low_resolution_score))
<< low_resolution_score;
}
} // namespace

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

@ -29,7 +29,6 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/transform/transform.h"
@ -142,7 +141,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
const Submap* const submap) {
auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
submap->high_resolution_hybrid_grid(), submap_nodes,
submap->high_resolution_hybrid_grid(),
&submap->low_resolution_hybrid_grid(), submap_nodes,
options_.fast_correlative_scan_matcher_options_3d());
common::MutexLocker locker(&mutex_);
submap_scan_matchers_[submap_id] = {&submap->high_resolution_hybrid_grid(),
@ -173,8 +173,6 @@ void ConstraintBuilder::ComputeConstraint(
std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id);
const sensor::PointCloud point_cloud =
constant_data->range_data.returns.Decompress();
// The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'high_resolution_point_cloud' in scan j and
@ -184,20 +182,15 @@ void ConstraintBuilder::ComputeConstraint(
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,
&constant_data->low_resolution_point_cloud);
// 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(
initial_pose.rotation(), constant_data->high_resolution_point_cloud,
point_cloud, options_.global_localization_min_score(),
low_resolution_matcher, &score, &pose_estimate, &rotational_score,
&low_resolution_score)) {
initial_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());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
@ -208,8 +201,7 @@ void ConstraintBuilder::ComputeConstraint(
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, constant_data->high_resolution_point_cloud,
point_cloud, options_.min_score(), low_resolution_matcher, &score,
initial_pose, *constant_data, options_.min_score(), &score,
&pose_estimate, &rotational_score, &low_resolution_score)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
@ -274,7 +266,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
CHECK_EQ(submap_queued_work_items_.size(), 0);
if (when_done_ != nullptr) {
for (const std::unique_ptr<OptimizationProblem::Constraint>&
constraint : constraints_) {
constraint : constraints_) {
if (constraint != nullptr) {
result.push_back(*constraint);
}