Kevin Daun 2018-08-16 14:04:09 +02:00 committed by Wally B. Feed
parent 5261c90c34
commit 1c00e8a970
5 changed files with 197 additions and 85 deletions

View File

@ -75,12 +75,9 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
transform::Rigid2d initial_ceres_pose = pose_prediction; transform::Rigid2d initial_ceres_pose = pose_prediction;
if (options_.use_online_correlative_scan_matching()) { if (options_.use_online_correlative_scan_matching()) {
CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
proto::GridOptions2D_GridType_PROBABILITY_GRID);
const double score = real_time_correlative_scan_matcher_.Match( const double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud, pose_prediction, filtered_gravity_aligned_point_cloud,
*static_cast<const ProbabilityGrid*>(matching_submap->grid()), *matching_submap->grid(), &initial_ceres_pose);
&initial_ceres_pose);
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
} }

View File

@ -71,22 +71,23 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
initial_pose_estimate.rotation().angle()}; initial_pose_estimate.rotation().angle()};
ceres::Problem problem; ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.); CHECK_GT(options_.occupied_space_weight(), 0.);
if (grid.GetGridType() == GridType::PROBABILITY_GRID) { switch (grid.GetGridType()) {
case GridType::PROBABILITY_GRID:
problem.AddResidualBlock( problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D( CreateOccupiedSpaceCostFunction2D(
options_.occupied_space_weight() / options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid), point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate); nullptr /* loss function */, ceres_pose_estimate);
} else if (grid.GetGridType() == GridType::TSDF) { break;
case GridType::TSDF:
problem.AddResidualBlock( problem.AddResidualBlock(
CreateTSDFMatchCostFunction2D( CreateTSDFMatchCostFunction2D(
options_.occupied_space_weight() / options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())), std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, static_cast<const TSDF2D&>(grid)), point_cloud, static_cast<const TSDF2D&>(grid)),
nullptr /* loss function */, ceres_pose_estimate); nullptr /* loss function */, ceres_pose_estimate);
} else { break;
LOG(FATAL) << "Unknown GridType.";
} }
CHECK_GT(options_.translation_weight(), 0.); CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(

View File

@ -25,6 +25,7 @@
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/tsdf_2d.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -32,6 +33,48 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace scan_matching { namespace scan_matching {
namespace {
float ComputeCandidateScore(const TSDF2D& tsdf,
const DiscreteScan2D& discrete_scan,
int x_index_offset, int y_index_offset) {
float candidate_score = 0.f;
float summed_weight = 0.f;
for (const Eigen::Array2i& xy_index : discrete_scan) {
const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
xy_index.y() + y_index_offset);
const std::pair<float, float> tsd_and_weight =
tsdf.GetTSDAndWeight(proposed_xy_index);
const float normalized_tsd_score =
(tsdf.GetMaxCorrespondenceCost() - std::abs(tsd_and_weight.first)) /
tsdf.GetMaxCorrespondenceCost();
const float weight = tsd_and_weight.second;
candidate_score += normalized_tsd_score * weight;
summed_weight += weight;
}
if (summed_weight == 0.f) return 0.f;
candidate_score /= summed_weight;
CHECK_GE(candidate_score, 0.f);
return candidate_score;
}
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
const DiscreteScan2D& discrete_scan,
int x_index_offset, int y_index_offset) {
float candidate_score = 0.f;
for (const Eigen::Array2i& xy_index : discrete_scan) {
const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
xy_index.y() + y_index_offset);
const float probability =
probability_grid.GetProbability(proposed_xy_index);
candidate_score += probability;
}
candidate_score /= static_cast<float>(discrete_scan.size());
CHECK_GT(candidate_score, 0.f);
return candidate_score;
}
} // namespace
RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D( RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D(
const proto::RealTimeCorrelativeScanMatcherOptions& options) const proto::RealTimeCorrelativeScanMatcherOptions& options)
@ -73,8 +116,7 @@ RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
double RealTimeCorrelativeScanMatcher2D::Match( double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate, const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud, const Grid2D& grid,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const { transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate); CHECK_NOTNULL(pose_estimate);
@ -85,18 +127,17 @@ double RealTimeCorrelativeScanMatcher2D::Match(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ()))); 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, grid.limits().resolution());
const std::vector<sensor::PointCloud> rotated_scans = const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters); GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid.limits(), rotated_scans, grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(), Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y())); initial_pose_estimate.translation().y()));
std::vector<Candidate2D> candidates = std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters); GenerateExhaustiveSearchCandidates(search_parameters);
ScoreCandidates(probability_grid, discrete_scans, search_parameters, ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
&candidates);
const Candidate2D& best_candidate = const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end()); *std::max_element(candidates.begin(), candidates.end());
@ -108,29 +149,29 @@ double RealTimeCorrelativeScanMatcher2D::Match(
} }
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates( void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
const ProbabilityGrid& probability_grid, const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters, const SearchParameters& search_parameters,
std::vector<Candidate2D>* const candidates) const { std::vector<Candidate2D>* const candidates) const {
for (Candidate2D& candidate : *candidates) { for (Candidate2D& candidate : *candidates) {
candidate.score = 0.f; switch (grid.GetGridType()) {
for (const Eigen::Array2i& xy_index : case GridType::PROBABILITY_GRID:
discrete_scans[candidate.scan_index]) { candidate.score = ComputeCandidateScore(
const Eigen::Array2i proposed_xy_index( static_cast<const ProbabilityGrid&>(grid),
xy_index.x() + candidate.x_index_offset, discrete_scans[candidate.scan_index], candidate.x_index_offset,
xy_index.y() + candidate.y_index_offset); candidate.y_index_offset);
const float probability = break;
probability_grid.GetProbability(proposed_xy_index); case GridType::TSDF:
candidate.score += probability; candidate.score = ComputeCandidateScore(
static_cast<const TSDF2D&>(grid),
discrete_scans[candidate.scan_index], candidate.x_index_offset,
candidate.y_index_offset);
break;
} }
candidate.score /=
static_cast<float>(discrete_scans[candidate.scan_index].size());
candidate.score *= candidate.score *=
std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) * std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
options_.translation_delta_cost_weight() + options_.translation_delta_cost_weight() +
std::abs(candidate.orientation) * std::abs(candidate.orientation) *
options_.rotation_delta_cost_weight())); options_.rotation_delta_cost_weight()));
CHECK_GT(candidate.score, 0.f);
} }
} }

View File

@ -41,7 +41,7 @@
#include <vector> #include <vector>
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h"
#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h"
@ -60,20 +60,19 @@ class RealTimeCorrelativeScanMatcher2D {
RealTimeCorrelativeScanMatcher2D& operator=( RealTimeCorrelativeScanMatcher2D& operator=(
const RealTimeCorrelativeScanMatcher2D&) = delete; const RealTimeCorrelativeScanMatcher2D&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an // Aligns 'point_cloud' within the 'grid' given an
// '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::PointCloud& point_cloud, const sensor::PointCloud& point_cloud, const Grid2D& grid,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const; transform::Rigid2d* pose_estimate) const;
// Computes the score for each Candidate2D in a collection. The cost is // Computes the score for each Candidate2D in a collection. The cost is
// computed as the sum of probabilities, different from the Ceres // computed as the sum of probabilities or normalized TSD values, different
// CostFunctions: http://ceres-solver.org/modeling.html // from the Ceres CostFunctions: http://ceres-solver.org/modeling.html
// //
// Visible for testing. // Visible for testing.
void ScoreCandidates(const ProbabilityGrid& probability_grid, void ScoreCandidates(const Grid2D& grid,
const std::vector<DiscreteScan2D>& discrete_scans, const std::vector<DiscreteScan2D>& discrete_scans,
const SearchParameters& search_parameters, const SearchParameters& search_parameters,
std::vector<Candidate2D>* candidates) const; std::vector<Candidate2D>* candidates) const;

View File

@ -24,6 +24,8 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
#include "cartographer/mapping/2d/tsdf_2d.h"
#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h"
#include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -34,12 +36,66 @@ namespace mapping {
namespace scan_matching { namespace scan_matching {
namespace { namespace {
proto::RealTimeCorrelativeScanMatcherOptions
CreateRealTimeCorrelativeScanMatcherTestOptions2D() {
auto parameter_dictionary = common::MakeDictionary(
"return {"
"linear_search_window = 0.6, "
"angular_search_window = 0.16, "
"translation_delta_cost_weight = 0., "
"rotation_delta_cost_weight = 0., "
"}");
return CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary.get());
}
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected: protected:
RealTimeCorrelativeScanMatcherTest() RealTimeCorrelativeScanMatcherTest() {
: probability_grid_( point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
real_time_correlative_scan_matcher_ =
absl::make_unique<RealTimeCorrelativeScanMatcher2D>(
CreateRealTimeCorrelativeScanMatcherTestOptions2D());
}
void SetUpTSDF() {
grid_ = absl::make_unique<TSDF2D>(
MapLimits(0.05, Eigen::Vector2d(0.3, 0.5), CellLimits(20, 20)), 0.3,
1.0, &conversion_tables_);
{
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
truncation_distance = 0.3,
maximum_weight = 10.,
update_free_space = false,
normal_estimation_options = {
num_normal_samples = 4,
sample_radius = 0.5,
},
project_sdf_distance_to_scan_normal = true,
update_weight_range_exponent = 0,
update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0.5,
update_weight_distance_cell_to_hit_kernel_bandwith = 0.5,
})text");
range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(
CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()));
}
range_data_inserter_->Insert(
sensor::RangeData{Eigen::Vector3f(0.5f, -0.5f, 0.f), point_cloud_, {}},
grid_.get());
grid_->FinishUpdate();
}
void SetUpProbabilityGrid() {
grid_ = absl::make_unique<ProbabilityGrid>(
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)), MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)),
&conversion_tables_) { &conversion_tables_);
{ {
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return { " "return { "
@ -52,51 +108,31 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
CreateProbabilityGridRangeDataInserterOptions2D( CreateProbabilityGridRangeDataInserterOptions2D(
parameter_dictionary.get())); parameter_dictionary.get()));
} }
point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
range_data_inserter_->Insert( range_data_inserter_->Insert(
sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_); grid_.get());
probability_grid_.FinishUpdate(); grid_->FinishUpdate();
{
auto parameter_dictionary = common::MakeDictionary(
"return {"
"linear_search_window = 0.6, "
"angular_search_window = 0.16, "
"translation_delta_cost_weight = 0., "
"rotation_delta_cost_weight = 0., "
"}");
real_time_correlative_scan_matcher_ =
absl::make_unique<RealTimeCorrelativeScanMatcher2D>(
CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary.get()));
}
} }
ValueConversionTables conversion_tables_; ValueConversionTables conversion_tables_;
ProbabilityGrid probability_grid_; std::unique_ptr<Grid2D> grid_;
std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_; std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher2D> std::unique_ptr<RealTimeCorrelativeScanMatcher2D>
real_time_correlative_scan_matcher_; real_time_correlative_scan_matcher_;
}; };
TEST_F(RealTimeCorrelativeScanMatcherTest, TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePerfectHighResolutionCandidate) { ScorePerfectHighResolutionCandidateProbabilityGrid) {
SetUpProbabilityGrid();
const std::vector<sensor::PointCloud> 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<DiscreteScan2D> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan2D> discrete_scans =
probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
std::vector<Candidate2D> candidates; std::vector<Candidate2D> candidates;
candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates( real_time_correlative_scan_matcher_->ScoreCandidates(
probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
&candidates);
EXPECT_EQ(0, candidates[0].scan_index); EXPECT_EQ(0, candidates[0].scan_index);
EXPECT_EQ(0, candidates[0].x_index_offset); EXPECT_EQ(0, candidates[0].x_index_offset);
EXPECT_EQ(0, candidates[0].y_index_offset); EXPECT_EQ(0, candidates[0].y_index_offset);
@ -105,16 +141,35 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
} }
TEST_F(RealTimeCorrelativeScanMatcherTest, TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidate) { ScorePerfectHighResolutionCandidateTSDF) {
SetUpTSDF();
const std::vector<sensor::PointCloud> 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<DiscreteScan2D> discrete_scans = DiscretizeScans( const std::vector<DiscreteScan2D> discrete_scans =
probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
std::vector<Candidate2D> candidates;
candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates(
*grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
EXPECT_EQ(0, candidates[0].scan_index);
EXPECT_EQ(0, candidates[0].x_index_offset);
EXPECT_EQ(0, candidates[0].y_index_offset);
// Every point should align perfectly.
EXPECT_NEAR(1.0, candidates[0].score, 1e-1);
EXPECT_LT(0.95, candidates[0].score);
}
TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidateProbabilityGrid) {
SetUpProbabilityGrid();
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan2D> discrete_scans =
DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
std::vector<Candidate2D> candidates; std::vector<Candidate2D> candidates;
candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates( real_time_correlative_scan_matcher_->ScoreCandidates(
probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
&candidates);
EXPECT_EQ(0, candidates[0].scan_index); EXPECT_EQ(0, candidates[0].scan_index);
EXPECT_EQ(0, candidates[0].x_index_offset); EXPECT_EQ(0, candidates[0].x_index_offset);
EXPECT_EQ(1, candidates[0].y_index_offset); EXPECT_EQ(1, candidates[0].y_index_offset);
@ -123,6 +178,25 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
EXPECT_GT(0.7, candidates[0].score); EXPECT_GT(0.7, candidates[0].score);
} }
TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidateTSDF) {
SetUpTSDF();
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan2D> discrete_scans =
DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
std::vector<Candidate2D> candidates;
candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates(
*grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
EXPECT_EQ(0, candidates[0].scan_index);
EXPECT_EQ(0, candidates[0].x_index_offset);
EXPECT_EQ(1, candidates[0].y_index_offset);
// 3 points should align perfectly.
EXPECT_LT(1.0 - 4. / (7. * 6.), candidates[0].score);
EXPECT_GT(1.0, candidates[0].score);
}
} // namespace } // namespace
} // namespace scan_matching } // namespace scan_matching
} // namespace mapping } // namespace mapping