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

View File

@ -23,6 +23,7 @@
#include <limits> #include <limits>
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
@ -166,9 +167,7 @@ uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
return cell_value; return cell_value;
} }
class PrecomputationGridStack { PrecomputationGridStack2D::PrecomputationGridStack2D(
public:
PrecomputationGridStack(
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions2D& options) { const proto::FastCorrelativeScanMatcherOptions2D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.branch_and_bound_depth(), 1);
@ -183,25 +182,15 @@ class PrecomputationGridStack {
precomputation_grids_.emplace_back(probability_grid, limits, width, precomputation_grids_.emplace_back(probability_grid, limits, width,
&reusable_intermediate_grid); &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( FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
const proto::FastCorrelativeScanMatcherOptions2D& options) const proto::FastCorrelativeScanMatcherOptions2D& options)
: options_(options), : options_(options),
limits_(probability_grid.limits()), limits_(probability_grid.limits()),
precomputation_grid_stack_( precomputation_grid_stack_(common::make_unique<PrecomputationGridStack2D>(
new PrecomputationGridStack(probability_grid, options)) {} probability_grid, options)) {}
FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {} FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}

View File

@ -92,7 +92,21 @@ class PrecomputationGrid2D {
std::vector<uint8> cells_; 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. // An implementation of "Real-Time Correlative Scan Matching" by Olson.
class FastCorrelativeScanMatcher2D { class FastCorrelativeScanMatcher2D {
@ -146,7 +160,7 @@ class FastCorrelativeScanMatcher2D {
const proto::FastCorrelativeScanMatcherOptions2D options_; const proto::FastCorrelativeScanMatcherOptions2D options_;
MapLimits limits_; MapLimits limits_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_; std::unique_ptr<PrecomputationGridStack2D> precomputation_grid_stack_;
}; };
} // namespace scan_matching } // namespace scan_matching

View File

@ -26,7 +26,6 @@
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.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/low_resolution_matcher.h"
#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -55,9 +54,7 @@ CreateFastCorrelativeScanMatcherOptions3D(
return options; return options;
} }
class PrecomputationGridStack { PrecomputationGridStack3D::PrecomputationGridStack3D(
public:
PrecomputationGridStack(
const HybridGrid& hybrid_grid, const HybridGrid& hybrid_grid,
const proto::FastCorrelativeScanMatcherOptions3D& options) { const proto::FastCorrelativeScanMatcherOptions3D& options) {
CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.branch_and_bound_depth(), 1);
@ -70,25 +67,14 @@ class PrecomputationGridStack {
const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones()); const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
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 = (next_width - last_width +
(next_width - last_width +
(full_voxels_per_high_resolution_voxel - 1)) / (full_voxels_per_high_resolution_voxel - 1)) /
full_voxels_per_high_resolution_voxel; full_voxels_per_high_resolution_voxel;
precomputation_grids_.push_back( precomputation_grids_.push_back(
PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift)); PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift));
last_width = next_width; 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 { struct DiscreteScan3D {
transform::Rigid3f pose; transform::Rigid3f pose;
@ -150,7 +136,7 @@ FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
resolution_(hybrid_grid.resolution()), resolution_(hybrid_grid.resolution()),
width_in_voxels_(hybrid_grid.grid_size()), width_in_voxels_(hybrid_grid.grid_size()),
precomputation_grid_stack_( 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), low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {} rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}

View File

@ -28,6 +28,7 @@
#include "cartographer/mapping/3d/hybrid_grid.h" #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/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/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/internal/3d/scan_matching/rotational_scan_matcher.h"
#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
@ -40,7 +41,22 @@ proto::FastCorrelativeScanMatcherOptions3D
CreateFastCorrelativeScanMatcherOptions3D( CreateFastCorrelativeScanMatcherOptions3D(
common::LuaParameterDictionary* parameter_dictionary); 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 DiscreteScan3D;
struct Candidate3D; struct Candidate3D;
@ -129,7 +145,7 @@ class FastCorrelativeScanMatcher3D {
const proto::FastCorrelativeScanMatcherOptions3D options_; const proto::FastCorrelativeScanMatcherOptions3D options_;
const float resolution_; const float resolution_;
const int width_in_voxels_; 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_; const HybridGrid* const low_resolution_hybrid_grid_;
RotationalScanMatcher rotational_scan_matcher_; RotationalScanMatcher rotational_scan_matcher_;
}; };