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

Add TSDF support for RealTimeCorrelativeScanMatcher. (#1389) 

Continues https://github.com/googlecartographer/cartographer/pull/1376.
[RFC=0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)
master
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -75,12 +75,9 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
transform::Rigid2d initial_ceres_pose = pose_prediction;
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(
pose_prediction, filtered_gravity_aligned_point_cloud,
*static_cast<const ProbabilityGrid*>(matching_submap->grid()),
&initial_ceres_pose);
*matching_submap->grid(), &initial_ceres_pose);
kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
}

33
cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc
View File

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

81
cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc
View File

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

13
cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h
View File

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

150
cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc
View File

@ -24,6 +24,8 @@
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/mapping/2d/probability_grid.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/sensor/point_cloud.h"
#include "cartographer/transform/transform.h"
@ -34,12 +36,66 @@ namespace mapping {
namespace scan_matching {
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 {
protected:
RealTimeCorrelativeScanMatcherTest()
: probability_grid_(
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)),
&conversion_tables_) {
RealTimeCorrelativeScanMatcherTest() {
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)),
&conversion_tables_);
{
auto parameter_dictionary = common::MakeDictionary(
"return { "
@ -52,51 +108,31 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
CreateProbabilityGridRangeDataInserterOptions2D(
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(
sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_);
probability_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()));
}
grid_.get());
grid_->FinishUpdate();
}
ValueConversionTables conversion_tables_;
ProbabilityGrid probability_grid_;
std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
std::unique_ptr<Grid2D> grid_;
std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
sensor::PointCloud point_cloud_;
std::unique_ptr<RealTimeCorrelativeScanMatcher2D>
real_time_correlative_scan_matcher_;
};
TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePerfectHighResolutionCandidate) {
ScorePerfectHighResolutionCandidateProbabilityGrid) {
SetUpProbabilityGrid();
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
const std::vector<DiscreteScan2D> discrete_scans =
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(
probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
&candidates);
*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);
@ -105,16 +141,35 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
}
TEST_F(RealTimeCorrelativeScanMatcherTest,
ScorePartiallyCorrectHighResolutionCandidate) {
ScorePerfectHighResolutionCandidateTSDF) {
SetUpTSDF();
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
const std::vector<DiscreteScan2D> discrete_scans =
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;
candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
real_time_correlative_scan_matcher_->ScoreCandidates(
probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
&candidates);
*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);
@ -123,6 +178,25 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
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 scan_matching
} // namespace mapping