25 #include "Eigen/Geometry" 30 #include "glog/logging.h" 33 namespace mapping_2d {
34 namespace scan_matching {
41 class SlidingWindowMaximum {
43 void AddValue(
const float value) {
51 void RemoveValue(
const float value) {
61 float GetMaximum()
const {
78 proto::FastCorrelativeScanMatcherOptions
81 proto::FastCorrelativeScanMatcherOptions options;
82 options.set_linear_search_window(
83 parameter_dictionary->
GetDouble(
"linear_search_window"));
84 options.set_angular_search_window(
85 parameter_dictionary->
GetDouble(
"angular_search_window"));
86 options.set_branch_and_bound_depth(
87 parameter_dictionary->
GetInt(
"branch_and_bound_depth"));
93 const int width, std::vector<float>* reusable_intermediate_grid)
94 : offset_(-width + 1, -width + 1),
95 wide_limits_(limits.num_x_cells + width - 1,
96 limits.num_y_cells + width - 1),
97 cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
104 std::vector<float>& intermediate = *reusable_intermediate_grid;
107 SlidingWindowMaximum current_values;
108 current_values.AddValue(
110 for (
int x = -width + 1; x != 0; ++x) {
111 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
113 current_values.AddValue(
117 for (
int x = 0; x < limits.
num_x_cells - width; ++x) {
118 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
119 current_values.RemoveValue(
121 current_values.AddValue(
124 for (
int x = std::max(limits.
num_x_cells - width, 0);
126 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
127 current_values.RemoveValue(
130 current_values.CheckIsEmpty();
136 SlidingWindowMaximum current_values;
137 current_values.AddValue(intermediate[x]);
138 for (
int y = -width + 1; y != 0; ++y) {
139 cells_[x + (y + width - 1) * stride] =
142 current_values.AddValue(intermediate[x + (y + width) * stride]);
145 for (
int y = 0; y < limits.
num_y_cells - width; ++y) {
146 cells_[x + (y + width - 1) * stride] =
148 current_values.RemoveValue(intermediate[x + y * stride]);
149 current_values.AddValue(intermediate[x + (y + width) * stride]);
151 for (
int y = std::max(limits.
num_y_cells - width, 0);
153 cells_[x + (y + width - 1) * stride] =
155 current_values.RemoveValue(intermediate[x + y * stride]);
157 current_values.CheckIsEmpty();
165 CHECK_GE(cell_value, 0);
166 CHECK_LE(cell_value, 255);
174 const proto::FastCorrelativeScanMatcherOptions& options) {
175 CHECK_GE(options.branch_and_bound_depth(), 1);
176 const int max_width = 1 << (options.branch_and_bound_depth() - 1);
177 precomputation_grids_.reserve(options.branch_and_bound_depth());
178 std::vector<float> reusable_intermediate_grid;
180 reusable_intermediate_grid.reserve((limits.
num_x_cells + max_width - 1) *
182 for (
int i = 0; i != options.branch_and_bound_depth(); ++i) {
183 const int width = 1 << i;
184 precomputation_grids_.emplace_back(probability_grid, limits, width,
185 &reusable_intermediate_grid);
190 return precomputation_grids_[index];
193 int max_depth()
const {
return precomputation_grids_.size() - 1; }
201 const proto::FastCorrelativeScanMatcherOptions& options)
203 limits_(probability_grid.limits()),
204 precomputation_grid_stack_(
217 point_cloud, min_score, score,
235 min_score, score, pose_estimate);
243 CHECK_NOTNULL(score);
244 CHECK_NOTNULL(pose_estimate);
246 const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.
rotation();
250 initial_rotation.cast<
float>().angle(), Eigen::Vector3f::UnitZ())));
251 const std::vector<sensor::PointCloud> rotated_scans =
255 Eigen::Translation2f(initial_pose_estimate.
translation().x(),
259 const std::vector<Candidate> lowest_resolution_candidates =
262 discrete_scans, search_parameters, lowest_resolution_candidates,
264 if (best_candidate.
score > min_score) {
265 *score = best_candidate.
score;
267 {initial_pose_estimate.
translation().x() + best_candidate.
x,
268 initial_pose_estimate.
translation().y() + best_candidate.
y},
269 initial_rotation * Eigen::Rotation2Dd(best_candidate.
orientation));
275 std::vector<Candidate>
277 const std::vector<DiscreteScan>& discrete_scans,
279 std::vector<Candidate> lowest_resolution_candidates =
283 discrete_scans, search_parameters, &lowest_resolution_candidates);
284 return lowest_resolution_candidates;
287 std::vector<Candidate>
291 int num_candidates = 0;
292 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
294 const int num_lowest_resolution_linear_x_candidates =
296 search_parameters.
linear_bounds[scan_index].min_x + linear_step_size) /
298 const int num_lowest_resolution_linear_y_candidates =
300 search_parameters.
linear_bounds[scan_index].min_y + linear_step_size) /
302 num_candidates += num_lowest_resolution_linear_x_candidates *
303 num_lowest_resolution_linear_y_candidates;
305 std::vector<Candidate> candidates;
306 candidates.reserve(num_candidates);
307 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
309 for (
int x_index_offset = search_parameters.
linear_bounds[scan_index].min_x;
310 x_index_offset <= search_parameters.
linear_bounds[scan_index].max_x;
311 x_index_offset += linear_step_size) {
312 for (
int y_index_offset =
314 y_index_offset <= search_parameters.
linear_bounds[scan_index].max_y;
315 y_index_offset += linear_step_size) {
316 candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
321 CHECK_EQ(candidates.size(), num_candidates);
327 const std::vector<DiscreteScan>& discrete_scans,
329 std::vector<Candidate>*
const candidates)
const {
330 for (
Candidate& candidate : *candidates) {
332 for (
const Eigen::Array2i& xy_index :
333 discrete_scans[candidate.scan_index]) {
334 const Eigen::Array2i proposed_xy_index(
335 xy_index.x() + candidate.x_index_offset,
336 xy_index.y() + candidate.y_index_offset);
337 sum += precomputation_grid.
GetValue(proposed_xy_index);
340 sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
342 std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
346 const std::vector<DiscreteScan>& discrete_scans,
348 const std::vector<Candidate>& candidates,
const int candidate_depth,
349 float min_score)
const {
350 if (candidate_depth == 0) {
352 return *candidates.begin();
355 Candidate best_high_resolution_candidate(0, 0, 0, search_parameters);
356 best_high_resolution_candidate.
score = min_score;
357 for (
const Candidate& candidate : candidates) {
358 if (candidate.score <= min_score) {
361 std::vector<Candidate> higher_resolution_candidates;
362 const int half_width = 1 << (candidate_depth - 1);
363 for (
int x_offset : {0, half_width}) {
364 if (candidate.x_index_offset + x_offset >
365 search_parameters.
linear_bounds[candidate.scan_index].max_x) {
368 for (
int y_offset : {0, half_width}) {
369 if (candidate.y_index_offset + y_offset >
370 search_parameters.
linear_bounds[candidate.scan_index].max_y) {
373 higher_resolution_candidates.emplace_back(
374 candidate.scan_index, candidate.x_index_offset + x_offset,
375 candidate.y_index_offset + y_offset, search_parameters);
379 discrete_scans, search_parameters,
380 &higher_resolution_candidates);
381 best_high_resolution_candidate = std::max(
382 best_high_resolution_candidate,
384 higher_resolution_candidates, candidate_depth - 1,
385 best_high_resolution_candidate.score));
387 return best_high_resolution_candidate;
const PrecomputationGrid & Get(int index)
std::vector< DiscreteScan > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
void ScoreCandidates(const PrecomputationGrid &precomputation_grid, const std::vector< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters, std::vector< Candidate > *const candidates) const
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
FastCorrelativeScanMatcher(const ProbabilityGrid &probability_grid, const proto::FastCorrelativeScanMatcherOptions &options)
std::vector< Candidate > ComputeLowestResolutionCandidates(const std::vector< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters) const
int GetInt(const string &key)
const Eigen::Vector2d & max() const
int RoundToInt(const float x)
std::vector< PrecomputationGrid > precomputation_grids_
constexpr float kMinProbability
std::vector< LinearBounds > linear_bounds
int GetValue(const Eigen::Array2i &xy_index) const
~FastCorrelativeScanMatcher()
double GetDouble(const string &key)
uint8 ComputeCellValue(float probability) const
double resolution() const
constexpr float kMaxProbability
float GetProbability(const Eigen::Array2i &xy_index) const
const CellLimits & cell_limits() const
std::unique_ptr< PrecomputationGridStack > precomputation_grid_stack_
Candidate BranchAndBound(const std::vector< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters, const std::vector< Candidate > &candidates, int candidate_depth, float min_score) const
std::deque< float > non_ascending_maxima_
PrecomputationGrid(const ProbabilityGrid &probability_grid, const CellLimits &limits, int width, std::vector< float > *reusable_intermediate_grid)
std::vector< Eigen::Vector3f > PointCloud
const MapLimits & limits() const
static float ToProbability(float value)
const proto::FastCorrelativeScanMatcherOptions options_
proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
PrecomputationGridStack(const ProbabilityGrid &probability_grid, const proto::FastCorrelativeScanMatcherOptions &options)
void ShrinkToFit(const std::vector< DiscreteScan > &scans, const CellLimits &cell_limits)
const CellLimits wide_limits_
std::vector< Candidate > GenerateLowestResolutionCandidates(const SearchParameters &search_parameters) const
bool Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, float min_score, float *score, transform::Rigid2d *pose_estimate) const
std::vector< uint8 > cells_
bool MatchWithSearchParameters(SearchParameters search_parameters, const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, float min_score, float *score, transform::Rigid2d *pose_estimate) const
bool MatchFullSubmap(const sensor::PointCloud &point_cloud, float min_score, float *score, transform::Rigid2d *pose_estimate) const
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)