ray_casting.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 namespace cartographer {
20 namespace mapping_2d {
21 
22 namespace {
23 
24 // Factor for subpixel accuracy of start and end point.
25 constexpr int kSubpixelScale = 1000;
26 
27 // We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
28 // and 'end' are coordinates at subpixel precision. We compute all pixels in
29 // which some part of the line segment connecting 'begin' and 'end' lies.
30 void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
31  const std::function<void(const Eigen::Array2i&)>& visitor) {
32  // For simplicity, we order 'begin' and 'end' by their x coordinate.
33  if (begin.x() > end.x()) {
34  CastRay(end, begin, visitor);
35  return;
36  }
37 
38  CHECK_GE(begin.x(), 0);
39  CHECK_GE(begin.y(), 0);
40  CHECK_GE(end.y(), 0);
41 
42  // Special case: We have to draw a vertical line in full pixels, as 'begin'
43  // and 'end' have the same full pixel x coordinate.
44  if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
45  Eigen::Array2i current(begin.x() / kSubpixelScale,
46  std::min(begin.y(), end.y()) / kSubpixelScale);
47  const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
48  for (; current.y() <= end_y; ++current.y()) {
49  visitor(current);
50  }
51  return;
52  }
53 
54  const int64 dx = end.x() - begin.x();
55  const int64 dy = end.y() - begin.y();
56  const int64 denominator = 2 * kSubpixelScale * dx;
57 
58  // The current full pixel coordinates. We begin at 'begin'.
59  Eigen::Array2i current = begin / kSubpixelScale;
60 
61  // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
62  // the denominator.
63  // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
64  // | | | |
65  // +-+-+-+
66  // | | | |
67  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
68  // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
69  // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)
70 
71  // The center of the subpixel part of 'begin.y()' assuming the
72  // 'denominator', i.e., sub_y / denominator is in (0, 1).
73  int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;
74 
75  // The distance from the from 'begin' to the right pixel border, to be divided
76  // by 2 * 'kSubpixelScale'.
77  const int first_pixel =
78  2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
79  // The same from the left pixel border to 'end'.
80  const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;
81 
82  // The full pixel x coordinate of 'end'.
83  const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;
84 
85  // Move from 'begin' to the next pixel border to the right.
86  sub_y += dy * first_pixel;
87  if (dy > 0) {
88  while (true) {
89  visitor(current);
90  while (sub_y > denominator) {
91  sub_y -= denominator;
92  ++current.y();
93  visitor(current);
94  }
95  ++current.x();
96  if (sub_y == denominator) {
97  sub_y -= denominator;
98  ++current.y();
99  }
100  if (current.x() == end_x) {
101  break;
102  }
103  // Move from one pixel border to the next.
104  sub_y += dy * 2 * kSubpixelScale;
105  }
106  // Move from the pixel border on the right to 'end'.
107  sub_y += dy * last_pixel;
108  visitor(current);
109  while (sub_y > denominator) {
110  sub_y -= denominator;
111  ++current.y();
112  visitor(current);
113  }
114  CHECK_NE(sub_y, denominator);
115  CHECK_EQ(current.y(), end.y() / kSubpixelScale);
116  return;
117  }
118 
119  // Same for lines non-ascending in y coordinates.
120  while (true) {
121  visitor(current);
122  while (sub_y < 0) {
123  sub_y += denominator;
124  --current.y();
125  visitor(current);
126  }
127  ++current.x();
128  if (sub_y == 0) {
129  sub_y += denominator;
130  --current.y();
131  }
132  if (current.x() == end_x) {
133  break;
134  }
135  sub_y += dy * 2 * kSubpixelScale;
136  }
137  sub_y += dy * last_pixel;
138  visitor(current);
139  while (sub_y < 0) {
140  sub_y += denominator;
141  --current.y();
142  visitor(current);
143  }
144  CHECK_NE(sub_y, 0);
145  CHECK_EQ(current.y(), end.y() / kSubpixelScale);
146 }
147 
148 } // namespace
149 
150 void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
151  const std::function<void(const Eigen::Array2i&)>& hit_visitor,
152  const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
153  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
154  const MapLimits superscaled_limits(
155  superscaled_resolution, limits.max(),
156  CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
157  limits.cell_limits().num_y_cells * kSubpixelScale));
158  const Eigen::Array2i begin =
159  superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(),
160  range_data.origin.y());
161 
162  // Compute and add the end points.
163  std::vector<Eigen::Array2i> ends;
164  ends.reserve(range_data.returns.size());
165  for (const Eigen::Vector3f& hit : range_data.returns) {
166  ends.push_back(
167  superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y()));
168  hit_visitor(ends.back() / kSubpixelScale);
169  }
170 
171  // Now add the misses.
172  for (const Eigen::Array2i& end : ends) {
173  CastRay(begin, end, miss_visitor);
174  }
175 
176  // Finally, compute and add empty rays based on misses in the scan.
177  for (const Eigen::Vector3f& missing_echo : range_data.misses) {
178  CastRay(begin,
179  superscaled_limits.GetXYIndexOfCellContainingPoint(
180  missing_echo.x(), missing_echo.y()),
181  miss_visitor);
182  }
183 }
184 
185 } // namespace mapping_2d
186 } // namespace cartographer
const Eigen::Vector2d & max() const
Definition: map_limits.h:61
Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x, const double y) const
Definition: map_limits.h:69
const CellLimits & cell_limits() const
Definition: map_limits.h:64
bool hit
Definition: 3d/submaps.cc:36
int64_t int64
Definition: port.h:31
void CastRays(const sensor::RangeData &range_data, const MapLimits &limits, const std::function< void(const Eigen::Array2i &)> &hit_visitor, const std::function< void(const Eigen::Array2i &)> &miss_visitor)
Definition: ray_casting.cc:150


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