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;
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,

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,
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,

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

@ -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,

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),
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 {
@ -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_;
};

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,
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;
@ -91,10 +91,10 @@ 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 *
@ -102,22 +102,24 @@ std::vector<sensor::PointCloud2D> GenerateRotatedScans(
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()));

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,
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,
@ -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.

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

@ -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);
@ -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());

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

@ -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());
@ -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.
@ -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(
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid2d::Rotation(initial_rotation).cast<float>());
const std::vector<sensor::PointCloud2D> rotated_scans =
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,

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
// 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:
@ -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,

45
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;
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(
@ -149,9 +149,11 @@ 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(),
laser_fan_inserter.Insert(
sensor::LaserFan{
expected_pose.translation(),
sensor::TransformPointCloud2D(
point_cloud, expected_pose),
sensor::ProjectToPointCloud2D(point_cloud), expected_pose),
{}},
&probability_grid);
@ -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)) *
@ -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);

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

@ -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) {

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

@ -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

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
// 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),
@ -107,7 +107,7 @@ class OccupiedSpaceCostFunctor {
};
const double scaling_factor_;
const sensor::PointCloud2D& point_cloud_;
const sensor::PointCloud& point_cloud_;
const ProbabilityGrid& probability_grid_;
};

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

@ -90,21 +90,21 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
double RealTimeCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud =
sensor::TransformPointCloud2D(
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid2d::Rotation(initial_rotation).cast<float>());
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
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);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
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
// returns the score.
double Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid,
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>(
CreateLaserFanInserterOptions(parameter_dictionary.get()));
}
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);
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);
probability_grid_.StartUpdate();
laser_fan_inserter_->Insert(
sensor::LaserFan{Eigen::Vector2f::Zero(), point_cloud_, {}},
sensor::LaserFan{Eigen::Vector2f::Zero(),
sensor::ProjectToPointCloud2D(point_cloud_),
{}},
&probability_grid_);
{
auto parameter_dictionary = common::MakeDictionary(
@ -77,14 +79,14 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
ProbabilityGrid probability_grid_;
std::unique_ptr<LaserFanInserter> laser_fan_inserter_;
sensor::PointCloud2D point_cloud_;
sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher>
real_time_correlative_scan_matcher_;
};
TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePerfectHighResolutionCandidate) {
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(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
@ -102,7 +104,7 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidate) {
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(
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;
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_index);
const sensor::PointCloud2D filtered_point_cloud =
adaptive_voxel_filter_.Filter(*point_cloud);
const sensor::PointCloud filtered_point_cloud =
sensor::ToPointCloud(adaptive_voxel_filter_.Filter(*point_cloud));
// The 'constraint_transform' (i <- j) is computed from:
// - a 'filtered_point_cloud' in j,