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

Correctly use PrecomputationGridStack for 2D and 3D (#1040) 

FIXES=#1038
master
gaschler 2018-04-09 14:29:45 +02:00 committed by Wally B. Feed
parent b1dcb0cc5e
commit e89625d36a
4 changed files with 73 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc
View File

@ -23,6 +23,7 @@
#include <limits>
#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<float> 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<float> 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<PrecomputationGrid2D> 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<PrecomputationGridStack2D>(
probability_grid, options)) {}
FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}

18
cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h
View File

@ -92,7 +92,21 @@ class PrecomputationGrid2D {
std::vector<uint8> 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<PrecomputationGrid2D> 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<PrecomputationGridStack> precomputation_grid_stack_;
std::unique_ptr<PrecomputationGridStack2D> precomputation_grid_stack_;
};
} // namespace scan_matching

56
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc
View File

@ -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<PrecomputationGrid3D> 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<PrecomputationGridStack>(hybrid_grid, options)),
common::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}

20
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h
View File

@ -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<PrecomputationGrid3D> 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<PrecomputationGridStack> precomputation_grid_stack_;
std::unique_ptr<PrecomputationGridStack3D> precomputation_grid_stack_;
const HybridGrid* const low_resolution_hybrid_grid_;
RotationalScanMatcher rotational_scan_matcher_;
};