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

Change 2D scan matching to use 3D point clouds. (#60)

master
Wolfgang Hess 2016-10-13 17:52:05 +02:00 committed by GitHub
parent 9006fb6fb1
commit 5aad2d6feb
17 changed files with 104 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -107,9 +107,8 @@ void LocalTrajectoryBuilder::ScanMatch(
transform::Rigid2d initial_ceres_pose = pose_prediction_2d; transform::Rigid2d initial_ceres_pose = pose_prediction_2d;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter( sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options()); options_.adaptive_voxel_filter_options());
const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d = const sensor::PointCloud filtered_point_cloud_in_tracking_2d =
sensor::ProjectToPointCloud2D( adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns);
adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns));
if (options_.use_online_correlative_scan_matching()) { if (options_.use_online_correlative_scan_matching()) {
real_time_correlative_scan_matcher_.Match( real_time_correlative_scan_matcher_.Match(
pose_prediction_2d, filtered_point_cloud_in_tracking_2d, pose_prediction_2d, filtered_point_cloud_in_tracking_2d,

2
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View File

@ -66,7 +66,7 @@ CeresScanMatcher::~CeresScanMatcher() {}
void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
transform::Rigid2d* const pose_estimate, transform::Rigid2d* const pose_estimate,
kalman_filter::Pose2DCovariance* const covariance, kalman_filter::Pose2DCovariance* const covariance,

2
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h
View File

@ -49,7 +49,7 @@ class CeresScanMatcher {
// the solver 'summary'. // the solver 'summary'.
void Match(const transform::Rigid2d& previous_pose, void Match(const transform::Rigid2d& previous_pose,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate, transform::Rigid2d* pose_estimate,
kalman_filter::Pose2DCovariance* covariance, kalman_filter::Pose2DCovariance* covariance,

4
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -40,7 +40,7 @@ class CeresScanMatcherTest : public ::testing::Test {
probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5),
mapping::kMaxProbability); mapping::kMaxProbability);
point_cloud_.emplace_back(-3., 2.); point_cloud_.emplace_back(-3.f, 2.f, 0.f);
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
@ -74,7 +74,7 @@ class CeresScanMatcherTest : public ::testing::Test {
} }
ProbabilityGrid probability_grid_; ProbabilityGrid probability_grid_;
sensor::PointCloud2D point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_; std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
}; };

26
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc
View File

@ -26,14 +26,14 @@ namespace scan_matching {
SearchParameters::SearchParameters(const double linear_search_window, SearchParameters::SearchParameters(const double linear_search_window,
const double angular_search_window, const double angular_search_window,
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const double resolution) const double resolution)
: resolution(resolution) { : resolution(resolution) {
// We set this value to something on the order of resolution to make sure that // We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined. // the std::acos() below is defined.
float max_scan_range = 3.f * resolution; float max_scan_range = 3.f * resolution;
for (const Eigen::Vector2f& point : point_cloud) { for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.norm(); const float range = point.head<2>().norm();
max_scan_range = std::max(range, max_scan_range); max_scan_range = std::max(range, max_scan_range);
} }
const double kSafetyMargin = 1. - 1e-3; const double kSafetyMargin = 1. - 1e-3;
@ -91,10 +91,10 @@ void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
} }
} }
std::vector<sensor::PointCloud2D> GenerateRotatedScans( std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters) { const SearchParameters& search_parameters) {
std::vector<sensor::PointCloud2D> rotated_scans; std::vector<sensor::PointCloud> rotated_scans;
rotated_scans.reserve(search_parameters.num_scans); rotated_scans.reserve(search_parameters.num_scans);
double delta_theta = -search_parameters.num_angular_perturbations * double delta_theta = -search_parameters.num_angular_perturbations *
@ -102,22 +102,24 @@ std::vector<sensor::PointCloud2D> GenerateRotatedScans(
for (int scan_index = 0; scan_index < search_parameters.num_scans; for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index, ++scan_index,
delta_theta += search_parameters.angular_perturbation_step_size) { delta_theta += search_parameters.angular_perturbation_step_size) {
rotated_scans.push_back(sensor::TransformPointCloud2D( rotated_scans.push_back(sensor::TransformPointCloud(
point_cloud, transform::Rigid2f::Rotation(delta_theta))); point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));
} }
return rotated_scans; return rotated_scans;
} }
std::vector<DiscreteScan> DiscretizeScans( std::vector<DiscreteScan> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud2D>& scans, const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) { const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan> discrete_scans; std::vector<DiscreteScan> discrete_scans;
discrete_scans.reserve(scans.size()); discrete_scans.reserve(scans.size());
for (const sensor::PointCloud2D& scan : scans) { for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back(); discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size()); discrete_scans.back().reserve(scan.size());
for (const Eigen::Vector2f& point : scan) { for (const Eigen::Vector3f& point : scan) {
const Eigen::Vector2f translated_point = initial_translation * point; const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.head<2>();
discrete_scans.back().push_back( discrete_scans.back().push_back(
map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(), map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(),
translated_point.y())); translated_point.y()));

8
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h
View File

@ -42,7 +42,7 @@ struct SearchParameters {
}; };
SearchParameters(double linear_search_window, double angular_search_window, SearchParameters(double linear_search_window, double angular_search_window,
const sensor::PointCloud2D& point_cloud, double resolution); const sensor::PointCloud& point_cloud, double resolution);
// For testing. // For testing.
SearchParameters(int num_linear_perturbations, int num_angular_perturbations, SearchParameters(int num_linear_perturbations, int num_angular_perturbations,
@ -60,14 +60,14 @@ struct SearchParameters {
}; };
// Generates a collection of rotated scans. // Generates a collection of rotated scans.
std::vector<sensor::PointCloud2D> GenerateRotatedScans( std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters); const SearchParameters& search_parameters);
// Translates and discretizes the rotated scans into a vector of integer // Translates and discretizes the rotated scans into a vector of integer
// indices. // indices.
std::vector<DiscreteScan> DiscretizeScans( std::vector<DiscreteScan> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud2D>& scans, const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation); const Eigen::Translation2f& initial_translation);
// A possible solution. // A possible solution.

24
cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc
View File

@ -57,9 +57,9 @@ TEST(Candidate, Construction) {
} }
TEST(GenerateRotatedScans, GenerateRotatedScans) { TEST(GenerateRotatedScans, GenerateRotatedScans) {
sensor::PointCloud2D point_cloud; sensor::PointCloud point_cloud;
point_cloud.emplace_back(-1., 1.); point_cloud.emplace_back(-1.f, 1.f, 0.f);
const std::vector<sensor::PointCloud2D> scans = const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.)); GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.));
EXPECT_EQ(3, scans.size()); EXPECT_EQ(3, scans.size());
EXPECT_NEAR(1., scans[0][0].x(), 1e-6); EXPECT_NEAR(1., scans[0][0].x(), 1e-6);
@ -71,17 +71,17 @@ TEST(GenerateRotatedScans, GenerateRotatedScans) {
} }
TEST(DiscretizeScans, DiscretizeScans) { TEST(DiscretizeScans, DiscretizeScans) {
sensor::PointCloud2D point_cloud; sensor::PointCloud point_cloud;
point_cloud.emplace_back(0.025, 0.175); point_cloud.emplace_back(0.025f, 0.175f, 0.f);
point_cloud.emplace_back(-0.025, 0.175); point_cloud.emplace_back(-0.025f, 0.175f, 0.f);
point_cloud.emplace_back(-0.075, 0.175); point_cloud.emplace_back(-0.075f, 0.175f, 0.f);
point_cloud.emplace_back(-0.125, 0.175); point_cloud.emplace_back(-0.125f, 0.175f, 0.f);
point_cloud.emplace_back(-0.125, 0.125); point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud.emplace_back(-0.125, 0.075); point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud.emplace_back(-0.125, 0.025); point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
CellLimits(6, 6)); CellLimits(6, 6));
const std::vector<sensor::PointCloud2D> scans = const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans = const std::vector<DiscreteScan> discrete_scans =
DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity()); DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity());

18
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -208,8 +208,8 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
bool FastCorrelativeScanMatcher::Match( bool FastCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const float min_score, const sensor::PointCloud& point_cloud, const float min_score, float* score,
float* score, transform::Rigid2d* pose_estimate) const { transform::Rigid2d* pose_estimate) const {
const SearchParameters search_parameters(options_.linear_search_window(), const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(), options_.angular_search_window(),
point_cloud, limits_.resolution()); point_cloud, limits_.resolution());
@ -219,7 +219,7 @@ bool FastCorrelativeScanMatcher::Match(
} }
bool FastCorrelativeScanMatcher::MatchFullSubmap( bool FastCorrelativeScanMatcher::MatchFullSubmap(
const sensor::PointCloud2D& point_cloud, float min_score, float* score, const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const { transform::Rigid2d* pose_estimate) const {
// Compute a search window around the center of the submap that includes it // Compute a search window around the center of the submap that includes it
// fully. // fully.
@ -239,17 +239,17 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
bool FastCorrelativeScanMatcher::MatchWithSearchParameters( bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
SearchParameters search_parameters, SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, float min_score, float* score, const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const { transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(score); CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate); CHECK_NOTNULL(pose_estimate);
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud = const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
sensor::TransformPointCloud2D( point_cloud,
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
transform::Rigid2d::Rotation(initial_rotation).cast<float>()); initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const std::vector<sensor::PointCloud2D> rotated_scans = const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters); GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
limits_, rotated_scans, limits_, rotated_scans,

6
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -111,14 +111,14 @@ class FastCorrelativeScanMatcher {
// is possible, true is returned, and 'score' and 'pose_estimate' are updated // is possible, true is returned, and 'score' and 'pose_estimate' are updated
// with the result. // with the result.
bool Match(const transform::Rigid2d& initial_pose_estimate, bool Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, float min_score, const sensor::PointCloud& point_cloud, float min_score,
float* score, transform::Rigid2d* pose_estimate) const; float* score, transform::Rigid2d* pose_estimate) const;
// Aligns 'point_cloud' within the full 'probability_grid', i.e., not // Aligns 'point_cloud' within the full 'probability_grid', i.e., not
// restricted to the configured search window. If a score above 'min_score' // restricted to the configured search window. If a score above 'min_score'
// (excluding equality) is possible, true is returned, and 'score' and // (excluding equality) is possible, true is returned, and 'score' and
// 'pose_estimate' are updated with the result. // 'pose_estimate' are updated with the result.
bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score, bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score,
float* score, transform::Rigid2d* pose_estimate) const; float* score, transform::Rigid2d* pose_estimate) const;
private: private:
@ -128,7 +128,7 @@ class FastCorrelativeScanMatcher {
bool MatchWithSearchParameters( bool MatchWithSearchParameters(
SearchParameters search_parameters, SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, float min_score, float* score, const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const; transform::Rigid2d* pose_estimate) const;
std::vector<Candidate> ComputeLowestResolutionCandidates( std::vector<Candidate> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans, const std::vector<DiscreteScan>& discrete_scans,

51
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -133,13 +133,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);
sensor::PointCloud2D point_cloud; sensor::PointCloud point_cloud;
point_cloud.emplace_back(-2.5f, 0.5f); point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
point_cloud.emplace_back(-2.f, 0.5f); point_cloud.emplace_back(-2.f, 0.5f, 0.f);
point_cloud.emplace_back(0.f, -0.5f); point_cloud.emplace_back(0.f, -0.5f, 0.f);
point_cloud.emplace_back(0.5f, -1.6f); point_cloud.emplace_back(0.5f, -1.6f, 0.f);
point_cloud.emplace_back(2.5f, 0.5f); point_cloud.emplace_back(2.5f, 0.5f, 0.f);
point_cloud.emplace_back(2.5f, 1.7f); point_cloud.emplace_back(2.5f, 1.7f, 0.f);
for (int i = 0; i != 50; ++i) { for (int i = 0; i != 50; ++i) {
const transform::Rigid2f expected_pose( const transform::Rigid2f expected_pose(
@ -149,11 +149,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
ProbabilityGrid probability_grid( ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate(); probability_grid.StartUpdate();
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(), laser_fan_inserter.Insert(
sensor::TransformPointCloud2D( sensor::LaserFan{
point_cloud, expected_pose), expected_pose.translation(),
{}}, sensor::TransformPointCloud2D(
&probability_grid); sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
{}},
&probability_grid);
FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options); options);
@ -177,20 +179,24 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
constexpr float kMinScore = 0.1f; constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);
sensor::PointCloud2D unperturbed_point_cloud; sensor::PointCloud unperturbed_point_cloud;
unperturbed_point_cloud.emplace_back(-2.5, 0.5); unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(-2.25, 0.5); unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(0., 0.5); unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(0.25, 1.6); unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f);
unperturbed_point_cloud.emplace_back(2.5, 0.5); unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(2.0, 1.8); unperturbed_point_cloud.emplace_back(2.0f, 1.8f, 0.f);
for (int i = 0; i != 20; ++i) { for (int i = 0; i != 20; ++i) {
const transform::Rigid2f perturbation( const transform::Rigid2f perturbation(
{10. * distribution(prng), 10. * distribution(prng)}, {10. * distribution(prng), 10. * distribution(prng)},
1.6 * distribution(prng)); 1.6 * distribution(prng));
const sensor::PointCloud2D point_cloud = const sensor::PointCloud point_cloud = sensor::TransformPointCloud(
sensor::TransformPointCloud2D(unperturbed_point_cloud, perturbation); unperturbed_point_cloud,
transform::Rigid3f(Eigen::Vector3f(perturbation.translation().x(),
perturbation.translation().y(), 0.f),
Eigen::AngleAxisf(perturbation.rotation().angle(),
Eigen::Vector3f::UnitZ())));
const transform::Rigid2f expected_pose = const transform::Rigid2f expected_pose =
transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)}, transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)},
0.5 * distribution(prng)) * 0.5 * distribution(prng)) *
@ -202,7 +208,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
laser_fan_inserter.Insert( laser_fan_inserter.Insert(
sensor::LaserFan{ sensor::LaserFan{
(expected_pose * perturbation).translation(), (expected_pose * perturbation).translation(),
sensor::TransformPointCloud2D(point_cloud, expected_pose), sensor::TransformPointCloud2D(
sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
{}}, {}},
&probability_grid); &probability_grid);

4
cartographer/mapping_2d/scan_matching/fast_global_localizer.cc
View File

@ -28,14 +28,14 @@ bool PerformGlobalLocalization(
const std::vector< const std::vector<
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
matchers, matchers,
const cartographer::sensor::PointCloud2D& point_cloud, const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* const best_pose_estimate, float* const best_score) { transform::Rigid2d* const best_pose_estimate, float* const best_score) {
CHECK(best_pose_estimate != nullptr) CHECK(best_pose_estimate != nullptr)
<< "Need a non-null output_pose_estimate!"; << "Need a non-null output_pose_estimate!";
CHECK(best_score != nullptr) << "Need a non-null best_score!"; CHECK(best_score != nullptr) << "Need a non-null best_score!";
*best_score = cutoff; *best_score = cutoff;
transform::Rigid2d pose_estimate; transform::Rigid2d pose_estimate;
const sensor::PointCloud2D filtered_point_cloud = const sensor::PointCloud filtered_point_cloud =
voxel_filter.Filter(point_cloud); voxel_filter.Filter(point_cloud);
bool success = false; bool success = false;
if (matchers.size() == 0) { if (matchers.size() == 0) {

2
cartographer/mapping_2d/scan_matching/fast_global_localizer.h
View File

@ -41,7 +41,7 @@ bool PerformGlobalLocalization(
const std::vector< const std::vector<
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
matchers, matchers,
const cartographer::sensor::PointCloud2D& point_cloud, const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* best_pose_estimate, float* best_score); transform::Rigid2d* best_pose_estimate, float* best_score);
} // namespace scan_matching } // namespace scan_matching

4
cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h
View File

@ -36,7 +36,7 @@ class OccupiedSpaceCostFunctor {
// Creates an OccupiedSpaceCostFunctor using the specified map, resolution // Creates an OccupiedSpaceCostFunctor using the specified map, resolution
// level, and point cloud. // level, and point cloud.
OccupiedSpaceCostFunctor(const double scaling_factor, OccupiedSpaceCostFunctor(const double scaling_factor,
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid) const ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor), : scaling_factor_(scaling_factor),
point_cloud_(point_cloud), point_cloud_(point_cloud),
@ -107,7 +107,7 @@ class OccupiedSpaceCostFunctor {
}; };
const double scaling_factor_; const double scaling_factor_;
const sensor::PointCloud2D& point_cloud_; const sensor::PointCloud& point_cloud_;
const ProbabilityGrid& probability_grid_; const ProbabilityGrid& probability_grid_;
}; };

12
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc
View File

@ -90,21 +90,21 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
double RealTimeCorrelativeScanMatcher::Match( double RealTimeCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const { transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate); CHECK_NOTNULL(pose_estimate);
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud = const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
sensor::TransformPointCloud2D( point_cloud,
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
transform::Rigid2d::Rotation(initial_rotation).cast<float>()); initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const SearchParameters search_parameters( const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(), options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, probability_grid.limits().resolution()); rotated_point_cloud, probability_grid.limits().resolution());
const std::vector<sensor::PointCloud2D> rotated_scans = const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters); GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
probability_grid.limits(), rotated_scans, probability_grid.limits(), rotated_scans,

2
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h
View File

@ -68,7 +68,7 @@ class RealTimeCorrelativeScanMatcher {
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
// returns the score. // returns the score.
double Match(const transform::Rigid2d& initial_pose_estimate, double Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const; transform::Rigid2d* pose_estimate) const;

24
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -49,16 +49,18 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
laser_fan_inserter_ = common::make_unique<LaserFanInserter>( laser_fan_inserter_ = common::make_unique<LaserFanInserter>(
CreateLaserFanInserterOptions(parameter_dictionary.get())); CreateLaserFanInserterOptions(parameter_dictionary.get()));
} }
point_cloud_.emplace_back(0.025, 0.175); point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.025, 0.175); point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.075, 0.175); point_cloud_.emplace_back(-0.075f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.125, 0.175); point_cloud_.emplace_back(-0.125f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.125, 0.125); point_cloud_.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud_.emplace_back(-0.125, 0.075); point_cloud_.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud_.emplace_back(-0.125, 0.025); point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
probability_grid_.StartUpdate(); probability_grid_.StartUpdate();
laser_fan_inserter_->Insert( laser_fan_inserter_->Insert(
sensor::LaserFan{Eigen::Vector2f::Zero(), point_cloud_, {}}, sensor::LaserFan{Eigen::Vector2f::Zero(),
sensor::ProjectToPointCloud2D(point_cloud_),
{}},
&probability_grid_); &probability_grid_);
{ {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
@ -77,14 +79,14 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
ProbabilityGrid probability_grid_; ProbabilityGrid probability_grid_;
std::unique_ptr<LaserFanInserter> laser_fan_inserter_; std::unique_ptr<LaserFanInserter> laser_fan_inserter_;
sensor::PointCloud2D point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher> std::unique_ptr<RealTimeCorrelativeScanMatcher>
real_time_correlative_scan_matcher_; real_time_correlative_scan_matcher_;
}; };
TEST_F(RealTimeCorrelativeScanMatcherTest, TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePerfectHighResolutionCandidate) { ScorePerfectHighResolutionCandidate) {
const std::vector<sensor::PointCloud2D> scans = const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
@ -102,7 +104,7 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
TEST_F(RealTimeCorrelativeScanMatcherTest, TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidate) { ScorePartiallyCorrectHighResolutionCandidate) {
const std::vector<sensor::PointCloud2D> scans = const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); probability_grid_.limits(), scans, Eigen::Translation2f::Identity());

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

@ -181,8 +181,8 @@ void ConstraintBuilder::ComputeConstraint(
ComputeSubmapPose(*submap) * initial_relative_pose; ComputeSubmapPose(*submap) * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher = const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_index); GetSubmapScanMatcher(submap_index);
const sensor::PointCloud2D filtered_point_cloud = const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(*point_cloud); sensor::ToPointCloud(adaptive_voxel_filter_.Filter(*point_cloud));
// The 'constraint_transform' (i <- j) is computed from: // The 'constraint_transform' (i <- j) is computed from:
// - a 'filtered_point_cloud' in j, // - a 'filtered_point_cloud' in j,