correlative_scan_matcher_2d.h
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 
17 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
18 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
19 
20 #include <vector>
21 
22 #include "Eigen/Core"
27 
28 namespace cartographer {
29 namespace mapping {
30 namespace scan_matching {
31 
32 typedef std::vector<Eigen::Array2i> DiscreteScan2D;
33 
34 // Describes the search space.
36  // Linear search window in pixel offsets; bounds are inclusive.
37  struct LinearBounds {
38  int min_x;
39  int max_x;
40  int min_y;
41  int max_y;
42  };
43 
44  SearchParameters(double linear_search_window, double angular_search_window,
45  const sensor::PointCloud& point_cloud, double resolution);
46 
47  // For testing.
48  SearchParameters(int num_linear_perturbations, int num_angular_perturbations,
50 
51  // Tightens the search window as much as possible.
52  void ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
53  const CellLimits& cell_limits);
54 
57  double resolution;
58  int num_scans;
59  std::vector<LinearBounds> linear_bounds; // Per rotated scans.
60 };
61 
62 // Generates a collection of rotated scans.
63 std::vector<sensor::PointCloud> GenerateRotatedScans(
64  const sensor::PointCloud& point_cloud,
65  const SearchParameters& search_parameters);
66 
67 // Translates and discretizes the rotated scans into a vector of integer
68 // indices.
69 std::vector<DiscreteScan2D> DiscretizeScans(
70  const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
71  const Eigen::Translation2f& initial_translation);
72 
73 // A possible solution.
74 struct Candidate2D {
75  Candidate2D(const int init_scan_index, const int init_x_index_offset,
76  const int init_y_index_offset,
77  const SearchParameters& search_parameters)
78  : scan_index(init_scan_index),
79  x_index_offset(init_x_index_offset),
80  y_index_offset(init_y_index_offset),
81  x(-y_index_offset * search_parameters.resolution),
82  y(-x_index_offset * search_parameters.resolution),
83  orientation((scan_index - search_parameters.num_angular_perturbations) *
84  search_parameters.angular_perturbation_step_size) {}
85 
86  // Index into the rotated scans vector.
87  int scan_index = 0;
88 
89  // Linear offset from the initial pose.
90  int x_index_offset = 0;
91  int y_index_offset = 0;
92 
93  // Pose of this Candidate2D relative to the initial pose.
94  double x = 0.;
95  double y = 0.;
96  double orientation = 0.;
97 
98  // Score, higher is better.
99  float score = 0.f;
100 
101  bool operator<(const Candidate2D& other) const { return score < other.score; }
102  bool operator>(const Candidate2D& other) const { return score > other.score; }
103 };
104 
105 } // namespace scan_matching
106 } // namespace mapping
107 } // namespace cartographer
108 
109 #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_
std::vector< Eigen::Array2i > DiscreteScan2D
std::vector< DiscreteScan2D > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
void ShrinkToFit(const std::vector< DiscreteScan2D > &scans, const CellLimits &cell_limits)
Candidate2D(const int init_scan_index, const int init_x_index_offset, const int init_y_index_offset, const SearchParameters &search_parameters)
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


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