parent
297e9cc02d
commit
2e53586818
|
@ -36,6 +36,8 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
|
||||||
sensor::CreateAdaptiveVoxelFilterOptions(
|
sensor::CreateAdaptiveVoxelFilterOptions(
|
||||||
parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
|
parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
|
||||||
options.set_min_score(parameter_dictionary->GetDouble("min_score"));
|
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(
|
options.set_global_localization_min_score(
|
||||||
parameter_dictionary->GetDouble("global_localization_min_score"));
|
parameter_dictionary->GetDouble("global_localization_min_score"));
|
||||||
options.set_loop_closure_translation_weight(
|
options.set_loop_closure_translation_weight(
|
||||||
|
|
|
@ -23,6 +23,7 @@ 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";
|
import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
|
||||||
|
|
||||||
message ConstraintBuilderOptions {
|
message ConstraintBuilderOptions {
|
||||||
|
// Next ID: 18
|
||||||
// A constraint will be added if the proportion of added constraints to
|
// A constraint will be added if the proportion of added constraints to
|
||||||
// potential constraints drops below this number.
|
// potential constraints drops below this number.
|
||||||
optional double sampling_ratio = 1;
|
optional double sampling_ratio = 1;
|
||||||
|
@ -39,6 +40,10 @@ message ConstraintBuilderOptions {
|
||||||
// Low scores indicate that the scan and map do not look similar.
|
// Low scores indicate that the scan and map do not look similar.
|
||||||
optional double min_score = 4;
|
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.
|
// Threshold below which global localizations are not trusted.
|
||||||
optional double global_localization_min_score = 5;
|
optional double global_localization_min_score = 5;
|
||||||
|
|
||||||
|
|
|
@ -75,6 +75,7 @@ class SparsePoseGraphTest : public ::testing::Test {
|
||||||
max_range = 50.,
|
max_range = 50.,
|
||||||
},
|
},
|
||||||
min_score = 0.5,
|
min_score = 0.5,
|
||||||
|
min_low_resolution_score = 0.5,
|
||||||
global_localization_min_score = 0.6,
|
global_localization_min_score = 0.6,
|
||||||
loop_closure_translation_weight = 1.,
|
loop_closure_translation_weight = 1.,
|
||||||
loop_closure_rotation_weight = 1.,
|
loop_closure_rotation_weight = 1.,
|
||||||
|
|
|
@ -106,12 +106,13 @@ bool FastCorrelativeScanMatcher::Match(
|
||||||
const transform::Rigid3d& initial_pose_estimate,
|
const transform::Rigid3d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& coarse_point_cloud,
|
const sensor::PointCloud& coarse_point_cloud,
|
||||||
const sensor::PointCloud& fine_point_cloud, const float min_score,
|
const sensor::PointCloud& fine_point_cloud, const float min_score,
|
||||||
float* const score, transform::Rigid3d* const pose_estimate,
|
const MatchingFunction& matching_function, float* const score,
|
||||||
|
transform::Rigid3d* const pose_estimate,
|
||||||
float* const rotational_score) const {
|
float* const rotational_score) const {
|
||||||
const SearchParameters search_parameters{
|
const SearchParameters search_parameters{
|
||||||
common::RoundToInt(options_.linear_xy_search_window() / resolution_),
|
common::RoundToInt(options_.linear_xy_search_window() / resolution_),
|
||||||
common::RoundToInt(options_.linear_z_search_window() / resolution_),
|
common::RoundToInt(options_.linear_z_search_window() / resolution_),
|
||||||
options_.angular_search_window()};
|
options_.angular_search_window(), &matching_function};
|
||||||
return MatchWithSearchParameters(
|
return MatchWithSearchParameters(
|
||||||
search_parameters, initial_pose_estimate, coarse_point_cloud,
|
search_parameters, initial_pose_estimate, coarse_point_cloud,
|
||||||
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
|
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
|
||||||
|
@ -121,7 +122,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
||||||
const Eigen::Quaterniond& gravity_alignment,
|
const Eigen::Quaterniond& gravity_alignment,
|
||||||
const sensor::PointCloud& coarse_point_cloud,
|
const sensor::PointCloud& coarse_point_cloud,
|
||||||
const sensor::PointCloud& fine_point_cloud, const float min_score,
|
const sensor::PointCloud& fine_point_cloud, const float min_score,
|
||||||
float* const score, transform::Rigid3d* const pose_estimate,
|
const MatchingFunction& matching_function, float* const score,
|
||||||
|
transform::Rigid3d* const pose_estimate,
|
||||||
float* const rotational_score) const {
|
float* const rotational_score) const {
|
||||||
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
|
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
|
||||||
gravity_alignment);
|
gravity_alignment);
|
||||||
|
@ -132,8 +134,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
||||||
const int linear_window_size =
|
const int linear_window_size =
|
||||||
(width_in_voxels_ + 1) / 2 +
|
(width_in_voxels_ + 1) / 2 +
|
||||||
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
|
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
|
||||||
const SearchParameters search_parameters{linear_window_size,
|
const SearchParameters search_parameters{
|
||||||
linear_window_size, M_PI};
|
linear_window_size, linear_window_size, M_PI, &matching_function};
|
||||||
return MatchWithSearchParameters(
|
return MatchWithSearchParameters(
|
||||||
search_parameters, initial_pose_estimate, coarse_point_cloud,
|
search_parameters, initial_pose_estimate, coarse_point_cloud,
|
||||||
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
|
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
|
||||||
|
@ -161,14 +163,10 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
||||||
precomputation_grid_stack_->max_depth(), min_score);
|
precomputation_grid_stack_->max_depth(), min_score);
|
||||||
if (best_candidate.score > min_score) {
|
if (best_candidate.score > min_score) {
|
||||||
*score = best_candidate.score;
|
*score = best_candidate.score;
|
||||||
const auto& discrete_scan = discrete_scans[best_candidate.scan_index];
|
|
||||||
*pose_estimate =
|
*pose_estimate =
|
||||||
(transform::Rigid3f(
|
GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>();
|
||||||
resolution_ * best_candidate.offset.matrix().cast<float>(),
|
*rotational_score =
|
||||||
Eigen::Quaternionf::Identity()) *
|
discrete_scans[best_candidate.scan_index].rotational_score;
|
||||||
discrete_scan.pose)
|
|
||||||
.cast<double>();
|
|
||||||
*rotational_score = discrete_scan.rotational_score;
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
|
@ -334,18 +332,38 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
|
||||||
return lowest_resolution_candidates;
|
return lowest_resolution_candidates;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
transform::Rigid3f FastCorrelativeScanMatcher::GetPoseFromCandidate(
|
||||||
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
|
const Candidate& candidate) const {
|
||||||
|
return transform::Rigid3f::Translation(
|
||||||
|
resolution_ * candidate.offset.matrix().cast<float>()) *
|
||||||
|
discrete_scans[candidate.scan_index].pose;
|
||||||
|
}
|
||||||
|
|
||||||
Candidate FastCorrelativeScanMatcher::BranchAndBound(
|
Candidate FastCorrelativeScanMatcher::BranchAndBound(
|
||||||
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
|
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const std::vector<Candidate>& candidates, const int candidate_depth,
|
const std::vector<Candidate>& candidates, const int candidate_depth,
|
||||||
float min_score) const {
|
float min_score) const {
|
||||||
if (candidate_depth == 0) {
|
|
||||||
// Return the best candidate.
|
|
||||||
return *candidates.begin();
|
|
||||||
}
|
|
||||||
|
|
||||||
Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero());
|
Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero());
|
||||||
best_high_resolution_candidate.score = min_score;
|
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))) {
|
||||||
|
// We found the best candidate that passes the matching function.
|
||||||
|
return candidate;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// All candidates have good scores but none passes the matching function.
|
||||||
|
return best_high_resolution_candidate;
|
||||||
|
}
|
||||||
|
|
||||||
for (const Candidate& candidate : candidates) {
|
for (const Candidate& candidate : candidates) {
|
||||||
if (candidate.score <= min_score) {
|
if (candidate.score <= min_score) {
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -68,6 +68,8 @@ struct Candidate {
|
||||||
bool operator>(const Candidate& other) const { return score > other.score; }
|
bool operator>(const Candidate& other) const { return score > other.score; }
|
||||||
};
|
};
|
||||||
|
|
||||||
|
using MatchingFunction = std::function<bool(const transform::Rigid3f&)>;
|
||||||
|
|
||||||
class FastCorrelativeScanMatcher {
|
class FastCorrelativeScanMatcher {
|
||||||
public:
|
public:
|
||||||
FastCorrelativeScanMatcher(
|
FastCorrelativeScanMatcher(
|
||||||
|
@ -88,8 +90,8 @@ class FastCorrelativeScanMatcher {
|
||||||
bool Match(const transform::Rigid3d& initial_pose_estimate,
|
bool Match(const transform::Rigid3d& initial_pose_estimate,
|
||||||
const sensor::PointCloud& coarse_point_cloud,
|
const sensor::PointCloud& coarse_point_cloud,
|
||||||
const sensor::PointCloud& fine_point_cloud, float min_score,
|
const sensor::PointCloud& fine_point_cloud, float min_score,
|
||||||
float* score, transform::Rigid3d* pose_estimate,
|
const MatchingFunction& matching_function, float* score,
|
||||||
float* rotational_score) const;
|
transform::Rigid3d* pose_estimate, float* rotational_score) const;
|
||||||
|
|
||||||
// Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which
|
// Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which
|
||||||
// is expected to be approximately gravity aligned. If a score above
|
// is expected to be approximately gravity aligned. If a score above
|
||||||
|
@ -100,7 +102,8 @@ class FastCorrelativeScanMatcher {
|
||||||
bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
|
bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
|
||||||
const sensor::PointCloud& coarse_point_cloud,
|
const sensor::PointCloud& coarse_point_cloud,
|
||||||
const sensor::PointCloud& fine_point_cloud,
|
const sensor::PointCloud& fine_point_cloud,
|
||||||
float min_score, float* score,
|
float min_score,
|
||||||
|
const MatchingFunction& matching_function, float* score,
|
||||||
transform::Rigid3d* pose_estimate,
|
transform::Rigid3d* pose_estimate,
|
||||||
float* rotational_score) const;
|
float* rotational_score) const;
|
||||||
|
|
||||||
|
@ -109,6 +112,7 @@ class FastCorrelativeScanMatcher {
|
||||||
const int linear_xy_window_size; // voxels
|
const int linear_xy_window_size; // voxels
|
||||||
const int linear_z_window_size; // voxels
|
const int linear_z_window_size; // voxels
|
||||||
const double angular_search_window; // radians
|
const double angular_search_window; // radians
|
||||||
|
const MatchingFunction* const matching_function;
|
||||||
};
|
};
|
||||||
|
|
||||||
bool MatchWithSearchParameters(
|
bool MatchWithSearchParameters(
|
||||||
|
@ -138,6 +142,9 @@ class FastCorrelativeScanMatcher {
|
||||||
const std::vector<DiscreteScan>& discrete_scans,
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
const std::vector<Candidate>& candidates,
|
const std::vector<Candidate>& candidates,
|
||||||
int candidate_depth, float min_score) const;
|
int candidate_depth, float min_score) const;
|
||||||
|
transform::Rigid3f GetPoseFromCandidate(
|
||||||
|
const std::vector<DiscreteScan>& discrete_scans,
|
||||||
|
const Candidate& candidate) const;
|
||||||
|
|
||||||
const proto::FastCorrelativeScanMatcherOptions options_;
|
const proto::FastCorrelativeScanMatcherOptions options_;
|
||||||
const float resolution_;
|
const float resolution_;
|
||||||
|
|
|
@ -22,6 +22,7 @@
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||||
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/mapping_3d/range_data_inserter.h"
|
#include "cartographer/mapping_3d/range_data_inserter.h"
|
||||||
#include "cartographer/transform/rigid_transform_test_helpers.h"
|
#include "cartographer/transform/rigid_transform_test_helpers.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
|
@ -32,8 +33,35 @@ namespace mapping_3d {
|
||||||
namespace scan_matching {
|
namespace scan_matching {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
proto::FastCorrelativeScanMatcherOptions
|
class FastCorrelativeScanMatcherTest : public ::testing::Test {
|
||||||
CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
|
protected:
|
||||||
|
FastCorrelativeScanMatcherTest()
|
||||||
|
: range_data_inserter_(CreateRangeDataInserterTestOptions()),
|
||||||
|
options_(CreateFastCorrelativeScanMatcherTestOptions(5)){};
|
||||||
|
|
||||||
|
void SetUp() override {
|
||||||
|
point_cloud_ = {
|
||||||
|
Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f),
|
||||||
|
Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f),
|
||||||
|
Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f),
|
||||||
|
Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f),
|
||||||
|
Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f),
|
||||||
|
Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)};
|
||||||
|
}
|
||||||
|
|
||||||
|
transform::Rigid3f GetRandomPose() {
|
||||||
|
const float x = 0.7f * distribution_(prng_);
|
||||||
|
const float y = 0.7f * distribution_(prng_);
|
||||||
|
const float z = 0.7f * distribution_(prng_);
|
||||||
|
const float theta = 0.2f * distribution_(prng_);
|
||||||
|
return transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) *
|
||||||
|
transform::Rigid3f::Rotation(
|
||||||
|
Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
|
||||||
|
}
|
||||||
|
|
||||||
|
static proto::FastCorrelativeScanMatcherOptions
|
||||||
|
CreateFastCorrelativeScanMatcherTestOptions(
|
||||||
|
const int branch_and_bound_depth) {
|
||||||
auto parameter_dictionary = common::MakeDictionary(
|
auto parameter_dictionary = common::MakeDictionary(
|
||||||
"return {"
|
"return {"
|
||||||
"branch_and_bound_depth = " +
|
"branch_and_bound_depth = " +
|
||||||
|
@ -51,7 +79,7 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
|
||||||
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
|
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
mapping_3d::proto::RangeDataInserterOptions
|
static mapping_3d::proto::RangeDataInserterOptions
|
||||||
CreateRangeDataInserterTestOptions() {
|
CreateRangeDataInserterTestOptions() {
|
||||||
auto parameter_dictionary = common::MakeDictionary(
|
auto parameter_dictionary = common::MakeDictionary(
|
||||||
"return { "
|
"return { "
|
||||||
|
@ -62,57 +90,84 @@ CreateRangeDataInserterTestOptions() {
|
||||||
return CreateRangeDataInserterOptions(parameter_dictionary.get());
|
return CreateRangeDataInserterOptions(parameter_dictionary.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
|
||||||
std::mt19937 prng(42);
|
const proto::FastCorrelativeScanMatcherOptions& options,
|
||||||
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
|
const transform::Rigid3f& pose) {
|
||||||
RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
|
|
||||||
constexpr float kMinScore = 0.1f;
|
|
||||||
const auto options = CreateFastCorrelativeScanMatcherTestOptions(5);
|
|
||||||
|
|
||||||
sensor::PointCloud point_cloud{
|
|
||||||
Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f),
|
|
||||||
Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f),
|
|
||||||
Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f),
|
|
||||||
Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f),
|
|
||||||
Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f),
|
|
||||||
Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)};
|
|
||||||
|
|
||||||
for (int i = 0; i != 20; ++i) {
|
|
||||||
const float x = 0.7f * distribution(prng);
|
|
||||||
const float y = 0.7f * distribution(prng);
|
|
||||||
const float z = 0.7f * distribution(prng);
|
|
||||||
const float theta = 0.2f * distribution(prng);
|
|
||||||
const auto expected_pose =
|
|
||||||
transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) *
|
|
||||||
transform::Rigid3f::Rotation(
|
|
||||||
Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
|
|
||||||
|
|
||||||
HybridGrid hybrid_grid(0.05f);
|
HybridGrid hybrid_grid(0.05f);
|
||||||
range_data_inserter.Insert(
|
range_data_inserter_.Insert(
|
||||||
sensor::RangeData{
|
sensor::RangeData{pose.translation(),
|
||||||
expected_pose.translation(),
|
sensor::TransformPointCloud(point_cloud_, pose),
|
||||||
sensor::TransformPointCloud(point_cloud, expected_pose),
|
|
||||||
{}},
|
{}},
|
||||||
&hybrid_grid);
|
&hybrid_grid);
|
||||||
hybrid_grid.FinishUpdate();
|
hybrid_grid.FinishUpdate();
|
||||||
|
|
||||||
FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
|
return common::make_unique<FastCorrelativeScanMatcher>(
|
||||||
options);
|
hybrid_grid, std::vector<mapping::TrajectoryNode>(), options);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::mt19937 prng_ = std::mt19937(42);
|
||||||
|
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;
|
||||||
|
|
||||||
|
TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
|
||||||
|
for (int i = 0; i != 20; ++i) {
|
||||||
|
const auto expected_pose = GetRandomPose();
|
||||||
|
|
||||||
|
std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
|
||||||
|
GetFastCorrelativeScanMatcher(options_, expected_pose));
|
||||||
|
|
||||||
float score = 0.f;
|
float score = 0.f;
|
||||||
transform::Rigid3d pose_estimate;
|
transform::Rigid3d pose_estimate;
|
||||||
float rotational_score = 0.f;
|
float rotational_score = 0.f;
|
||||||
EXPECT_TRUE(fast_correlative_scan_matcher.Match(
|
EXPECT_TRUE(fast_correlative_scan_matcher->Match(
|
||||||
transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore,
|
transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
|
||||||
&score, &pose_estimate, &rotational_score));
|
[](const transform::Rigid3f&) { return true; }, &score, &pose_estimate,
|
||||||
|
&rotational_score));
|
||||||
EXPECT_LT(kMinScore, score);
|
EXPECT_LT(kMinScore, score);
|
||||||
EXPECT_LT(0.09f, rotational_score);
|
EXPECT_LT(0.09f, rotational_score);
|
||||||
EXPECT_THAT(expected_pose,
|
EXPECT_THAT(expected_pose,
|
||||||
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
|
transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
|
||||||
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
|
<< "Actual: " << transform::ToProto(pose_estimate).DebugString()
|
||||||
<< "\nExpected: " << transform::ToProto(expected_pose).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 false; }, &score, &pose_estimate,
|
||||||
|
&rotational_score));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
|
||||||
|
const auto expected_pose = GetRandomPose();
|
||||||
|
|
||||||
|
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;
|
||||||
|
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));
|
||||||
|
EXPECT_LT(kMinScore, score);
|
||||||
|
EXPECT_LT(0.09f, rotational_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 false; }, &score, &pose_estimate,
|
||||||
|
&rotational_score));
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace scan_matching
|
} // namespace scan_matching
|
||||||
} // namespace mapping_3d
|
} // namespace mapping_3d
|
||||||
|
|
|
@ -0,0 +1,51 @@
|
||||||
|
/*
|
||||||
|
* Copyright 2016 The Cartographer Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
|
||||||
|
|
||||||
|
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) {
|
||||||
|
return [=](const transform::Rigid3f& pose) {
|
||||||
|
return EvaluateLowResolutionScore(
|
||||||
|
*low_resolution_grid,
|
||||||
|
sensor::TransformPointCloud(*points, pose)) >=
|
||||||
|
min_low_resolution_score;
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace scan_matching
|
||||||
|
} // namespace mapping_3d
|
||||||
|
} // namespace cartographer
|
|
@ -0,0 +1,38 @@
|
||||||
|
/*
|
||||||
|
* Copyright 2016 The Cartographer Authors
|
||||||
|
*
|
||||||
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
* you may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at
|
||||||
|
*
|
||||||
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_
|
||||||
|
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_
|
||||||
|
|
||||||
|
#include <functional>
|
||||||
|
|
||||||
|
#include "cartographer/mapping_3d/hybrid_grid.h"
|
||||||
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
} // namespace scan_matching
|
||||||
|
} // namespace mapping_3d
|
||||||
|
} // namespace cartographer
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_
|
|
@ -29,6 +29,7 @@
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/common/math.h"
|
#include "cartographer/common/math.h"
|
||||||
#include "cartographer/common/thread_pool.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/ceres_scan_matcher_options.pb.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
|
@ -179,6 +180,9 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
|
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
|
||||||
const sensor::PointCloud high_resolution_point_cloud =
|
const sensor::PointCloud high_resolution_point_cloud =
|
||||||
high_resolution_adaptive_voxel_filter_.Filter(point_cloud);
|
high_resolution_adaptive_voxel_filter_.Filter(point_cloud);
|
||||||
|
const sensor::PointCloud low_resolution_point_cloud =
|
||||||
|
low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
|
||||||
|
|
||||||
// The 'constraint_transform' (submap i <- scan j) is computed from:
|
// The 'constraint_transform' (submap i <- scan j) is computed from:
|
||||||
// - a 'high_resolution_point_cloud' in scan j and
|
// - a 'high_resolution_point_cloud' in scan j and
|
||||||
// - the initial guess 'initial_pose' (submap i <- scan j).
|
// - the initial guess 'initial_pose' (submap i <- scan j).
|
||||||
|
@ -186,6 +190,10 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
transform::Rigid3d pose_estimate;
|
transform::Rigid3d pose_estimate;
|
||||||
float rotational_score = 0.f;
|
float rotational_score = 0.f;
|
||||||
|
|
||||||
|
const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
|
||||||
|
submap_scan_matcher->low_resolution_hybrid_grid,
|
||||||
|
&low_resolution_point_cloud, options_.min_low_resolution_score());
|
||||||
|
|
||||||
// Compute 'pose_estimate' in three stages:
|
// Compute 'pose_estimate' in three stages:
|
||||||
// 1. Fast estimate using the fast correlative scan matcher.
|
// 1. Fast estimate using the fast correlative scan matcher.
|
||||||
// 2. Prune if the score is too low.
|
// 2. Prune if the score is too low.
|
||||||
|
@ -193,8 +201,8 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
if (match_full_submap) {
|
if (match_full_submap) {
|
||||||
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
|
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
|
||||||
initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
|
initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
|
||||||
options_.global_localization_min_score(), &score, &pose_estimate,
|
options_.global_localization_min_score(), low_resolution_matcher,
|
||||||
&rotational_score)) {
|
&score, &pose_estimate, &rotational_score)) {
|
||||||
CHECK_GT(score, options_.global_localization_min_score());
|
CHECK_GT(score, options_.global_localization_min_score());
|
||||||
CHECK_GE(node_id.trajectory_id, 0);
|
CHECK_GE(node_id.trajectory_id, 0);
|
||||||
CHECK_GE(submap_id.trajectory_id, 0);
|
CHECK_GE(submap_id.trajectory_id, 0);
|
||||||
|
@ -206,7 +214,8 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
} else {
|
} else {
|
||||||
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
|
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
|
||||||
initial_pose, high_resolution_point_cloud, point_cloud,
|
initial_pose, high_resolution_point_cloud, point_cloud,
|
||||||
options_.min_score(), &score, &pose_estimate, &rotational_score)) {
|
options_.min_score(), low_resolution_matcher, &score,
|
||||||
|
&pose_estimate, &rotational_score)) {
|
||||||
// We've reported a successful local match.
|
// We've reported a successful local match.
|
||||||
CHECK_GT(score, options_.min_score());
|
CHECK_GT(score, options_.min_score());
|
||||||
} else {
|
} else {
|
||||||
|
@ -222,9 +231,6 @@ void ConstraintBuilder::ComputeConstraint(
|
||||||
// Use the CSM estimate as both the initial and previous pose. This has the
|
// Use the CSM estimate as both the initial and previous pose. This has the
|
||||||
// effect that, in the absence of better information, we prefer the original
|
// effect that, in the absence of better information, we prefer the original
|
||||||
// CSM estimate.
|
// CSM estimate.
|
||||||
const sensor::PointCloud low_resolution_point_cloud =
|
|
||||||
low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
|
|
||||||
|
|
||||||
ceres::Solver::Summary unused_summary;
|
ceres::Solver::Summary unused_summary;
|
||||||
transform::Rigid3d constraint_transform;
|
transform::Rigid3d constraint_transform;
|
||||||
ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
|
ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
|
||||||
|
|
|
@ -23,6 +23,7 @@ SPARSE_POSE_GRAPH = {
|
||||||
max_range = 50.,
|
max_range = 50.,
|
||||||
},
|
},
|
||||||
min_score = 0.55,
|
min_score = 0.55,
|
||||||
|
min_low_resolution_score = 0.55,
|
||||||
global_localization_min_score = 0.6,
|
global_localization_min_score = 0.6,
|
||||||
loop_closure_translation_weight = 1.1e4,
|
loop_closure_translation_weight = 1.1e4,
|
||||||
loop_closure_rotation_weight = 1e5,
|
loop_closure_rotation_weight = 1e5,
|
||||||
|
|
|
@ -96,6 +96,7 @@ cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
|
||||||
=====================================================================
|
=====================================================================
|
||||||
|
|
||||||
double sampling_ratio
|
double sampling_ratio
|
||||||
|
Next ID: 18
|
||||||
A constraint will be added if the proportion of added constraints to
|
A constraint will be added if the proportion of added constraints to
|
||||||
potential constraints drops below this number.
|
potential constraints drops below this number.
|
||||||
|
|
||||||
|
@ -110,6 +111,10 @@ double min_score
|
||||||
Threshold for the scan match score below which a match is not considered.
|
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.
|
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
|
double global_localization_min_score
|
||||||
Threshold below which global localizations are not trusted.
|
Threshold below which global localizations are not trusted.
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue