24 #include "Eigen/Geometry" 30 #include "glog/logging.h" 33 namespace mapping_2d {
34 namespace scan_matching {
36 proto::RealTimeCorrelativeScanMatcherOptions
39 proto::RealTimeCorrelativeScanMatcherOptions options;
40 options.set_linear_search_window(
41 parameter_dictionary->
GetDouble(
"linear_search_window"));
42 options.set_angular_search_window(
43 parameter_dictionary->
GetDouble(
"angular_search_window"));
44 options.set_translation_delta_cost_weight(
45 parameter_dictionary->
GetDouble(
"translation_delta_cost_weight"));
46 options.set_rotation_delta_cost_weight(
47 parameter_dictionary->
GetDouble(
"rotation_delta_cost_weight"));
48 CHECK_GE(options.translation_delta_cost_weight(), 0.);
49 CHECK_GE(options.rotation_delta_cost_weight(), 0.);
54 const proto::RealTimeCorrelativeScanMatcherOptions& options)
57 std::vector<Candidate>
60 int num_candidates = 0;
61 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
63 const int num_linear_x_candidates =
66 const int num_linear_y_candidates =
69 num_candidates += num_linear_x_candidates * num_linear_y_candidates;
71 std::vector<Candidate> candidates;
72 candidates.reserve(num_candidates);
73 for (
int scan_index = 0; scan_index != search_parameters.
num_scans;
75 for (
int x_index_offset = search_parameters.
linear_bounds[scan_index].min_x;
76 x_index_offset <= search_parameters.
linear_bounds[scan_index].max_x;
78 for (
int y_index_offset =
80 y_index_offset <= search_parameters.
linear_bounds[scan_index].max_y;
82 candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
87 CHECK_EQ(candidates.size(), num_candidates);
96 CHECK_NOTNULL(pose_estimate);
98 const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.
rotation();
102 initial_rotation.cast<
float>().angle(), Eigen::Vector3f::UnitZ())));
107 const std::vector<sensor::PointCloud> rotated_scans =
110 probability_grid.
limits(), rotated_scans,
111 Eigen::Translation2f(initial_pose_estimate.
translation().x(),
113 std::vector<Candidate> candidates =
119 *std::max_element(candidates.begin(), candidates.end());
121 {initial_pose_estimate.
translation().x() + best_candidate.
x,
122 initial_pose_estimate.
translation().y() + best_candidate.
y},
123 initial_rotation * Eigen::Rotation2Dd(best_candidate.
orientation));
124 return best_candidate.
score;
129 const std::vector<DiscreteScan>& discrete_scans,
131 std::vector<Candidate>*
const candidates)
const {
132 for (
Candidate& candidate : *candidates) {
133 candidate.score = 0.f;
134 for (
const Eigen::Array2i& xy_index :
135 discrete_scans[candidate.scan_index]) {
136 const Eigen::Array2i proposed_xy_index(
137 xy_index.x() + candidate.x_index_offset,
138 xy_index.y() + candidate.y_index_offset);
139 const float probability =
141 candidate.score += probability;
144 static_cast<float>(discrete_scans[candidate.scan_index].size());
146 std::exp(-
common::Pow2(std::hypot(candidate.x, candidate.y) *
147 options_.translation_delta_cost_weight() +
148 std::abs(candidate.orientation) *
149 options_.rotation_delta_cost_weight()));
150 CHECK_GT(candidate.score, 0.f);
std::vector< DiscreteScan > 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< DiscreteScan > &discrete_scans, const SearchParameters &search_parameters, std::vector< Candidate > *candidates) const
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
RealTimeCorrelativeScanMatcher(const proto::RealTimeCorrelativeScanMatcherOptions &options)
std::vector< LinearBounds > linear_bounds
double GetDouble(const string &key)
double resolution() const
const proto::RealTimeCorrelativeScanMatcherOptions options_
float GetProbability(const Eigen::Array2i &xy_index) const
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
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
std::vector< Candidate > GenerateExhaustiveSearchCandidates(const SearchParameters &search_parameters) const
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)