correlative_scan_matcher_2d.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 <cmath>
20 
22 
23 namespace cartographer {
24 namespace mapping {
25 namespace scan_matching {
26 
27 SearchParameters::SearchParameters(const double linear_search_window,
28  const double angular_search_window,
29  const sensor::PointCloud& point_cloud,
30  const double resolution)
31  : resolution(resolution) {
32  // We set this value to something on the order of resolution to make sure that
33  // the std::acos() below is defined.
34  float max_scan_range = 3.f * resolution;
35  for (const Eigen::Vector3f& point : point_cloud) {
36  const float range = point.head<2>().norm();
37  max_scan_range = std::max(range, max_scan_range);
38  }
39  const double kSafetyMargin = 1. - 1e-3;
41  kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
42  (2. * common::Pow2(max_scan_range)));
44  std::ceil(angular_search_window / angular_perturbation_step_size);
46 
47  const int num_linear_perturbations =
48  std::ceil(linear_search_window / resolution);
49  linear_bounds.reserve(num_scans);
50  for (int i = 0; i != num_scans; ++i) {
51  linear_bounds.push_back(
52  LinearBounds{-num_linear_perturbations, num_linear_perturbations,
53  -num_linear_perturbations, num_linear_perturbations});
54  }
55 }
56 
57 SearchParameters::SearchParameters(const int num_linear_perturbations,
58  const int num_angular_perturbations,
59  const double angular_perturbation_step_size,
60  const double resolution)
61  : num_angular_perturbations(num_angular_perturbations),
62  angular_perturbation_step_size(angular_perturbation_step_size),
63  resolution(resolution),
64  num_scans(2 * num_angular_perturbations + 1) {
65  linear_bounds.reserve(num_scans);
66  for (int i = 0; i != num_scans; ++i) {
67  linear_bounds.push_back(
68  LinearBounds{-num_linear_perturbations, num_linear_perturbations,
69  -num_linear_perturbations, num_linear_perturbations});
70  }
71 }
72 
73 void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
74  const CellLimits& cell_limits) {
75  CHECK_EQ(scans.size(), num_scans);
76  CHECK_EQ(linear_bounds.size(), num_scans);
77  for (int i = 0; i != num_scans; ++i) {
78  Eigen::Array2i min_bound = Eigen::Array2i::Zero();
79  Eigen::Array2i max_bound = Eigen::Array2i::Zero();
80  for (const Eigen::Array2i& xy_index : scans[i]) {
81  min_bound = min_bound.min(-xy_index);
82  max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
83  cell_limits.num_y_cells - 1) -
84  xy_index);
85  }
86  linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x());
87  linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x());
88  linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y());
89  linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y());
90  }
91 }
92 
93 std::vector<sensor::PointCloud> GenerateRotatedScans(
94  const sensor::PointCloud& point_cloud,
95  const SearchParameters& search_parameters) {
96  std::vector<sensor::PointCloud> rotated_scans;
97  rotated_scans.reserve(search_parameters.num_scans);
98 
99  double delta_theta = -search_parameters.num_angular_perturbations *
100  search_parameters.angular_perturbation_step_size;
101  for (int scan_index = 0; scan_index < search_parameters.num_scans;
102  ++scan_index,
103  delta_theta += search_parameters.angular_perturbation_step_size) {
104  rotated_scans.push_back(sensor::TransformPointCloud(
105  point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
106  delta_theta, Eigen::Vector3f::UnitZ()))));
107  }
108  return rotated_scans;
109 }
110 
111 std::vector<DiscreteScan2D> DiscretizeScans(
112  const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
113  const Eigen::Translation2f& initial_translation) {
114  std::vector<DiscreteScan2D> discrete_scans;
115  discrete_scans.reserve(scans.size());
116  for (const sensor::PointCloud& scan : scans) {
117  discrete_scans.emplace_back();
118  discrete_scans.back().reserve(scan.size());
119  for (const Eigen::Vector3f& point : scan) {
120  const Eigen::Vector2f translated_point =
121  Eigen::Affine2f(initial_translation) * point.head<2>();
122  discrete_scans.back().push_back(
123  map_limits.GetCellIndex(translated_point));
124  }
125  }
126  return discrete_scans;
127 }
128 
129 } // namespace scan_matching
130 } // namespace mapping
131 } // namespace cartographer
std::vector< DiscreteScan2D > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
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
void ShrinkToFit(const std::vector< DiscreteScan2D > &scans, const CellLimits &cell_limits)
SearchParameters(double linear_search_window, double angular_search_window, const sensor::PointCloud &point_cloud, double resolution)
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:32
Eigen::Array2i GetCellIndex(const Eigen::Vector2f &point) const
Definition: map_limits.h:69


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58