mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <algorithm>
20 #include <cmath>
21 #include <deque>
22 #include <functional>
23 #include <limits>
24 
25 #include "Eigen/Geometry"
30 #include "glog/logging.h"
31 
32 namespace cartographer {
33 namespace mapping_2d {
34 namespace scan_matching {
35 
36 namespace {
37 
38 // A collection of values which can be added and later removed, and the maximum
39 // of the current values in the collection can be retrieved.
40 // All of it in (amortized) O(1).
41 class SlidingWindowMaximum {
42  public:
43  void AddValue(const float value) {
44  while (!non_ascending_maxima_.empty() &&
45  value > non_ascending_maxima_.back()) {
46  non_ascending_maxima_.pop_back();
47  }
48  non_ascending_maxima_.push_back(value);
49  }
50 
51  void RemoveValue(const float value) {
52  // DCHECK for performance, since this is done for every value in the
53  // precomputation grid.
54  DCHECK(!non_ascending_maxima_.empty());
55  DCHECK_LE(value, non_ascending_maxima_.front());
56  if (value == non_ascending_maxima_.front()) {
57  non_ascending_maxima_.pop_front();
58  }
59  }
60 
61  float GetMaximum() const {
62  // DCHECK for performance, since this is done for every value in the
63  // precomputation grid.
64  DCHECK_GT(non_ascending_maxima_.size(), 0);
65  return non_ascending_maxima_.front();
66  }
67 
68  void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); }
69 
70  private:
71  // Maximum of the current sliding window at the front. Then the maximum of the
72  // remaining window that came after this values first occurence, and so on.
73  std::deque<float> non_ascending_maxima_;
74 };
75 
76 } // namespace
77 
78 proto::FastCorrelativeScanMatcherOptions
80  common::LuaParameterDictionary* const parameter_dictionary) {
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"));
88  return options;
89 }
90 
92  const ProbabilityGrid& probability_grid, const CellLimits& limits,
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) {
98  CHECK_GE(width, 1);
99  CHECK_GE(limits.num_x_cells, 1);
100  CHECK_GE(limits.num_y_cells, 1);
101  const int stride = wide_limits_.num_x_cells;
102  // First we compute the maximum probability for each (x0, y) achieved in the
103  // span defined by x0 <= x < x0 + width.
104  std::vector<float>& intermediate = *reusable_intermediate_grid;
105  intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
106  for (int y = 0; y != limits.num_y_cells; ++y) {
107  SlidingWindowMaximum current_values;
108  current_values.AddValue(
109  probability_grid.GetProbability(Eigen::Array2i(0, y)));
110  for (int x = -width + 1; x != 0; ++x) {
111  intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
112  if (x + width < limits.num_x_cells) {
113  current_values.AddValue(
114  probability_grid.GetProbability(Eigen::Array2i(x + width, y)));
115  }
116  }
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(
120  probability_grid.GetProbability(Eigen::Array2i(x, y)));
121  current_values.AddValue(
122  probability_grid.GetProbability(Eigen::Array2i(x + width, y)));
123  }
124  for (int x = std::max(limits.num_x_cells - width, 0);
125  x != limits.num_x_cells; ++x) {
126  intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
127  current_values.RemoveValue(
128  probability_grid.GetProbability(Eigen::Array2i(x, y)));
129  }
130  current_values.CheckIsEmpty();
131  }
132  // For each (x, y), we compute the maximum probability in the width x width
133  // region starting at each (x, y) and precompute the resulting bound on the
134  // score.
135  for (int x = 0; x != wide_limits_.num_x_cells; ++x) {
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] =
140  ComputeCellValue(current_values.GetMaximum());
141  if (y + width < limits.num_y_cells) {
142  current_values.AddValue(intermediate[x + (y + width) * stride]);
143  }
144  }
145  for (int y = 0; y < limits.num_y_cells - width; ++y) {
146  cells_[x + (y + width - 1) * stride] =
147  ComputeCellValue(current_values.GetMaximum());
148  current_values.RemoveValue(intermediate[x + y * stride]);
149  current_values.AddValue(intermediate[x + (y + width) * stride]);
150  }
151  for (int y = std::max(limits.num_y_cells - width, 0);
152  y != limits.num_y_cells; ++y) {
153  cells_[x + (y + width - 1) * stride] =
154  ComputeCellValue(current_values.GetMaximum());
155  current_values.RemoveValue(intermediate[x + y * stride]);
156  }
157  current_values.CheckIsEmpty();
158  }
159 }
160 
161 uint8 PrecomputationGrid::ComputeCellValue(const float probability) const {
162  const int cell_value = common::RoundToInt(
163  (probability - mapping::kMinProbability) *
165  CHECK_GE(cell_value, 0);
166  CHECK_LE(cell_value, 255);
167  return cell_value;
168 }
169 
171  public:
173  const ProbabilityGrid& probability_grid,
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;
179  const CellLimits limits = probability_grid.limits().cell_limits();
180  reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
181  limits.num_y_cells);
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);
186  }
187  }
188 
189  const PrecomputationGrid& Get(int index) {
190  return precomputation_grids_[index];
191  }
192 
193  int max_depth() const { return precomputation_grids_.size() - 1; }
194 
195  private:
196  std::vector<PrecomputationGrid> precomputation_grids_;
197 };
198 
200  const ProbabilityGrid& probability_grid,
201  const proto::FastCorrelativeScanMatcherOptions& options)
202  : options_(options),
203  limits_(probability_grid.limits()),
204  precomputation_grid_stack_(
205  new PrecomputationGridStack(probability_grid, options)) {}
206 
208 
210  const transform::Rigid2d& initial_pose_estimate,
211  const sensor::PointCloud& point_cloud, const float min_score, float* score,
212  transform::Rigid2d* pose_estimate) const {
213  const SearchParameters search_parameters(options_.linear_search_window(),
214  options_.angular_search_window(),
215  point_cloud, limits_.resolution());
216  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
217  point_cloud, min_score, score,
218  pose_estimate);
219 }
220 
222  const sensor::PointCloud& point_cloud, float min_score, float* score,
223  transform::Rigid2d* pose_estimate) const {
224  // Compute a search window around the center of the submap that includes it
225  // fully.
226  const SearchParameters search_parameters(
227  1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
228  M_PI, // Angular search window, 180 degrees in both directions.
229  point_cloud, limits_.resolution());
231  limits_.max() - 0.5 * limits_.resolution() *
232  Eigen::Vector2d(limits_.cell_limits().num_y_cells,
234  return MatchWithSearchParameters(search_parameters, center, point_cloud,
235  min_score, score, pose_estimate);
236 }
237 
239  SearchParameters search_parameters,
240  const transform::Rigid2d& initial_pose_estimate,
241  const sensor::PointCloud& point_cloud, float min_score, float* score,
242  transform::Rigid2d* pose_estimate) const {
243  CHECK_NOTNULL(score);
244  CHECK_NOTNULL(pose_estimate);
245 
246  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
247  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
248  point_cloud,
249  transform::Rigid3f::Rotation(Eigen::AngleAxisf(
250  initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
251  const std::vector<sensor::PointCloud> rotated_scans =
252  GenerateRotatedScans(rotated_point_cloud, search_parameters);
253  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
254  limits_, rotated_scans,
255  Eigen::Translation2f(initial_pose_estimate.translation().x(),
256  initial_pose_estimate.translation().y()));
257  search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
258 
259  const std::vector<Candidate> lowest_resolution_candidates =
260  ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
261  const Candidate best_candidate = BranchAndBound(
262  discrete_scans, search_parameters, lowest_resolution_candidates,
263  precomputation_grid_stack_->max_depth(), min_score);
264  if (best_candidate.score > min_score) {
265  *score = best_candidate.score;
266  *pose_estimate = transform::Rigid2d(
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));
270  return true;
271  }
272  return false;
273 }
274 
275 std::vector<Candidate>
277  const std::vector<DiscreteScan>& discrete_scans,
278  const SearchParameters& search_parameters) const {
279  std::vector<Candidate> lowest_resolution_candidates =
280  GenerateLowestResolutionCandidates(search_parameters);
283  discrete_scans, search_parameters, &lowest_resolution_candidates);
284  return lowest_resolution_candidates;
285 }
286 
287 std::vector<Candidate>
289  const SearchParameters& search_parameters) const {
290  const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
291  int num_candidates = 0;
292  for (int scan_index = 0; scan_index != search_parameters.num_scans;
293  ++scan_index) {
294  const int num_lowest_resolution_linear_x_candidates =
295  (search_parameters.linear_bounds[scan_index].max_x -
296  search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
297  linear_step_size;
298  const int num_lowest_resolution_linear_y_candidates =
299  (search_parameters.linear_bounds[scan_index].max_y -
300  search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
301  linear_step_size;
302  num_candidates += num_lowest_resolution_linear_x_candidates *
303  num_lowest_resolution_linear_y_candidates;
304  }
305  std::vector<Candidate> candidates;
306  candidates.reserve(num_candidates);
307  for (int scan_index = 0; scan_index != search_parameters.num_scans;
308  ++scan_index) {
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 =
313  search_parameters.linear_bounds[scan_index].min_y;
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,
317  search_parameters);
318  }
319  }
320  }
321  CHECK_EQ(candidates.size(), num_candidates);
322  return candidates;
323 }
324 
326  const PrecomputationGrid& precomputation_grid,
327  const std::vector<DiscreteScan>& discrete_scans,
328  const SearchParameters& search_parameters,
329  std::vector<Candidate>* const candidates) const {
330  for (Candidate& candidate : *candidates) {
331  int sum = 0;
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);
338  }
339  candidate.score = PrecomputationGrid::ToProbability(
340  sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
341  }
342  std::sort(candidates->begin(), candidates->end(), std::greater<Candidate>());
343 }
344 
346  const std::vector<DiscreteScan>& discrete_scans,
347  const SearchParameters& search_parameters,
348  const std::vector<Candidate>& candidates, const int candidate_depth,
349  float min_score) const {
350  if (candidate_depth == 0) {
351  // Return the best candidate.
352  return *candidates.begin();
353  }
354 
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) {
359  break;
360  }
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) {
366  break;
367  }
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) {
371  break;
372  }
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);
376  }
377  }
378  ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
379  discrete_scans, search_parameters,
380  &higher_resolution_candidates);
381  best_high_resolution_candidate = std::max(
382  best_high_resolution_candidate,
383  BranchAndBound(discrete_scans, search_parameters,
384  higher_resolution_candidates, candidate_depth - 1,
385  best_high_resolution_candidate.score));
386  }
387  return best_high_resolution_candidate;
388 }
389 
390 } // namespace scan_matching
391 } // namespace mapping_2d
392 } // namespace cartographer
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
const Vector & translation() const
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
FastCorrelativeScanMatcher(const ProbabilityGrid &probability_grid, const proto::FastCorrelativeScanMatcherOptions &options)
static Rigid3 Rotation(const AngleAxis &angle_axis)
std::vector< Candidate > ComputeLowestResolutionCandidates(const std::vector< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters) const
const Eigen::Vector2d & max() const
Definition: map_limits.h:61
int RoundToInt(const float x)
Definition: port.h:42
constexpr float kMinProbability
uint8_t uint8
Definition: port.h:32
constexpr float kMaxProbability
Rigid2< double > Rigid2d
float GetProbability(const Eigen::Array2i &xy_index) const
const CellLimits & cell_limits() const
Definition: map_limits.h:64
Candidate BranchAndBound(const std::vector< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters, const std::vector< Candidate > &candidates, int candidate_depth, float min_score) const
PrecomputationGrid(const ProbabilityGrid &probability_grid, const CellLimits &limits, int width, std::vector< float > *reusable_intermediate_grid)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
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)
std::vector< Candidate > GenerateLowestResolutionCandidates(const SearchParameters &search_parameters) const
float value
bool Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, float min_score, float *score, transform::Rigid2d *pose_estimate) 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 MatchFullSubmap(const sensor::PointCloud &point_cloud, float min_score, float *score, transform::Rigid2d *pose_estimate) const
static Rigid2 Translation(const Vector &vector)
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58