12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Prepare 3D precomputation grid for full submap matches. (#144) 

Removes an optimization to improve the bounds computed
via the precomputation grid in 3D that takes the limited
search window into account.

This is to allow reusing the same precomputation grids
for full submap matches.
master
Wolfgang Hess 2016-11-25 15:45:26 +01:00 committed by GitHub
parent 17a3393694
commit 88e4ea2866
1 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -58,8 +58,7 @@ class PrecomputationGridStack {
public: public:
PrecomputationGridStack( PrecomputationGridStack(
const HybridGrid& hybrid_grid, const HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions& options, const proto::FastCorrelativeScanMatcherOptions& options) {
const Eigen::Array3i& max_widths) {
CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.branch_and_bound_depth(), 1);
CHECK_GE(options.full_resolution_depth(), 1); CHECK_GE(options.full_resolution_depth(), 1);
precomputation_grids_.reserve(options.branch_and_bound_depth()); precomputation_grids_.reserve(options.branch_and_bound_depth());
@ -67,8 +66,7 @@ class PrecomputationGridStack {
Eigen::Array3i last_width = Eigen::Array3i::Ones(); Eigen::Array3i last_width = Eigen::Array3i::Ones();
for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) { for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) {
const bool half_resolution = depth >= options.full_resolution_depth(); const bool half_resolution = depth >= options.full_resolution_depth();
const Eigen::Array3i next_width = const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
((1 << depth) * Eigen::Array3i::Ones()).cwiseMin(max_widths);
const int full_voxels_per_high_resolution_voxel = const int full_voxels_per_high_resolution_voxel =
1 << std::max(0, depth - options.full_resolution_depth()); 1 << std::max(0, depth - options.full_resolution_depth());
const Eigen::Array3i shift = const Eigen::Array3i shift =
@ -101,10 +99,8 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
common::RoundToInt(options_.linear_xy_search_window() / resolution_)), common::RoundToInt(options_.linear_xy_search_window() / resolution_)),
linear_z_window_size_( linear_z_window_size_(
common::RoundToInt(options_.linear_z_search_window() / resolution_)), common::RoundToInt(options_.linear_z_search_window() / resolution_)),
precomputation_grid_stack_(common::make_unique<PrecomputationGridStack>( precomputation_grid_stack_(
hybrid_grid, options, Eigen::Array3i(1 + 2 * linear_xy_window_size_, common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
1 + 2 * linear_xy_window_size_,
1 + 2 * linear_z_window_size_))),
rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {} rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {}
FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}