21 #include "Eigen/Geometry" 24 #include "glog/logging.h" 28 namespace scan_matching {
31 const proto::RealTimeCorrelativeScanMatcherOptions& options)
34 float RealTimeCorrelativeScanMatcher3D::Match(
38 CHECK_NOTNULL(pose_estimate);
39 float best_score = -1.f;
43 initial_pose_estimate.
cast<
float>() * transform;
44 const float score = ScoreCandidate(
47 if (score > best_score) {
49 *pose_estimate = candidate.
cast<
double>();
55 std::vector<transform::Rigid3f>
56 RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms(
58 std::vector<transform::Rigid3f> result;
59 const int linear_window_size =
63 float max_scan_range = 3.f * resolution;
64 for (
const Eigen::Vector3f& point : point_cloud) {
65 const float range = point.norm();
66 max_scan_range = std::max(range, max_scan_range);
68 const float kSafetyMargin = 1.f - 1e-3f;
69 const float angular_step_size =
70 kSafetyMargin * std::acos(1.f -
common::Pow2(resolution) /
72 const int angular_window_size =
74 for (
int z = -linear_window_size; z <= linear_window_size; ++z) {
75 for (
int y = -linear_window_size; y <= linear_window_size; ++y) {
76 for (
int x = -linear_window_size; x <= linear_window_size; ++x) {
77 for (
int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
78 for (
int ry = -angular_window_size; ry <= angular_window_size; ++ry) {
79 for (
int rx = -angular_window_size; rx <= angular_window_size;
81 const Eigen::Vector3f angle_axis(rx * angular_step_size,
82 ry * angular_step_size,
83 rz * angular_step_size);
85 Eigen::Vector3f(x * resolution, y * resolution,
97 float RealTimeCorrelativeScanMatcher3D::ScoreCandidate(
102 for (
const Eigen::Vector3f& point : transformed_point_cloud) {
105 score /=
static_cast<float>(transformed_point_cloud.size());
109 options_.translation_delta_cost_weight() +
110 angle *
options_.rotation_delta_cost_weight()));
111 CHECK_GT(score, 0.f);
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
RealTimeCorrelativeScanMatcher3D(const scan_matching::proto::RealTimeCorrelativeScanMatcherOptions &options)
int RoundToInt(const float x)
float GetProbability(const Eigen::Array3i &index) const
proto::ProbabilityGridRangeDataInserterOptions2D options_
std::vector< Eigen::Vector3f > PointCloud
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const