From e89625d36aebaf427d9d633c117d7a5d20e2ef14 Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 9 Apr 2018 14:29:45 +0200 Subject: [PATCH] Correctly use PrecomputationGridStack for 2D and 3D (#1040) FIXES=#1038 --- .../fast_correlative_scan_matcher_2d.cc | 47 ++++++---------- .../fast_correlative_scan_matcher_2d.h | 18 +++++- .../fast_correlative_scan_matcher_3d.cc | 56 +++++++------------ .../fast_correlative_scan_matcher_3d.h | 20 ++++++- 4 files changed, 73 insertions(+), 68 deletions(-) diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc index afe6439..e5ed349 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc @@ -23,6 +23,7 @@ #include #include "Eigen/Geometry" +#include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" @@ -166,42 +167,30 @@ uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const { return cell_value; } -class PrecomputationGridStack { - public: - PrecomputationGridStack( - const ProbabilityGrid& probability_grid, - const proto::FastCorrelativeScanMatcherOptions2D& options) { - CHECK_GE(options.branch_and_bound_depth(), 1); - const int max_width = 1 << (options.branch_and_bound_depth() - 1); - precomputation_grids_.reserve(options.branch_and_bound_depth()); - std::vector reusable_intermediate_grid; - const CellLimits limits = probability_grid.limits().cell_limits(); - reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * - limits.num_y_cells); - for (int i = 0; i != options.branch_and_bound_depth(); ++i) { - const int width = 1 << i; - precomputation_grids_.emplace_back(probability_grid, limits, width, - &reusable_intermediate_grid); - } +PrecomputationGridStack2D::PrecomputationGridStack2D( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions2D& options) { + CHECK_GE(options.branch_and_bound_depth(), 1); + const int max_width = 1 << (options.branch_and_bound_depth() - 1); + precomputation_grids_.reserve(options.branch_and_bound_depth()); + std::vector reusable_intermediate_grid; + const CellLimits limits = probability_grid.limits().cell_limits(); + reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * + limits.num_y_cells); + for (int i = 0; i != options.branch_and_bound_depth(); ++i) { + const int width = 1 << i; + precomputation_grids_.emplace_back(probability_grid, limits, width, + &reusable_intermediate_grid); } - - const PrecomputationGrid2D& Get(int index) { - return precomputation_grids_[index]; - } - - int max_depth() const { return precomputation_grids_.size() - 1; } - - private: - std::vector precomputation_grids_; -}; +} FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D( const ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions2D& options) : options_(options), limits_(probability_grid.limits()), - precomputation_grid_stack_( - new PrecomputationGridStack(probability_grid, options)) {} + precomputation_grid_stack_(common::make_unique( + probability_grid, options)) {} FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {} diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h index 6ead8cd..41dae4a 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h @@ -92,7 +92,21 @@ class PrecomputationGrid2D { std::vector cells_; }; -class PrecomputationGridStack; +class PrecomputationGridStack2D { + public: + PrecomputationGridStack2D( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions2D& options); + + const PrecomputationGrid2D& Get(int index) { + return precomputation_grids_[index]; + } + + int max_depth() const { return precomputation_grids_.size() - 1; } + + private: + std::vector precomputation_grids_; +}; // An implementation of "Real-Time Correlative Scan Matching" by Olson. class FastCorrelativeScanMatcher2D { @@ -146,7 +160,7 @@ class FastCorrelativeScanMatcher2D { const proto::FastCorrelativeScanMatcherOptions2D options_; MapLimits limits_; - std::unique_ptr precomputation_grid_stack_; + std::unique_ptr precomputation_grid_stack_; }; } // namespace scan_matching diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc index 22e5f3d..9b8c7bf 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc @@ -26,7 +26,6 @@ #include "cartographer/common/math.h" #include "cartographer/mapping/3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h" -#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -55,40 +54,27 @@ CreateFastCorrelativeScanMatcherOptions3D( return options; } -class PrecomputationGridStack { - public: - PrecomputationGridStack( - const HybridGrid& hybrid_grid, - const proto::FastCorrelativeScanMatcherOptions3D& options) { - CHECK_GE(options.branch_and_bound_depth(), 1); - CHECK_GE(options.full_resolution_depth(), 1); - precomputation_grids_.reserve(options.branch_and_bound_depth()); - precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid)); - Eigen::Array3i last_width = Eigen::Array3i::Ones(); - for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) { - const bool half_resolution = depth >= options.full_resolution_depth(); - const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones()); - const int full_voxels_per_high_resolution_voxel = - 1 << std::max(0, depth - options.full_resolution_depth()); - const Eigen::Array3i shift = - (next_width - last_width + - (full_voxels_per_high_resolution_voxel - 1)) / - full_voxels_per_high_resolution_voxel; - precomputation_grids_.push_back( - PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift)); - last_width = next_width; - } +PrecomputationGridStack3D::PrecomputationGridStack3D( + const HybridGrid& hybrid_grid, + const proto::FastCorrelativeScanMatcherOptions3D& options) { + CHECK_GE(options.branch_and_bound_depth(), 1); + CHECK_GE(options.full_resolution_depth(), 1); + precomputation_grids_.reserve(options.branch_and_bound_depth()); + precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid)); + Eigen::Array3i last_width = Eigen::Array3i::Ones(); + for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) { + const bool half_resolution = depth >= options.full_resolution_depth(); + const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones()); + const int full_voxels_per_high_resolution_voxel = + 1 << std::max(0, depth - options.full_resolution_depth()); + const Eigen::Array3i shift = (next_width - last_width + + (full_voxels_per_high_resolution_voxel - 1)) / + full_voxels_per_high_resolution_voxel; + precomputation_grids_.push_back( + PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift)); + last_width = next_width; } - - const PrecomputationGrid3D& Get(int depth) const { - return precomputation_grids_.at(depth); - } - - int max_depth() const { return precomputation_grids_.size() - 1; } - - private: - std::vector precomputation_grids_; -}; +} struct DiscreteScan3D { transform::Rigid3f pose; @@ -150,7 +136,7 @@ FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D( resolution_(hybrid_grid.resolution()), width_in_voxels_(hybrid_grid.grid_size()), precomputation_grid_stack_( - common::make_unique(hybrid_grid, options)), + common::make_unique(hybrid_grid, options)), low_resolution_hybrid_grid_(low_resolution_hybrid_grid), rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {} diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h index 0fcacc7..5033776 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h @@ -28,6 +28,7 @@ #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h" +#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/point_cloud.h" @@ -40,7 +41,22 @@ proto::FastCorrelativeScanMatcherOptions3D CreateFastCorrelativeScanMatcherOptions3D( common::LuaParameterDictionary* parameter_dictionary); -class PrecomputationGridStack; +class PrecomputationGridStack3D { + public: + PrecomputationGridStack3D( + const HybridGrid& hybrid_grid, + const proto::FastCorrelativeScanMatcherOptions3D& options); + + const PrecomputationGrid3D& Get(int depth) const { + return precomputation_grids_.at(depth); + } + + int max_depth() const { return precomputation_grids_.size() - 1; } + + private: + std::vector precomputation_grids_; +}; + struct DiscreteScan3D; struct Candidate3D; @@ -129,7 +145,7 @@ class FastCorrelativeScanMatcher3D { const proto::FastCorrelativeScanMatcherOptions3D options_; const float resolution_; const int width_in_voxels_; - std::unique_ptr precomputation_grid_stack_; + std::unique_ptr precomputation_grid_stack_; const HybridGrid* const low_resolution_hybrid_grid_; RotationalScanMatcher rotational_scan_matcher_; };