24 #include "Eigen/Geometry" 30 #include "glog/logging.h" 34 namespace scan_matching {
37 const proto::RealTimeCorrelativeScanMatcherOptions& options)
40 std::vector<Candidate2D>
43 int num_candidates = 0;
44 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
46 const int num_linear_x_candidates =
49 const int num_linear_y_candidates =
52 num_candidates += num_linear_x_candidates * num_linear_y_candidates;
54 std::vector<Candidate2D> candidates;
55 candidates.reserve(num_candidates);
56 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
58 for (
int x_index_offset = search_parameters.
linear_bounds[scan_index].min_x;
59 x_index_offset <= search_parameters.
linear_bounds[scan_index].max_x;
61 for (
int y_index_offset =
63 y_index_offset <= search_parameters.
linear_bounds[scan_index].max_y;
65 candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
70 CHECK_EQ(candidates.size(), num_candidates);
79 CHECK_NOTNULL(pose_estimate);
81 const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.
rotation();
85 initial_rotation.cast<
float>().angle(), Eigen::Vector3f::UnitZ())));
90 const std::vector<sensor::PointCloud> rotated_scans =
93 probability_grid.
limits(), rotated_scans,
94 Eigen::Translation2f(initial_pose_estimate.
translation().x(),
96 std::vector<Candidate2D> candidates =
102 *std::max_element(candidates.begin(), candidates.end());
104 {initial_pose_estimate.
translation().x() + best_candidate.
x,
105 initial_pose_estimate.
translation().y() + best_candidate.
y},
106 initial_rotation * Eigen::Rotation2Dd(best_candidate.
orientation));
107 return best_candidate.
score;
112 const std::vector<DiscreteScan2D>& discrete_scans,
114 std::vector<Candidate2D>*
const candidates)
const {
116 candidate.score = 0.f;
117 for (
const Eigen::Array2i& xy_index :
118 discrete_scans[candidate.scan_index]) {
119 const Eigen::Array2i proposed_xy_index(
120 xy_index.x() + candidate.x_index_offset,
121 xy_index.y() + candidate.y_index_offset);
122 const float probability =
124 candidate.score += probability;
127 static_cast<float>(discrete_scans[candidate.scan_index].size());
129 std::exp(-
common::Pow2(std::hypot(candidate.x, candidate.y) *
130 options_.translation_delta_cost_weight() +
131 std::abs(candidate.orientation) *
132 options_.rotation_delta_cost_weight()));
133 CHECK_GT(candidate.score, 0.f);
std::vector< DiscreteScan2D > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
void ScoreCandidates(const ProbabilityGrid &probability_grid, const std::vector< DiscreteScan2D > &discrete_scans, const SearchParameters &search_parameters, std::vector< Candidate2D > *candidates) const
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
std::vector< LinearBounds > linear_bounds
double resolution() const
std::vector< Candidate2D > GenerateExhaustiveSearchCandidates(const SearchParameters &search_parameters) const
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)
proto::ProbabilityGridRangeDataInserterOptions2D options_
RealTimeCorrelativeScanMatcher2D(const proto::RealTimeCorrelativeScanMatcherOptions &options)
float GetProbability(const Eigen::Array2i &cell_index) const
const proto::RealTimeCorrelativeScanMatcherOptions options_
std::vector< Eigen::Vector3f > PointCloud
const MapLimits & limits() const
double Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const ProbabilityGrid &probability_grid, transform::Rigid2d *pose_estimate) const