00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h"
00018
00019 #include <algorithm>
00020 #include <cmath>
00021 #include <deque>
00022 #include <functional>
00023 #include <limits>
00024
00025 #include "Eigen/Geometry"
00026 #include "absl/memory/memory.h"
00027 #include "cartographer/common/math.h"
00028 #include "cartographer/mapping/2d/grid_2d.h"
00029 #include "cartographer/sensor/point_cloud.h"
00030 #include "cartographer/transform/transform.h"
00031 #include "glog/logging.h"
00032
00033 namespace cartographer {
00034 namespace mapping {
00035 namespace scan_matching {
00036 namespace {
00037
00038
00039
00040
00041 class SlidingWindowMaximum {
00042 public:
00043 void AddValue(const float value) {
00044 while (!non_ascending_maxima_.empty() &&
00045 value > non_ascending_maxima_.back()) {
00046 non_ascending_maxima_.pop_back();
00047 }
00048 non_ascending_maxima_.push_back(value);
00049 }
00050
00051 void RemoveValue(const float value) {
00052
00053
00054 DCHECK(!non_ascending_maxima_.empty());
00055 DCHECK_LE(value, non_ascending_maxima_.front());
00056 if (value == non_ascending_maxima_.front()) {
00057 non_ascending_maxima_.pop_front();
00058 }
00059 }
00060
00061 float GetMaximum() const {
00062
00063
00064 DCHECK_GT(non_ascending_maxima_.size(), 0);
00065 return non_ascending_maxima_.front();
00066 }
00067
00068 void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); }
00069
00070 private:
00071
00072
00073 std::deque<float> non_ascending_maxima_;
00074 };
00075
00076 }
00077
00078 proto::FastCorrelativeScanMatcherOptions2D
00079 CreateFastCorrelativeScanMatcherOptions2D(
00080 common::LuaParameterDictionary* const parameter_dictionary) {
00081 proto::FastCorrelativeScanMatcherOptions2D options;
00082 options.set_linear_search_window(
00083 parameter_dictionary->GetDouble("linear_search_window"));
00084 options.set_angular_search_window(
00085 parameter_dictionary->GetDouble("angular_search_window"));
00086 options.set_branch_and_bound_depth(
00087 parameter_dictionary->GetInt("branch_and_bound_depth"));
00088 return options;
00089 }
00090
00091 PrecomputationGrid2D::PrecomputationGrid2D(
00092 const Grid2D& grid, const CellLimits& limits, const int width,
00093 std::vector<float>* reusable_intermediate_grid)
00094 : offset_(-width + 1, -width + 1),
00095 wide_limits_(limits.num_x_cells + width - 1,
00096 limits.num_y_cells + width - 1),
00097 min_score_(1.f - grid.GetMaxCorrespondenceCost()),
00098 max_score_(1.f - grid.GetMinCorrespondenceCost()),
00099 cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
00100 CHECK_GE(width, 1);
00101 CHECK_GE(limits.num_x_cells, 1);
00102 CHECK_GE(limits.num_y_cells, 1);
00103 const int stride = wide_limits_.num_x_cells;
00104
00105
00106 std::vector<float>& intermediate = *reusable_intermediate_grid;
00107 intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells);
00108 for (int y = 0; y != limits.num_y_cells; ++y) {
00109 SlidingWindowMaximum current_values;
00110 current_values.AddValue(
00111 1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));
00112 for (int x = -width + 1; x != 0; ++x) {
00113 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
00114 if (x + width < limits.num_x_cells) {
00115 current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
00116 Eigen::Array2i(x + width, y))));
00117 }
00118 }
00119 for (int x = 0; x < limits.num_x_cells - width; ++x) {
00120 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
00121 current_values.RemoveValue(
00122 1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
00123 current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
00124 Eigen::Array2i(x + width, y))));
00125 }
00126 for (int x = std::max(limits.num_x_cells - width, 0);
00127 x != limits.num_x_cells; ++x) {
00128 intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
00129 current_values.RemoveValue(
00130 1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
00131 }
00132 current_values.CheckIsEmpty();
00133 }
00134
00135
00136
00137 for (int x = 0; x != wide_limits_.num_x_cells; ++x) {
00138 SlidingWindowMaximum current_values;
00139 current_values.AddValue(intermediate[x]);
00140 for (int y = -width + 1; y != 0; ++y) {
00141 cells_[x + (y + width - 1) * stride] =
00142 ComputeCellValue(current_values.GetMaximum());
00143 if (y + width < limits.num_y_cells) {
00144 current_values.AddValue(intermediate[x + (y + width) * stride]);
00145 }
00146 }
00147 for (int y = 0; y < limits.num_y_cells - width; ++y) {
00148 cells_[x + (y + width - 1) * stride] =
00149 ComputeCellValue(current_values.GetMaximum());
00150 current_values.RemoveValue(intermediate[x + y * stride]);
00151 current_values.AddValue(intermediate[x + (y + width) * stride]);
00152 }
00153 for (int y = std::max(limits.num_y_cells - width, 0);
00154 y != limits.num_y_cells; ++y) {
00155 cells_[x + (y + width - 1) * stride] =
00156 ComputeCellValue(current_values.GetMaximum());
00157 current_values.RemoveValue(intermediate[x + y * stride]);
00158 }
00159 current_values.CheckIsEmpty();
00160 }
00161 }
00162
00163 uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
00164 const int cell_value = common::RoundToInt(
00165 (probability - min_score_) * (255.f / (max_score_ - min_score_)));
00166 CHECK_GE(cell_value, 0);
00167 CHECK_LE(cell_value, 255);
00168 return cell_value;
00169 }
00170
00171 PrecomputationGridStack2D::PrecomputationGridStack2D(
00172 const Grid2D& grid,
00173 const proto::FastCorrelativeScanMatcherOptions2D& options) {
00174 CHECK_GE(options.branch_and_bound_depth(), 1);
00175 const int max_width = 1 << (options.branch_and_bound_depth() - 1);
00176 precomputation_grids_.reserve(options.branch_and_bound_depth());
00177 std::vector<float> reusable_intermediate_grid;
00178 const CellLimits limits = grid.limits().cell_limits();
00179 reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
00180 limits.num_y_cells);
00181 for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
00182 const int width = 1 << i;
00183 precomputation_grids_.emplace_back(grid, limits, width,
00184 &reusable_intermediate_grid);
00185 }
00186 }
00187
00188 FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
00189 const Grid2D& grid,
00190 const proto::FastCorrelativeScanMatcherOptions2D& options)
00191 : options_(options),
00192 limits_(grid.limits()),
00193 precomputation_grid_stack_(
00194 absl::make_unique<PrecomputationGridStack2D>(grid, options)) {}
00195
00196 FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}
00197
00198 bool FastCorrelativeScanMatcher2D::Match(
00199 const transform::Rigid2d& initial_pose_estimate,
00200 const sensor::PointCloud& point_cloud, const float min_score, float* score,
00201 transform::Rigid2d* pose_estimate) const {
00202 const SearchParameters search_parameters(options_.linear_search_window(),
00203 options_.angular_search_window(),
00204 point_cloud, limits_.resolution());
00205 return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
00206 point_cloud, min_score, score,
00207 pose_estimate);
00208 }
00209
00210 bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
00211 const sensor::PointCloud& point_cloud, float min_score, float* score,
00212 transform::Rigid2d* pose_estimate) const {
00213
00214
00215 const SearchParameters search_parameters(
00216 1e6 * limits_.resolution(),
00217 M_PI,
00218 point_cloud, limits_.resolution());
00219 const transform::Rigid2d center = transform::Rigid2d::Translation(
00220 limits_.max() - 0.5 * limits_.resolution() *
00221 Eigen::Vector2d(limits_.cell_limits().num_y_cells,
00222 limits_.cell_limits().num_x_cells));
00223 return MatchWithSearchParameters(search_parameters, center, point_cloud,
00224 min_score, score, pose_estimate);
00225 }
00226
00227 bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
00228 SearchParameters search_parameters,
00229 const transform::Rigid2d& initial_pose_estimate,
00230 const sensor::PointCloud& point_cloud, float min_score, float* score,
00231 transform::Rigid2d* pose_estimate) const {
00232 CHECK(score != nullptr);
00233 CHECK(pose_estimate != nullptr);
00234
00235 const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
00236 const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
00237 point_cloud,
00238 transform::Rigid3f::Rotation(Eigen::AngleAxisf(
00239 initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
00240 const std::vector<sensor::PointCloud> rotated_scans =
00241 GenerateRotatedScans(rotated_point_cloud, search_parameters);
00242 const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
00243 limits_, rotated_scans,
00244 Eigen::Translation2f(initial_pose_estimate.translation().x(),
00245 initial_pose_estimate.translation().y()));
00246 search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
00247
00248 const std::vector<Candidate2D> lowest_resolution_candidates =
00249 ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
00250 const Candidate2D best_candidate = BranchAndBound(
00251 discrete_scans, search_parameters, lowest_resolution_candidates,
00252 precomputation_grid_stack_->max_depth(), min_score);
00253 if (best_candidate.score > min_score) {
00254 *score = best_candidate.score;
00255 *pose_estimate = transform::Rigid2d(
00256 {initial_pose_estimate.translation().x() + best_candidate.x,
00257 initial_pose_estimate.translation().y() + best_candidate.y},
00258 initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
00259 return true;
00260 }
00261 return false;
00262 }
00263
00264 std::vector<Candidate2D>
00265 FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
00266 const std::vector<DiscreteScan2D>& discrete_scans,
00267 const SearchParameters& search_parameters) const {
00268 std::vector<Candidate2D> lowest_resolution_candidates =
00269 GenerateLowestResolutionCandidates(search_parameters);
00270 ScoreCandidates(
00271 precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
00272 discrete_scans, search_parameters, &lowest_resolution_candidates);
00273 return lowest_resolution_candidates;
00274 }
00275
00276 std::vector<Candidate2D>
00277 FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
00278 const SearchParameters& search_parameters) const {
00279 const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
00280 int num_candidates = 0;
00281 for (int scan_index = 0; scan_index != search_parameters.num_scans;
00282 ++scan_index) {
00283 const int num_lowest_resolution_linear_x_candidates =
00284 (search_parameters.linear_bounds[scan_index].max_x -
00285 search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
00286 linear_step_size;
00287 const int num_lowest_resolution_linear_y_candidates =
00288 (search_parameters.linear_bounds[scan_index].max_y -
00289 search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
00290 linear_step_size;
00291 num_candidates += num_lowest_resolution_linear_x_candidates *
00292 num_lowest_resolution_linear_y_candidates;
00293 }
00294 std::vector<Candidate2D> candidates;
00295 candidates.reserve(num_candidates);
00296 for (int scan_index = 0; scan_index != search_parameters.num_scans;
00297 ++scan_index) {
00298 for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
00299 x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
00300 x_index_offset += linear_step_size) {
00301 for (int y_index_offset =
00302 search_parameters.linear_bounds[scan_index].min_y;
00303 y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
00304 y_index_offset += linear_step_size) {
00305 candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
00306 search_parameters);
00307 }
00308 }
00309 }
00310 CHECK_EQ(candidates.size(), num_candidates);
00311 return candidates;
00312 }
00313
00314 void FastCorrelativeScanMatcher2D::ScoreCandidates(
00315 const PrecomputationGrid2D& precomputation_grid,
00316 const std::vector<DiscreteScan2D>& discrete_scans,
00317 const SearchParameters& search_parameters,
00318 std::vector<Candidate2D>* const candidates) const {
00319 for (Candidate2D& candidate : *candidates) {
00320 int sum = 0;
00321 for (const Eigen::Array2i& xy_index :
00322 discrete_scans[candidate.scan_index]) {
00323 const Eigen::Array2i proposed_xy_index(
00324 xy_index.x() + candidate.x_index_offset,
00325 xy_index.y() + candidate.y_index_offset);
00326 sum += precomputation_grid.GetValue(proposed_xy_index);
00327 }
00328 candidate.score = precomputation_grid.ToScore(
00329 sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
00330 }
00331 std::sort(candidates->begin(), candidates->end(),
00332 std::greater<Candidate2D>());
00333 }
00334
00335 Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
00336 const std::vector<DiscreteScan2D>& discrete_scans,
00337 const SearchParameters& search_parameters,
00338 const std::vector<Candidate2D>& candidates, const int candidate_depth,
00339 float min_score) const {
00340 if (candidate_depth == 0) {
00341
00342 return *candidates.begin();
00343 }
00344
00345 Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
00346 best_high_resolution_candidate.score = min_score;
00347 for (const Candidate2D& candidate : candidates) {
00348 if (candidate.score <= min_score) {
00349 break;
00350 }
00351 std::vector<Candidate2D> higher_resolution_candidates;
00352 const int half_width = 1 << (candidate_depth - 1);
00353 for (int x_offset : {0, half_width}) {
00354 if (candidate.x_index_offset + x_offset >
00355 search_parameters.linear_bounds[candidate.scan_index].max_x) {
00356 break;
00357 }
00358 for (int y_offset : {0, half_width}) {
00359 if (candidate.y_index_offset + y_offset >
00360 search_parameters.linear_bounds[candidate.scan_index].max_y) {
00361 break;
00362 }
00363 higher_resolution_candidates.emplace_back(
00364 candidate.scan_index, candidate.x_index_offset + x_offset,
00365 candidate.y_index_offset + y_offset, search_parameters);
00366 }
00367 }
00368 ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
00369 discrete_scans, search_parameters,
00370 &higher_resolution_candidates);
00371 best_high_resolution_candidate = std::max(
00372 best_high_resolution_candidate,
00373 BranchAndBound(discrete_scans, search_parameters,
00374 higher_resolution_candidates, candidate_depth - 1,
00375 best_high_resolution_candidate.score));
00376 }
00377 return best_high_resolution_candidate;
00378 }
00379
00380 }
00381 }
00382 }