mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <algorithm>
20 #include <cmath>
21 #include <functional>
22 #include <limits>
23 
24 #include "Eigen/Geometry"
30 #include "glog/logging.h"
31 
32 namespace cartographer {
33 namespace mapping_2d {
34 namespace scan_matching {
35 
36 proto::RealTimeCorrelativeScanMatcherOptions
38  common::LuaParameterDictionary* const parameter_dictionary) {
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.);
50  return options;
51 }
52 
54  const proto::RealTimeCorrelativeScanMatcherOptions& options)
55  : options_(options) {}
56 
57 std::vector<Candidate>
59  const SearchParameters& search_parameters) const {
60  int num_candidates = 0;
61  for (int scan_index = 0; scan_index != search_parameters.num_scans;
62  ++scan_index) {
63  const int num_linear_x_candidates =
64  (search_parameters.linear_bounds[scan_index].max_x -
65  search_parameters.linear_bounds[scan_index].min_x + 1);
66  const int num_linear_y_candidates =
67  (search_parameters.linear_bounds[scan_index].max_y -
68  search_parameters.linear_bounds[scan_index].min_y + 1);
69  num_candidates += num_linear_x_candidates * num_linear_y_candidates;
70  }
71  std::vector<Candidate> candidates;
72  candidates.reserve(num_candidates);
73  for (int scan_index = 0; scan_index != search_parameters.num_scans;
74  ++scan_index) {
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;
77  ++x_index_offset) {
78  for (int y_index_offset =
79  search_parameters.linear_bounds[scan_index].min_y;
80  y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
81  ++y_index_offset) {
82  candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
83  search_parameters);
84  }
85  }
86  }
87  CHECK_EQ(candidates.size(), num_candidates);
88  return candidates;
89 }
90 
92  const transform::Rigid2d& initial_pose_estimate,
93  const sensor::PointCloud& point_cloud,
94  const ProbabilityGrid& probability_grid,
95  transform::Rigid2d* pose_estimate) const {
96  CHECK_NOTNULL(pose_estimate);
97 
98  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
99  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
100  point_cloud,
101  transform::Rigid3f::Rotation(Eigen::AngleAxisf(
102  initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
103  const SearchParameters search_parameters(
104  options_.linear_search_window(), options_.angular_search_window(),
105  rotated_point_cloud, probability_grid.limits().resolution());
106 
107  const std::vector<sensor::PointCloud> rotated_scans =
108  GenerateRotatedScans(rotated_point_cloud, search_parameters);
109  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
110  probability_grid.limits(), rotated_scans,
111  Eigen::Translation2f(initial_pose_estimate.translation().x(),
112  initial_pose_estimate.translation().y()));
113  std::vector<Candidate> candidates =
114  GenerateExhaustiveSearchCandidates(search_parameters);
115  ScoreCandidates(probability_grid, discrete_scans, search_parameters,
116  &candidates);
117 
118  const Candidate& best_candidate =
119  *std::max_element(candidates.begin(), candidates.end());
120  *pose_estimate = transform::Rigid2d(
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;
125 }
126 
128  const ProbabilityGrid& probability_grid,
129  const std::vector<DiscreteScan>& discrete_scans,
130  const SearchParameters& search_parameters,
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 =
140  probability_grid.GetProbability(proposed_xy_index);
141  candidate.score += probability;
142  }
143  candidate.score /=
144  static_cast<float>(discrete_scans[candidate.scan_index].size());
145  candidate.score *=
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);
151  }
152 }
153 
154 } // namespace scan_matching
155 } // namespace mapping_2d
156 } // namespace cartographer
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
const Vector & translation() const
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
static Rigid3 Rotation(const AngleAxis &angle_axis)
constexpr T Pow2(T a)
Definition: math.h:50
Rigid2< double > Rigid2d
float GetProbability(const Eigen::Array2i &xy_index) const
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
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)


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39