25 #include "Eigen/Geometry" 31 #include "glog/logging.h" 35 namespace scan_matching {
41 class SlidingWindowMaximum {
43 void AddValue(
const float value) {
51 void RemoveValue(
const float value) {
61 float GetMaximum()
const {
78 proto::FastCorrelativeScanMatcherOptions2D
81 proto::FastCorrelativeScanMatcherOptions2D 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 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 min_score_(1.f - grid.GetMaxCorrespondenceCost()),
98 max_score_(1.f - grid.GetMinCorrespondenceCost()),
99 cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
106 std::vector<float>& intermediate = *reusable_intermediate_grid;
109 SlidingWindowMaximum current_values;
110 current_values.AddValue(
112 for (
int x = -width + 1; x != 0; ++x) {
113 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
116 Eigen::Array2i(x + width, y))));
119 for (
int x = 0; x < limits.
num_x_cells - width; ++x) {
120 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
121 current_values.RemoveValue(
124 Eigen::Array2i(x + width, y))));
126 for (
int x = std::max(limits.
num_x_cells - width, 0);
128 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
129 current_values.RemoveValue(
132 current_values.CheckIsEmpty();
138 SlidingWindowMaximum current_values;
139 current_values.AddValue(intermediate[x]);
140 for (
int y = -width + 1; y != 0; ++y) {
141 cells_[x + (y + width - 1) * stride] =
144 current_values.AddValue(intermediate[x + (y + width) * stride]);
147 for (
int y = 0; y < limits.
num_y_cells - width; ++y) {
148 cells_[x + (y + width - 1) * stride] =
150 current_values.RemoveValue(intermediate[x + y * stride]);
151 current_values.AddValue(intermediate[x + (y + width) * stride]);
153 for (
int y = std::max(limits.
num_y_cells - width, 0);
155 cells_[x + (y + width - 1) * stride] =
157 current_values.RemoveValue(intermediate[x + y * stride]);
159 current_values.CheckIsEmpty();
166 CHECK_GE(cell_value, 0);
167 CHECK_LE(cell_value, 255);
173 const proto::FastCorrelativeScanMatcherOptions2D& options) {
174 CHECK_GE(options.branch_and_bound_depth(), 1);
175 const int max_width = 1 << (options.branch_and_bound_depth() - 1);
176 precomputation_grids_.reserve(options.branch_and_bound_depth());
177 std::vector<float> reusable_intermediate_grid;
179 reusable_intermediate_grid.reserve((limits.
num_x_cells + max_width - 1) *
181 for (
int i = 0; i != options.branch_and_bound_depth(); ++i) {
182 const int width = 1 << i;
183 precomputation_grids_.emplace_back(grid, limits, width,
184 &reusable_intermediate_grid);
190 const proto::FastCorrelativeScanMatcherOptions2D& options)
192 limits_(grid.limits()),
193 precomputation_grid_stack_(
206 point_cloud, min_score, score,
224 min_score, score, pose_estimate);
232 CHECK_NOTNULL(score);
233 CHECK_NOTNULL(pose_estimate);
235 const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.
rotation();
239 initial_rotation.cast<
float>().angle(), Eigen::Vector3f::UnitZ())));
240 const std::vector<sensor::PointCloud> rotated_scans =
244 Eigen::Translation2f(initial_pose_estimate.
translation().x(),
248 const std::vector<Candidate2D> lowest_resolution_candidates =
251 discrete_scans, search_parameters, lowest_resolution_candidates,
253 if (best_candidate.
score > min_score) {
254 *score = best_candidate.
score;
256 {initial_pose_estimate.
translation().x() + best_candidate.
x,
257 initial_pose_estimate.
translation().y() + best_candidate.
y},
258 initial_rotation * Eigen::Rotation2Dd(best_candidate.
orientation));
264 std::vector<Candidate2D>
266 const std::vector<DiscreteScan2D>& discrete_scans,
268 std::vector<Candidate2D> lowest_resolution_candidates =
272 discrete_scans, search_parameters, &lowest_resolution_candidates);
273 return lowest_resolution_candidates;
276 std::vector<Candidate2D>
280 int num_candidates = 0;
281 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
283 const int num_lowest_resolution_linear_x_candidates =
285 search_parameters.
linear_bounds[scan_index].min_x + linear_step_size) /
287 const int num_lowest_resolution_linear_y_candidates =
289 search_parameters.
linear_bounds[scan_index].min_y + linear_step_size) /
291 num_candidates += num_lowest_resolution_linear_x_candidates *
292 num_lowest_resolution_linear_y_candidates;
294 std::vector<Candidate2D> candidates;
295 candidates.reserve(num_candidates);
296 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
298 for (
int x_index_offset = search_parameters.
linear_bounds[scan_index].min_x;
299 x_index_offset <= search_parameters.
linear_bounds[scan_index].max_x;
300 x_index_offset += linear_step_size) {
301 for (
int y_index_offset =
303 y_index_offset <= search_parameters.
linear_bounds[scan_index].max_y;
304 y_index_offset += linear_step_size) {
305 candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
310 CHECK_EQ(candidates.size(), num_candidates);
316 const std::vector<DiscreteScan2D>& discrete_scans,
318 std::vector<Candidate2D>*
const candidates)
const {
321 for (
const Eigen::Array2i& xy_index :
322 discrete_scans[candidate.scan_index]) {
323 const Eigen::Array2i proposed_xy_index(
324 xy_index.x() + candidate.x_index_offset,
325 xy_index.y() + candidate.y_index_offset);
326 sum += precomputation_grid.
GetValue(proposed_xy_index);
328 candidate.score = precomputation_grid.
ToScore(
329 sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
331 std::sort(candidates->begin(), candidates->end(),
332 std::greater<Candidate2D>());
336 const std::vector<DiscreteScan2D>& discrete_scans,
338 const std::vector<Candidate2D>& candidates,
const int candidate_depth,
339 float min_score)
const {
340 if (candidate_depth == 0) {
342 return *candidates.begin();
345 Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
346 best_high_resolution_candidate.
score = min_score;
348 if (candidate.score <= min_score) {
351 std::vector<Candidate2D> higher_resolution_candidates;
352 const int half_width = 1 << (candidate_depth - 1);
353 for (
int x_offset : {0, half_width}) {
354 if (candidate.x_index_offset + x_offset >
355 search_parameters.
linear_bounds[candidate.scan_index].max_x) {
358 for (
int y_offset : {0, half_width}) {
359 if (candidate.y_index_offset + y_offset >
360 search_parameters.
linear_bounds[candidate.scan_index].max_y) {
363 higher_resolution_candidates.emplace_back(
364 candidate.scan_index, candidate.x_index_offset + x_offset,
365 candidate.y_index_offset + y_offset, search_parameters);
369 discrete_scans, search_parameters,
370 &higher_resolution_candidates);
371 best_high_resolution_candidate = std::max(
372 best_high_resolution_candidate,
374 higher_resolution_candidates, candidate_depth - 1,
375 best_high_resolution_candidate.score));
377 return best_high_resolution_candidate;
std::vector< DiscreteScan2D > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
float ToScore(float value) const
const Eigen::Vector2d & max() const
std::vector< LinearBounds > linear_bounds
bool MatchFullSubmap(const sensor::PointCloud &point_cloud, float min_score, float *score, transform::Rigid2d *pose_estimate) const
proto::FastCorrelativeScanMatcherOptions2D CreateFastCorrelativeScanMatcherOptions2D(common::LuaParameterDictionary *const parameter_dictionary)
const CellLimits wide_limits_
_Unique_if< T >::_Single_object make_unique(Args &&... args)
std::deque< float > non_ascending_maxima_
void ShrinkToFit(const std::vector< DiscreteScan2D > &scans, const CellLimits &cell_limits)
double resolution() const
int RoundToInt(const float x)
double GetDouble(const std::string &key)
std::map< CellId, StoredType > cells_
void ScoreCandidates(const PrecomputationGrid2D &precomputation_grid, const std::vector< DiscreteScan2D > &discrete_scans, const SearchParameters &search_parameters, std::vector< Candidate2D > *const candidates) const
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 Match(const transform::Rigid2d &initial_pose_estimate, 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)
FastCorrelativeScanMatcher2D(const Grid2D &grid, const proto::FastCorrelativeScanMatcherOptions2D &options)
const proto::FastCorrelativeScanMatcherOptions2D options_
proto::ProbabilityGridRangeDataInserterOptions2D options_
std::vector< uint8 > cells_
int GetValue(const Eigen::Array2i &xy_index) const
std::vector< Candidate2D > GenerateLowestResolutionCandidates(const SearchParameters &search_parameters) const
std::vector< Eigen::Vector3f > PointCloud
PrecomputationGridStack2D(const Grid2D &grid, const proto::FastCorrelativeScanMatcherOptions2D &options)
PrecomputationGrid2D(const Grid2D &grid, const CellLimits &limits, int width, std::vector< float > *reusable_intermediate_grid)
float GetCorrespondenceCost(const Eigen::Array2i &cell_index) const
Candidate2D BranchAndBound(const std::vector< DiscreteScan2D > &discrete_scans, const SearchParameters &search_parameters, const std::vector< Candidate2D > &candidates, int candidate_depth, float min_score) const
const MapLimits & limits() const
~FastCorrelativeScanMatcher2D()
uint8 ComputeCellValue(float probability) const
std::unique_ptr< PrecomputationGridStack2D > precomputation_grid_stack_
const CellLimits & cell_limits() const
int GetInt(const std::string &key)
std::vector< Candidate2D > ComputeLowestResolutionCandidates(const std::vector< DiscreteScan2D > &discrete_scans, const SearchParameters &search_parameters) const