21 #include "Eigen/Geometry" 24 #include "glog/logging.h" 27 namespace mapping_3d {
28 namespace scan_matching {
31 const mapping_2d::scan_matching::proto::
32 RealTimeCorrelativeScanMatcherOptions& options)
39 CHECK_NOTNULL(pose_estimate);
40 float best_score = -1.f;
44 initial_pose_estimate.
cast<
float>() * transform;
48 if (score > best_score) {
50 *pose_estimate = candidate.
cast<
double>();
56 std::vector<transform::Rigid3f>
59 std::vector<transform::Rigid3f> result;
60 const int linear_window_size =
64 float max_scan_range = 3.f * resolution;
65 for (
const Eigen::Vector3f& point : point_cloud) {
66 const float range = point.norm();
67 max_scan_range = std::max(range, max_scan_range);
69 const float kSafetyMargin = 1.f - 1e-3f;
70 const float angular_step_size =
71 kSafetyMargin * std::acos(1.f -
common::Pow2(resolution) /
73 const int angular_window_size =
75 for (
int z = -linear_window_size; z <= linear_window_size; ++z) {
76 for (
int y = -linear_window_size; y <= linear_window_size; ++y) {
77 for (
int x = -linear_window_size; x <= linear_window_size; ++x) {
78 for (
int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
79 for (
int ry = -angular_window_size; ry <= angular_window_size; ++ry) {
80 for (
int rx = -angular_window_size; rx <= angular_window_size;
82 const Eigen::Vector3f angle_axis(rx * angular_step_size,
83 ry * angular_step_size,
84 rz * angular_step_size);
86 Eigen::Vector3f(x * resolution, y * resolution,
103 for (
const Eigen::Vector3f& point : transformed_point_cloud) {
106 score /=
static_cast<float>(transformed_point_cloud.size());
110 options_.translation_delta_cost_weight() +
111 angle *
options_.rotation_delta_cost_weight()));
112 CHECK_GT(score, 0.f);
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
float ScoreCandidate(const HybridGrid &hybrid_grid, const sensor::PointCloud &transformed_point_cloud, const transform::Rigid3f &transform) const
float GetProbability(const Eigen::Array3i &index) const
int RoundToInt(const float x)
const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions options_
std::vector< Eigen::Vector3f > PointCloud
std::vector< transform::Rigid3f > GenerateExhaustiveSearchTransforms(float resolution, const sensor::PointCloud &point_cloud) const
float Match(const transform::Rigid3d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const HybridGrid &hybrid_grid, transform::Rigid3d *pose_estimate) const
RealTimeCorrelativeScanMatcher(const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions &options)