rotational_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 <map>
20 #include <vector>
21 
23 
24 namespace cartographer {
25 namespace mapping_3d {
26 namespace scan_matching {
27 
28 namespace {
29 
30 constexpr float kMinDistance = 0.2f;
31 constexpr float kMaxDistance = 0.9f;
32 constexpr float kSliceHeight = 0.2f;
33 
34 void AddValueToHistogram(float angle, const float value,
35  Eigen::VectorXf* histogram) {
36  // Map the angle to [0, pi), i.e. a vector and its inverse are considered to
37  // represent the same angle.
38  while (angle > static_cast<float>(M_PI)) {
39  angle -= static_cast<float>(M_PI);
40  }
41  while (angle < 0.f) {
42  angle += static_cast<float>(M_PI);
43  }
44  const float zero_to_one = angle / static_cast<float>(M_PI);
45  const int bucket = common::Clamp<int>(
46  common::RoundToInt(histogram->size() * zero_to_one - 0.5f), 0,
47  histogram->size() - 1);
48  (*histogram)(bucket) += value;
49 }
50 
51 Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) {
52  CHECK(!slice.empty());
53  Eigen::Vector3f sum = Eigen::Vector3f::Zero();
54  for (const Eigen::Vector3f& point : slice) {
55  sum += point;
56  }
57  return sum / static_cast<float>(slice.size());
58 }
59 
60 struct AngleValuePair {
61  float angle;
62  float value;
63 };
64 
65 void AddPointCloudSliceToValueVector(
66  const sensor::PointCloud& slice,
67  std::vector<AngleValuePair>* value_vector) {
68  if (slice.empty()) {
69  return;
70  }
71  // We compute the angle of the ray from a point to the centroid of the whole
72  // point cloud. If it is orthogonal to the angle we compute between points, we
73  // will add the angle between points to the histogram with the maximum weight.
74  // This is to reject, e.g., the angles observed on the ceiling and floor.
75  const Eigen::Vector3f centroid = ComputeCentroid(slice);
76  Eigen::Vector3f last_point = slice.front();
77  for (const Eigen::Vector3f& point : slice) {
78  const Eigen::Vector2f delta = (point - last_point).head<2>();
79  const Eigen::Vector2f direction = (point - centroid).head<2>();
80  const float distance = delta.norm();
81  if (distance < kMinDistance || direction.norm() < kMinDistance) {
82  continue;
83  }
84  if (distance > kMaxDistance) {
85  last_point = point;
86  continue;
87  }
88  const float angle = common::atan2(delta);
89  const float value = std::max(
90  0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized())));
91  value_vector->push_back(AngleValuePair{angle, value});
92  }
93 }
94 
95 // A function to sort the points in each slice by angle around the centroid.
96 // This is because the returns from different rangefinders are interleaved in
97 // the data.
98 sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
99  struct SortableAnglePointPair {
100  bool operator<(const SortableAnglePointPair& rhs) const {
101  return angle < rhs.angle;
102  }
103 
104  float angle;
105  Eigen::Vector3f point;
106  };
107  const Eigen::Vector3f centroid = ComputeCentroid(slice);
108  std::vector<SortableAnglePointPair> by_angle;
109  by_angle.reserve(slice.size());
110  for (const Eigen::Vector3f& point : slice) {
111  const Eigen::Vector2f delta = (point - centroid).head<2>();
112  if (delta.norm() < kMinDistance) {
113  continue;
114  }
115  by_angle.push_back(SortableAnglePointPair{common::atan2(delta), point});
116  }
117  std::sort(by_angle.begin(), by_angle.end());
118  sensor::PointCloud result;
119  for (const auto& pair : by_angle) {
120  result.push_back(pair.point);
121  }
122  return result;
123 }
124 
125 std::vector<AngleValuePair> GetValuesForHistogram(
126  const sensor::PointCloud& point_cloud) {
127  std::map<int, sensor::PointCloud> slices;
128  for (const Eigen::Vector3f& point : point_cloud) {
129  slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point);
130  }
131  std::vector<AngleValuePair> result;
132  for (const auto& slice : slices) {
133  AddPointCloudSliceToValueVector(SortSlice(slice.second), &result);
134  }
135  return result;
136 }
137 
138 void AddValuesToHistogram(const std::vector<AngleValuePair>& value_vector,
139  const float rotation, Eigen::VectorXf* histogram) {
140  for (const AngleValuePair& pair : value_vector) {
141  AddValueToHistogram(pair.angle + rotation, pair.value, histogram);
142  }
143 }
144 
145 } // namespace
146 
148  const std::vector<mapping::TrajectoryNode>& nodes, const int histogram_size)
149  : histogram_(Eigen::VectorXf::Zero(histogram_size)) {
150  for (const mapping::TrajectoryNode& node : nodes) {
151  AddValuesToHistogram(
152  GetValuesForHistogram(sensor::TransformPointCloud(
153  node.constant_data->range_data_3d.returns.Decompress(),
154  node.pose.cast<float>())),
155  0.f, &histogram_);
156  }
157 }
158 
159 std::vector<float> RotationalScanMatcher::Match(
160  const sensor::PointCloud& point_cloud,
161  const std::vector<float>& angles) const {
162  std::vector<float> result;
163  result.reserve(angles.size());
164  const std::vector<AngleValuePair> value_vector =
165  GetValuesForHistogram(point_cloud);
166  for (const float angle : angles) {
167  Eigen::VectorXf scan_histogram = Eigen::VectorXf::Zero(histogram_.size());
168  AddValuesToHistogram(value_vector, angle, &scan_histogram);
169  result.push_back(MatchHistogram(scan_histogram));
170  }
171  return result;
172 }
173 
175  const Eigen::VectorXf& scan_histogram) const {
176  // We compute the dot product of normalized histograms as a measure of
177  // similarity.
178  const float scan_histogram_norm = scan_histogram.norm();
179  const float histogram_norm = histogram_.norm();
180  const float normalization = scan_histogram_norm * histogram_norm;
181  if (normalization < 1e-3f) {
182  return 1.f;
183  }
184  return histogram_.dot(scan_histogram) / normalization;
185 }
186 
187 } // namespace scan_matching
188 } // namespace mapping_3d
189 } // namespace cartographer
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
RotationalScanMatcher(const std::vector< mapping::TrajectoryNode > &nodes, int histogram_size)
int RoundToInt(const float x)
Definition: port.h:42
T atan2(const Eigen::Matrix< T, 2, 1 > &vector)
Definition: math.h:73
float angle
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
float MatchHistogram(const Eigen::VectorXf &scan_histogram) const
float value
std::vector< float > Match(const sensor::PointCloud &point_cloud, const std::vector< float > &angles) const


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58