parent
297e9cc02d
commit
2e53586818
|
@ -36,6 +36,8 @@ 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,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";
|
||||
|
||||
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;
|
||||
|
@ -39,6 +40,10 @@ 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;
|
||||
|
||||
|
|
|
@ -75,6 +75,7 @@ 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.,
|
||||
|
|
|
@ -106,12 +106,13 @@ 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,
|
||||
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 {
|
||||
const SearchParameters search_parameters{
|
||||
common::RoundToInt(options_.linear_xy_search_window() / resolution_),
|
||||
common::RoundToInt(options_.linear_z_search_window() / resolution_),
|
||||
options_.angular_search_window()};
|
||||
options_.angular_search_window(), &matching_function};
|
||||
return MatchWithSearchParameters(
|
||||
search_parameters, initial_pose_estimate, coarse_point_cloud,
|
||||
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
|
||||
|
@ -121,7 +122,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
|||
const Eigen::Quaterniond& gravity_alignment,
|
||||
const sensor::PointCloud& coarse_point_cloud,
|
||||
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 {
|
||||
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
|
||||
gravity_alignment);
|
||||
|
@ -132,8 +134,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
|||
const int linear_window_size =
|
||||
(width_in_voxels_ + 1) / 2 +
|
||||
common::RoundToInt(max_point_distance / resolution_ + 0.5f);
|
||||
const SearchParameters search_parameters{linear_window_size,
|
||||
linear_window_size, M_PI};
|
||||
const SearchParameters search_parameters{
|
||||
linear_window_size, linear_window_size, M_PI, &matching_function};
|
||||
return MatchWithSearchParameters(
|
||||
search_parameters, initial_pose_estimate, coarse_point_cloud,
|
||||
fine_point_cloud, min_score, score, pose_estimate, rotational_score);
|
||||
|
@ -161,14 +163,10 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
|
|||
precomputation_grid_stack_->max_depth(), min_score);
|
||||
if (best_candidate.score > min_score) {
|
||||
*score = best_candidate.score;
|
||||
const auto& discrete_scan = discrete_scans[best_candidate.scan_index];
|
||||
*pose_estimate =
|
||||
(transform::Rigid3f(
|
||||
resolution_ * best_candidate.offset.matrix().cast<float>(),
|
||||
Eigen::Quaternionf::Identity()) *
|
||||
discrete_scan.pose)
|
||||
.cast<double>();
|
||||
*rotational_score = discrete_scan.rotational_score;
|
||||
GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>();
|
||||
*rotational_score =
|
||||
discrete_scans[best_candidate.scan_index].rotational_score;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -334,18 +332,38 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
|
|||
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(
|
||||
const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
|
||||
const std::vector<DiscreteScan>& discrete_scans,
|
||||
const std::vector<Candidate>& candidates, const int candidate_depth,
|
||||
float min_score) const {
|
||||
if (candidate_depth == 0) {
|
||||
// Return the best candidate.
|
||||
return *candidates.begin();
|
||||
}
|
||||
|
||||
Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero());
|
||||
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) {
|
||||
if (candidate.score <= min_score) {
|
||||
break;
|
||||
|
|
|
@ -68,6 +68,8 @@ struct Candidate {
|
|||
bool operator>(const Candidate& other) const { return score > other.score; }
|
||||
};
|
||||
|
||||
using MatchingFunction = std::function<bool(const transform::Rigid3f&)>;
|
||||
|
||||
class FastCorrelativeScanMatcher {
|
||||
public:
|
||||
FastCorrelativeScanMatcher(
|
||||
|
@ -88,8 +90,8 @@ class FastCorrelativeScanMatcher {
|
|||
bool Match(const transform::Rigid3d& initial_pose_estimate,
|
||||
const sensor::PointCloud& coarse_point_cloud,
|
||||
const sensor::PointCloud& fine_point_cloud, float min_score,
|
||||
float* score, transform::Rigid3d* pose_estimate,
|
||||
float* rotational_score) const;
|
||||
const MatchingFunction& matching_function, float* score,
|
||||
transform::Rigid3d* pose_estimate, float* rotational_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
|
||||
|
@ -100,7 +102,8 @@ class FastCorrelativeScanMatcher {
|
|||
bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
|
||||
const sensor::PointCloud& coarse_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,
|
||||
float* rotational_score) const;
|
||||
|
||||
|
@ -109,6 +112,7 @@ class FastCorrelativeScanMatcher {
|
|||
const int linear_xy_window_size; // voxels
|
||||
const int linear_z_window_size; // voxels
|
||||
const double angular_search_window; // radians
|
||||
const MatchingFunction* const matching_function;
|
||||
};
|
||||
|
||||
bool MatchWithSearchParameters(
|
||||
|
@ -138,6 +142,9 @@ class FastCorrelativeScanMatcher {
|
|||
const std::vector<DiscreteScan>& discrete_scans,
|
||||
const std::vector<Candidate>& candidates,
|
||||
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 float resolution_;
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
#include <string>
|
||||
|
||||
#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/transform/rigid_transform_test_helpers.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
@ -32,87 +33,141 @@ namespace mapping_3d {
|
|||
namespace scan_matching {
|
||||
namespace {
|
||||
|
||||
proto::FastCorrelativeScanMatcherOptions
|
||||
CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
|
||||
auto parameter_dictionary = common::MakeDictionary(
|
||||
"return {"
|
||||
"branch_and_bound_depth = " +
|
||||
std::to_string(branch_and_bound_depth) +
|
||||
", "
|
||||
"full_resolution_depth = " +
|
||||
std::to_string(branch_and_bound_depth) +
|
||||
", "
|
||||
"rotational_histogram_size = 30, "
|
||||
"min_rotational_score = 0.1, "
|
||||
"linear_xy_search_window = 0.8, "
|
||||
"linear_z_search_window = 0.8, "
|
||||
"angular_search_window = 0.3, "
|
||||
"}");
|
||||
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
|
||||
}
|
||||
class FastCorrelativeScanMatcherTest : public ::testing::Test {
|
||||
protected:
|
||||
FastCorrelativeScanMatcherTest()
|
||||
: range_data_inserter_(CreateRangeDataInserterTestOptions()),
|
||||
options_(CreateFastCorrelativeScanMatcherTestOptions(5)){};
|
||||
|
||||
mapping_3d::proto::RangeDataInserterOptions
|
||||
CreateRangeDataInserterTestOptions() {
|
||||
auto parameter_dictionary = common::MakeDictionary(
|
||||
"return { "
|
||||
"hit_probability = 0.7, "
|
||||
"miss_probability = 0.4, "
|
||||
"num_free_space_voxels = 5, "
|
||||
"}");
|
||||
return CreateRangeDataInserterOptions(parameter_dictionary.get());
|
||||
}
|
||||
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)};
|
||||
}
|
||||
|
||||
TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||
std::mt19937 prng(42);
|
||||
std::uniform_real_distribution<float> distribution(-1.f, 1.f);
|
||||
RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
|
||||
constexpr float kMinScore = 0.1f;
|
||||
const auto options = CreateFastCorrelativeScanMatcherTestOptions(5);
|
||||
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()));
|
||||
}
|
||||
|
||||
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)};
|
||||
static proto::FastCorrelativeScanMatcherOptions
|
||||
CreateFastCorrelativeScanMatcherTestOptions(
|
||||
const int branch_and_bound_depth) {
|
||||
auto parameter_dictionary = common::MakeDictionary(
|
||||
"return {"
|
||||
"branch_and_bound_depth = " +
|
||||
std::to_string(branch_and_bound_depth) +
|
||||
", "
|
||||
"full_resolution_depth = " +
|
||||
std::to_string(branch_and_bound_depth) +
|
||||
", "
|
||||
"rotational_histogram_size = 30, "
|
||||
"min_rotational_score = 0.1, "
|
||||
"linear_xy_search_window = 0.8, "
|
||||
"linear_z_search_window = 0.8, "
|
||||
"angular_search_window = 0.3, "
|
||||
"}");
|
||||
return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
|
||||
}
|
||||
|
||||
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()));
|
||||
static mapping_3d::proto::RangeDataInserterOptions
|
||||
CreateRangeDataInserterTestOptions() {
|
||||
auto parameter_dictionary = common::MakeDictionary(
|
||||
"return { "
|
||||
"hit_probability = 0.7, "
|
||||
"miss_probability = 0.4, "
|
||||
"num_free_space_voxels = 5, "
|
||||
"}");
|
||||
return CreateRangeDataInserterOptions(parameter_dictionary.get());
|
||||
}
|
||||
|
||||
std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
|
||||
const proto::FastCorrelativeScanMatcherOptions& options,
|
||||
const transform::Rigid3f& pose) {
|
||||
HybridGrid hybrid_grid(0.05f);
|
||||
range_data_inserter.Insert(
|
||||
sensor::RangeData{
|
||||
expected_pose.translation(),
|
||||
sensor::TransformPointCloud(point_cloud, expected_pose),
|
||||
{}},
|
||||
range_data_inserter_.Insert(
|
||||
sensor::RangeData{pose.translation(),
|
||||
sensor::TransformPointCloud(point_cloud_, pose),
|
||||
{}},
|
||||
&hybrid_grid);
|
||||
hybrid_grid.FinishUpdate();
|
||||
|
||||
FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
|
||||
options);
|
||||
return common::make_unique<FastCorrelativeScanMatcher>(
|
||||
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;
|
||||
transform::Rigid3d pose_estimate;
|
||||
float rotational_score = 0.f;
|
||||
EXPECT_TRUE(fast_correlative_scan_matcher.Match(
|
||||
transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore,
|
||||
&score, &pose_estimate, &rotational_score));
|
||||
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));
|
||||
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->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 scan_matching
|
||||
} // 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/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"
|
||||
|
@ -179,6 +180,9 @@ void ConstraintBuilder::ComputeConstraint(
|
|||
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
|
||||
const sensor::PointCloud high_resolution_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:
|
||||
// - a 'high_resolution_point_cloud' in scan j and
|
||||
// - the initial guess 'initial_pose' (submap i <- scan j).
|
||||
|
@ -186,6 +190,10 @@ void ConstraintBuilder::ComputeConstraint(
|
|||
transform::Rigid3d pose_estimate;
|
||||
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:
|
||||
// 1. Fast estimate using the fast correlative scan matcher.
|
||||
// 2. Prune if the score is too low.
|
||||
|
@ -193,8 +201,8 @@ void ConstraintBuilder::ComputeConstraint(
|
|||
if (match_full_submap) {
|
||||
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
|
||||
initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
|
||||
options_.global_localization_min_score(), &score, &pose_estimate,
|
||||
&rotational_score)) {
|
||||
options_.global_localization_min_score(), low_resolution_matcher,
|
||||
&score, &pose_estimate, &rotational_score)) {
|
||||
CHECK_GT(score, options_.global_localization_min_score());
|
||||
CHECK_GE(node_id.trajectory_id, 0);
|
||||
CHECK_GE(submap_id.trajectory_id, 0);
|
||||
|
@ -206,7 +214,8 @@ void ConstraintBuilder::ComputeConstraint(
|
|||
} else {
|
||||
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
|
||||
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.
|
||||
CHECK_GT(score, options_.min_score());
|
||||
} else {
|
||||
|
@ -222,9 +231,6 @@ void ConstraintBuilder::ComputeConstraint(
|
|||
// 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
|
||||
// CSM estimate.
|
||||
const sensor::PointCloud low_resolution_point_cloud =
|
||||
low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
|
||||
|
||||
ceres::Solver::Summary unused_summary;
|
||||
transform::Rigid3d constraint_transform;
|
||||
ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
|
||||
|
|
|
@ -23,6 +23,7 @@ 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,
|
||||
|
|
|
@ -96,6 +96,7 @@ 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.
|
||||
|
||||
|
@ -110,6 +111,10 @@ 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.
|
||||
|
||||
|
|
Loading…
Reference in New Issue