parent
b1dcb0cc5e
commit
e89625d36a
|
@ -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,9 +167,7 @@ uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
|
|||
return cell_value;
|
||||
}
|
||||
|
||||
class PrecomputationGridStack {
|
||||
public:
|
||||
PrecomputationGridStack(
|
||||
PrecomputationGridStack2D::PrecomputationGridStack2D(
|
||||
const ProbabilityGrid& probability_grid,
|
||||
const proto::FastCorrelativeScanMatcherOptions2D& options) {
|
||||
CHECK_GE(options.branch_and_bound_depth(), 1);
|
||||
|
@ -185,23 +184,13 @@ class PrecomputationGridStack {
|
|||
}
|
||||
}
|
||||
|
||||
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() {}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,9 +54,7 @@ CreateFastCorrelativeScanMatcherOptions3D(
|
|||
return options;
|
||||
}
|
||||
|
||||
class PrecomputationGridStack {
|
||||
public:
|
||||
PrecomputationGridStack(
|
||||
PrecomputationGridStack3D::PrecomputationGridStack3D(
|
||||
const HybridGrid& hybrid_grid,
|
||||
const proto::FastCorrelativeScanMatcherOptions3D& options) {
|
||||
CHECK_GE(options.branch_and_bound_depth(), 1);
|
||||
|
@ -70,8 +67,7 @@ class PrecomputationGridStack {
|
|||
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 +
|
||||
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(
|
||||
|
@ -80,16 +76,6 @@ class PrecomputationGridStack {
|
|||
}
|
||||
}
|
||||
|
||||
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;
|
||||
// Contains a vector of discretized scans for each 'depth'.
|
||||
|
@ -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)) {}
|
||||
|
||||
|
|
|
@ -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_;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue