Skip to content

Commit

Permalink
Change 2D scan matching to use 3D point clouds. (#60)
Browse files Browse the repository at this point in the history
  • Loading branch information
@wohe
wohe committed Oct 13, 2016
1 parent 9006fb6 commit 5aad2d6
Show file tree
Hide file tree
Showing 17 changed files with 104 additions and 94 deletions.
5 changes: 2 additions & 3 deletions cartographer/mapping_2d/local_trajectory_builder.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,8 @@ void LocalTrajectoryBuilder::ScanMatch(
transform::Rigid2d initial_ceres_pose = pose_prediction_2d;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
sensor::ProjectToPointCloud2D(
adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns));
const sensor::PointCloud filtered_point_cloud_in_tracking_2d =
adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.returns);
if (options_.use_online_correlative_scan_matching()) {
real_time_correlative_scan_matcher_.Match(
pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
Expand Down
2 changes: 1 addition & 1 deletion cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ CeresScanMatcher::~CeresScanMatcher() {}

void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* const pose_estimate,
kalman_filter::Pose2DCovariance* const covariance,
Expand Down
2 changes: 1 addition & 1 deletion cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class CeresScanMatcher {
// the solver 'summary'.
void Match(const transform::Rigid2d& previous_pose,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate,
kalman_filter::Pose2DCovariance* covariance,
Expand Down
4 changes: 2 additions & 2 deletions cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ class CeresScanMatcherTest : public ::testing::Test {
probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5),
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(
return {
Expand Down Expand Up @@ -74,7 +74,7 @@ class CeresScanMatcherTest : public ::testing::Test {
}

ProbabilityGrid probability_grid_;
sensor::PointCloud2D point_cloud_;
sensor::PointCloud point_cloud_;
std::unique_ptr<CeresScanMatcher> ceres_scan_matcher_;
};

Expand Down
26 changes: 14 additions & 12 deletions cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,14 +26,14 @@ namespace scan_matching {

SearchParameters::SearchParameters(const double linear_search_window,
const double angular_search_window,
const sensor::PointCloud2D& point_cloud,
const sensor::PointCloud& point_cloud,
const double resolution)
: resolution(resolution) {
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution;
for (const Eigen::Vector2f& point : point_cloud) {
const float range = point.norm();
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.head<2>().norm();
max_scan_range = std::max(range, max_scan_range);
}
const double kSafetyMargin = 1. - 1e-3;
Expand Down Expand Up @@ -91,33 +91,35 @@ void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan>& scans,
}
}

std::vector<sensor::PointCloud2D> GenerateRotatedScans(
const sensor::PointCloud2D& point_cloud,
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters) {
std::vector<sensor::PointCloud2D> rotated_scans;
std::vector<sensor::PointCloud> rotated_scans;
rotated_scans.reserve(search_parameters.num_scans);

double delta_theta = -search_parameters.num_angular_perturbations *
search_parameters.angular_perturbation_step_size;
for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index,
delta_theta += search_parameters.angular_perturbation_step_size) {
rotated_scans.push_back(sensor::TransformPointCloud2D(
point_cloud, transform::Rigid2f::Rotation(delta_theta)));
rotated_scans.push_back(sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));
}
return rotated_scans;
}

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) {
std::vector<DiscreteScan> discrete_scans;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud2D& scan : scans) {
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const Eigen::Vector2f& point : scan) {
const Eigen::Vector2f translated_point = initial_translation * point;
for (const Eigen::Vector3f& point : scan) {
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.head<2>();
discrete_scans.back().push_back(
map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(),
translated_point.y()));
Expand Down
8 changes: 4 additions & 4 deletions cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ struct SearchParameters {
};

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.
SearchParameters(int num_linear_perturbations, int num_angular_perturbations,
Expand All @@ -60,14 +60,14 @@ struct SearchParameters {
};

// Generates a collection of rotated scans.
std::vector<sensor::PointCloud2D> GenerateRotatedScans(
const sensor::PointCloud2D& point_cloud,
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters);

// Translates and discretizes the rotated scans into a vector of integer
// indices.
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);

// A possible solution.
Expand Down
24 changes: 12 additions & 12 deletions cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,9 @@ TEST(Candidate, Construction) {
}

TEST(GenerateRotatedScans, GenerateRotatedScans) {
sensor::PointCloud2D point_cloud;
point_cloud.emplace_back(-1., 1.);
const std::vector<sensor::PointCloud2D> scans =
sensor::PointCloud point_cloud;
point_cloud.emplace_back(-1.f, 1.f, 0.f);
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.));
EXPECT_EQ(3, scans.size());
EXPECT_NEAR(1., scans[0][0].x(), 1e-6);
Expand All @@ -71,17 +71,17 @@ TEST(GenerateRotatedScans, GenerateRotatedScans) {
}

TEST(DiscretizeScans, DiscretizeScans) {
sensor::PointCloud2D point_cloud;
point_cloud.emplace_back(0.025, 0.175);
point_cloud.emplace_back(-0.025, 0.175);
point_cloud.emplace_back(-0.075, 0.175);
point_cloud.emplace_back(-0.125, 0.175);
point_cloud.emplace_back(-0.125, 0.125);
point_cloud.emplace_back(-0.125, 0.075);
point_cloud.emplace_back(-0.125, 0.025);
sensor::PointCloud point_cloud;
point_cloud.emplace_back(0.025f, 0.175f, 0.f);
point_cloud.emplace_back(-0.025f, 0.175f, 0.f);
point_cloud.emplace_back(-0.075f, 0.175f, 0.f);
point_cloud.emplace_back(-0.125f, 0.175f, 0.f);
point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
CellLimits(6, 6));
const std::vector<sensor::PointCloud2D> scans =
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans =
DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity());
Expand Down
18 changes: 9 additions & 9 deletions cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,8 +208,8 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}

bool FastCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud, const float min_score,
float* score, transform::Rigid2d* pose_estimate) const {
const sensor::PointCloud& point_cloud, const float min_score, float* score,
transform::Rigid2d* pose_estimate) const {
const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(),
point_cloud, limits_.resolution());
Expand All @@ -219,7 +219,7 @@ bool FastCorrelativeScanMatcher::Match(
}

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 {
// Compute a search window around the center of the submap that includes it
// fully.
Expand All @@ -239,17 +239,17 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
SearchParameters search_parameters,
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 {
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);

const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud =
sensor::TransformPointCloud2D(
point_cloud,
transform::Rigid2d::Rotation(initial_rotation).cast<float>());
const std::vector<sensor::PointCloud2D> rotated_scans =
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Expand Down
6 changes: 3 additions & 3 deletions cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,14 +111,14 @@ class FastCorrelativeScanMatcher {
// is possible, true is returned, and 'score' and 'pose_estimate' are updated
// with the result.
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;

// Aligns 'point_cloud' within the full 'probability_grid', i.e., not
// restricted to the configured search window. If a score above 'min_score'
// (excluding equality) is possible, true is returned, and 'score' and
// '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;

private:
Expand All @@ -128,7 +128,7 @@ class FastCorrelativeScanMatcher {
bool MatchWithSearchParameters(
SearchParameters search_parameters,
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;
std::vector<Candidate> ComputeLowestResolutionCandidates(
const std::vector<DiscreteScan>& discrete_scans,
Expand Down
51 changes: 29 additions & 22 deletions cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,13 +133,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);

sensor::PointCloud2D point_cloud;
point_cloud.emplace_back(-2.5f, 0.5f);
point_cloud.emplace_back(-2.f, 0.5f);
point_cloud.emplace_back(0.f, -0.5f);
point_cloud.emplace_back(0.5f, -1.6f);
point_cloud.emplace_back(2.5f, 0.5f);
point_cloud.emplace_back(2.5f, 1.7f);
sensor::PointCloud point_cloud;
point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
point_cloud.emplace_back(-2.f, 0.5f, 0.f);
point_cloud.emplace_back(0.f, -0.5f, 0.f);
point_cloud.emplace_back(0.5f, -1.6f, 0.f);
point_cloud.emplace_back(2.5f, 0.5f, 0.f);
point_cloud.emplace_back(2.5f, 1.7f, 0.f);

for (int i = 0; i != 50; ++i) {
const transform::Rigid2f expected_pose(
Expand All @@ -149,11 +149,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(),
sensor::TransformPointCloud2D(
point_cloud, expected_pose),
{}},
&probability_grid);
laser_fan_inserter.Insert(
sensor::LaserFan{
expected_pose.translation(),
sensor::TransformPointCloud2D(
sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
{}},
&probability_grid);

FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
options);
Expand All @@ -177,20 +179,24 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
constexpr float kMinScore = 0.1f;
const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);

sensor::PointCloud2D unperturbed_point_cloud;
unperturbed_point_cloud.emplace_back(-2.5, 0.5);
unperturbed_point_cloud.emplace_back(-2.25, 0.5);
unperturbed_point_cloud.emplace_back(0., 0.5);
unperturbed_point_cloud.emplace_back(0.25, 1.6);
unperturbed_point_cloud.emplace_back(2.5, 0.5);
unperturbed_point_cloud.emplace_back(2.0, 1.8);
sensor::PointCloud unperturbed_point_cloud;
unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f);
unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(2.0f, 1.8f, 0.f);

for (int i = 0; i != 20; ++i) {
const transform::Rigid2f perturbation(
{10. * distribution(prng), 10. * distribution(prng)},
1.6 * distribution(prng));
const sensor::PointCloud2D point_cloud =
sensor::TransformPointCloud2D(unperturbed_point_cloud, perturbation);
const sensor::PointCloud point_cloud = sensor::TransformPointCloud(
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 =
transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)},
0.5 * distribution(prng)) *
Expand All @@ -202,7 +208,8 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
laser_fan_inserter.Insert(
sensor::LaserFan{
(expected_pose * perturbation).translation(),
sensor::TransformPointCloud2D(point_cloud, expected_pose),
sensor::TransformPointCloud2D(
sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
{}},
&probability_grid);

Expand Down
4 changes: 2 additions & 2 deletions cartographer/mapping_2d/scan_matching/fast_global_localizer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,14 @@ bool PerformGlobalLocalization(
const std::vector<
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
matchers,
const cartographer::sensor::PointCloud2D& point_cloud,
const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* const best_pose_estimate, float* const best_score) {
CHECK(best_pose_estimate != nullptr)
<< "Need a non-null output_pose_estimate!";
CHECK(best_score != nullptr) << "Need a non-null best_score!";
*best_score = cutoff;
transform::Rigid2d pose_estimate;
const sensor::PointCloud2D filtered_point_cloud =
const sensor::PointCloud filtered_point_cloud =
voxel_filter.Filter(point_cloud);
bool success = false;
if (matchers.size() == 0) {
Expand Down
2 changes: 1 addition & 1 deletion cartographer/mapping_2d/scan_matching/fast_global_localizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ bool PerformGlobalLocalization(
const std::vector<
cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>&
matchers,
const cartographer::sensor::PointCloud2D& point_cloud,
const cartographer::sensor::PointCloud& point_cloud,
transform::Rigid2d* best_pose_estimate, float* best_score);

} // namespace scan_matching
Expand Down
4 changes: 2 additions & 2 deletions cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ class OccupiedSpaceCostFunctor {
// Creates an OccupiedSpaceCostFunctor using the specified map, resolution
// level, and point cloud.
OccupiedSpaceCostFunctor(const double scaling_factor,
const sensor::PointCloud2D& point_cloud,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
Expand Down Expand Up @@ -107,7 +107,7 @@ class OccupiedSpaceCostFunctor {
};

const double scaling_factor_;
const sensor::PointCloud2D& point_cloud_;
const sensor::PointCloud& point_cloud_;
const ProbabilityGrid& probability_grid_;
};

Expand Down

0 comments on commit 5aad2d6

Please sign in to comment.