Branch data Line data Source code
1 : : // Copyright 2020 Embotech AG, Zurich, Switzerland
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : :
17 : : #ifndef GEOMETRY__INTERSECTION_HPP_
18 : : #define GEOMETRY__INTERSECTION_HPP_
19 : :
20 : :
21 : : #include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>
22 : : #include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
23 : : #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
24 : : #include <geometry/convex_hull.hpp>
25 : : #include <geometry/common_2d.hpp>
26 : :
27 : : #include <limits>
28 : : #include <vector>
29 : : #include <iostream>
30 : : #include <list>
31 : : #include <utility>
32 : : #include <type_traits>
33 : : #include <algorithm>
34 : :
35 : : namespace autoware
36 : : {
37 : : namespace common
38 : : {
39 : : namespace geometry
40 : : {
41 : : using autoware_auto_planning_msgs::msg::TrajectoryPoint;
42 : : using autoware_auto_perception_msgs::msg::BoundingBox;
43 : : using autoware::common::geometry::convex_hull;
44 : : using autoware::common::geometry::get_normal;
45 : : using autoware::common::geometry::dot_2d;
46 : : using autoware::common::geometry::minus_2d;
47 : : using autoware::common::geometry::times_2d;
48 : : using autoware::common::geometry::norm_2d;
49 : : using autoware::common::geometry::closest_line_point_2d;
50 : :
51 : : using Point = geometry_msgs::msg::Point32;
52 : :
53 : : namespace details
54 : : {
55 : :
56 : : /// Alias for a std::pair of two points
57 : : using Line = std::pair<Point, Point>;
58 : :
59 : : /// \tparam Iter1 Iterator over point-types that must have point adapters
60 : : // defined or have float members x and y
61 : : /// \brief Compute a sorted list of faces of a polyhedron given a list of points
62 : : /// \param[in] start Start iterator of the list of points
63 : : /// \param[in] end End iterator of the list of points
64 : : /// \return The list of faces
65 : : template<typename Iter>
66 : : std::vector<Line> get_sorted_face_list(const Iter start, const Iter end)
67 : : {
68 : : // First get a sorted list of points - convex_hull does that by modifying its argument
69 : : auto corner_list = std::list<Point>(start, end);
70 : : const auto first_interior_point = convex_hull(corner_list);
71 : :
72 : : std::vector<Line> face_list{};
73 : : auto itLast = corner_list.begin();
74 : : auto itNext = std::next(itLast, 1);
75 : : do {
76 : : face_list.emplace_back(Line{*itLast, *itNext});
77 : : itLast = itNext;
78 : : itNext = std::next(itNext, 1);
79 : : } while ((itNext != first_interior_point) && (itNext != corner_list.end()));
80 : :
81 : : face_list.emplace_back(Line{*itLast, corner_list.front()});
82 : :
83 : : return face_list;
84 : : }
85 : :
86 : : /// \brief Append points of the polygon `internal` that are contained in the polygon `exernal`.
87 : : template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
88 : : typename PointT>
89 : 18 : void append_contained_points(
90 : : const Iterable1T<PointT> & external,
91 : : const Iterable2T<PointT> & internal,
92 : : std::list<PointT> & result)
93 : : {
94 : 18 : std::copy_if(
95 : : internal.begin(), internal.end(), std::back_inserter(result),
96 : 128 : [&external](const auto & pt) {
97 [ + - ]: 64 : return common::geometry::is_point_inside_polygon_2d(external.begin(), external.end(), pt);
98 : : });
99 : 18 : }
100 : :
101 : : /// \brief Append the intersecting points between two polygons into the output list.
102 : : template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
103 : : typename PointT>
104 : 9 : void append_intersection_points(
105 : : const Iterable1T<PointT> & polygon1,
106 : : const Iterable2T<PointT> & polygon2,
107 : : std::list<PointT> & result)
108 : : {
109 : : using FloatT = decltype(point_adapter::x_(std::declval<PointT>()));
110 : : using Interval = common::geometry::Interval<float32_t>;
111 : :
112 : 162 : auto get_edge = [](const auto & list, const auto & iterator) {
113 [ + - ]: 162 : const auto next_it = std::next(iterator);
114 [ + + ]: 162 : const auto & next_pt = (next_it != list.end()) ? *next_it : list.front();
115 [ + - ]: 324 : return std::make_pair(*iterator, next_pt);
116 : : };
117 : :
118 : : // Get the max absolute value out of two intervals and a scalar.
119 : 182 : auto compute_eps_scale = [](const auto & i1, const auto & i2, const auto val) {
120 : 364 : auto get_abs_max = [](const auto & interval) {
121 : 364 : return std::max(std::fabs(Interval::min(interval)), std::fabs(Interval::max(interval)));
122 : : };
123 [ + - + - ]: 182 : return std::max(std::fabs(val), std::max(get_abs_max(i1), get_abs_max(i2)));
124 : : };
125 : :
126 : : // Compare each edge from polygon1 to each edge from polygon2
127 [ + + ]: 43 : for (auto corner1_it = polygon1.begin(); corner1_it != polygon1.end(); ++corner1_it) {
128 [ + - ]: 34 : const auto & edge1 = get_edge(polygon1, corner1_it);
129 : :
130 [ + - ]: 34 : Interval edge1_x_interval{
131 : 34 : std::min(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second)),
132 : 34 : std::max(point_adapter::x_(edge1.first), point_adapter::x_(edge1.second))};
133 : :
134 [ + - ]: 34 : Interval edge1_y_interval{
135 : 34 : std::min(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second)),
136 : 34 : std::max(point_adapter::y_(edge1.first), point_adapter::y_(edge1.second))};
137 : :
138 [ + + ]: 162 : for (auto corner2_it = polygon2.begin(); corner2_it != polygon2.end(); ++corner2_it) {
139 : : try {
140 [ + - ]: 128 : const auto & edge2 = get_edge(polygon2, corner2_it);
141 : 91 : const auto & intersection =
142 : 128 : common::geometry::intersection_2d(
143 [ + + ]: 128 : edge1.first, minus_2d(edge1.second, edge1.first),
144 : 165 : edge2.first, minus_2d(edge2.second, edge2.first));
145 : :
146 [ + - ]: 91 : Interval edge2_x_interval{
147 : 91 : std::min(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second)),
148 : 91 : std::max(point_adapter::x_(edge2.first), point_adapter::x_(edge2.second))};
149 : :
150 [ + - ]: 91 : Interval edge2_y_interval{
151 : 91 : std::min(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second)),
152 : 91 : std::max(point_adapter::y_(edge2.first), point_adapter::y_(edge2.second))};
153 : :
154 : : // The accumulated floating point error depends on the magnitudes of each end of the
155 : : // intervals. Hence the upper bound of the absolute magnitude should be taken into account
156 : : // while computing the epsilon.
157 : 182 : const auto max_feps_scale = std::max(
158 [ + - ]: 91 : compute_eps_scale(edge1_x_interval, edge2_x_interval, point_adapter::x_(intersection)),
159 [ + - ]: 91 : compute_eps_scale(edge1_y_interval, edge2_y_interval, point_adapter::y_(intersection))
160 : : );
161 : 91 : const auto feps = max_feps_scale * std::numeric_limits<FloatT>::epsilon();
162 : : // Only accept intersections that lie on both of the line segments (edges)
163 [ + - ]: 91 : if (Interval::contains(edge1_x_interval, point_adapter::x_(intersection), feps) &&
164 [ + - + + ]: 54 : Interval::contains(edge2_x_interval, point_adapter::x_(intersection), feps) &&
165 [ + + + - : 168 : Interval::contains(edge1_y_interval, point_adapter::y_(intersection), feps) &&
+ + ]
166 [ + - + + : 114 : Interval::contains(edge2_y_interval, point_adapter::y_(intersection), feps))
+ + ]
167 : : {
168 [ + - ]: 10 : result.push_back(intersection);
169 : : }
170 : 74 : } catch (const std::runtime_error &) {
171 : : // Parallel lines. TODO(yunus.caliskan): #1229
172 : 37 : continue;
173 : : }
174 : : }
175 : : }
176 : 9 : }
177 : :
178 : :
179 : : } // namespace details
180 : :
181 : : // TODO(s.me) implement GJK(+EPA) algorithm as well as per Chris Ho's suggestion
182 : : /// \tparam Iter Iterator over point-types that must have point adapters
183 : : // defined or have float members x and y
184 : : /// \brief Check if polyhedra defined by two given sets of points intersect
185 : : // This uses SAT for doing the actual checking
186 : : // (https://en.wikipedia.org/wiki/Hyperplane_separation_theorem#Use_in_collision_detection)
187 : : /// \param[in] begin1 Start iterator to first list of point types
188 : : /// \param[in] end1 End iterator to first list of point types
189 : : /// \param[in] begin2 Start iterator to first list of point types
190 : : /// \param[in] end2 End iterator to first list of point types
191 : : /// \return true if the boxes collide, false otherwise.
192 : : template<typename Iter>
193 : : bool intersect(const Iter begin1, const Iter end1, const Iter begin2, const Iter end2)
194 : : {
195 : : // Obtain sorted lists of faces of both boxes, merge them into one big list of faces
196 : : auto faces = details::get_sorted_face_list(begin1, end1);
197 : : const auto faces_2 = details::get_sorted_face_list(begin2, end2);
198 : : faces.reserve(faces.size() + faces_2.size());
199 : : faces.insert(faces.end(), faces_2.begin(), faces_2.end() );
200 : :
201 : : // Also look at last line
202 : : for (const auto & face : faces) {
203 : : // Compute normal vector to the face and define a closure to get progress along it
204 : : const auto normal = get_normal(minus_2d(face.second, face.first));
205 : : auto get_position_along_line = [&normal](auto point)
206 : : {
207 : : return dot_2d(normal, minus_2d(point, Point{}) );
208 : : };
209 : :
210 : : // Define a function to get the minimum and maximum projected position of the corners
211 : : // of a given bounding box along the normal line of the face
212 : : auto get_projected_min_max = [&get_position_along_line, &normal](Iter begin, Iter end)
213 : : {
214 : : const auto zero_point = Point{};
215 : : auto min_corners =
216 : : get_position_along_line(closest_line_point_2d(normal, zero_point, *begin));
217 : : auto max_corners = min_corners;
218 : :
219 : : for (auto & point = begin; point != end; ++point) {
220 : : const auto point_projected = closest_line_point_2d(normal, zero_point, *point);
221 : : const auto position_along_line = get_position_along_line(point_projected);
222 : : min_corners = std::min(min_corners, position_along_line);
223 : : max_corners = std::max(max_corners, position_along_line);
224 : : }
225 : : return std::pair<float, float>{min_corners, max_corners};
226 : : };
227 : :
228 : : // Perform the actual computations for the extent computation
229 : : auto minmax_1 = get_projected_min_max(begin1, end1);
230 : : auto minmax_2 = get_projected_min_max(begin2, end2);
231 : :
232 : : // Check for any intersections
233 : : const auto eps = std::numeric_limits<decltype(minmax_1.first)>::epsilon();
234 : : if (minmax_1.first > minmax_2.second + eps || minmax_2.first > minmax_1.second + eps) {
235 : : // Found separating hyperplane, stop
236 : : return false;
237 : : }
238 : : }
239 : :
240 : : // No separating hyperplane found, boxes collide
241 : : return true;
242 : : }
243 : :
244 : : /// \brief Get the intersection between two polygons. The polygons should be provided in an
245 : : /// identical format to the output of `convex_hull` function as in the corners should be ordered
246 : : /// in a CCW fashion.
247 : : /// The computation is done by:
248 : : /// * Find the corners of each polygon that are contained by the other polygon.
249 : : /// * Find the intersection points between two polygons
250 : : /// * Combine these points and order CCW to get the final polygon.
251 : : /// The criteria for intersection is better explained in:
252 : : /// "Area of intersection of arbitrary polygons" (Livermore, Calif, 1977)
253 : : /// See https://www.osti.gov/servlets/purl/7309916/, chapter II - B
254 : : /// TODO(yunus.caliskan): This is a naive implementation. We should scan for a more efficient
255 : : /// algorithm: #1230
256 : : /// \tparam Iterable1T A container class that has stl style iterators defined.
257 : : /// \tparam Iterable2T A container class that has stl style iterators defined.
258 : : /// \tparam PointT Point type that have the adapters for the x and y fields.
259 : : /// set to `Point1T`
260 : : /// \param polygon1 A convex polygon
261 : : /// \param polygon2 A convex polygon
262 : : /// \return The resulting conv
263 : : template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
264 : : typename PointT>
265 : 9 : std::list<PointT> convex_polygon_intersection2d(
266 : : const Iterable1T<PointT> & polygon1,
267 : : const Iterable2T<PointT> & polygon2)
268 : : {
269 : 9 : std::list<PointT> result;
270 [ + - ]: 9 : details::append_contained_points(polygon1, polygon2, result);
271 [ + - ]: 9 : details::append_contained_points(polygon2, polygon1, result);
272 [ + - ]: 9 : details::append_intersection_points(polygon1, polygon2, result);
273 [ + - ]: 9 : const auto end_it = common::geometry::convex_hull(result);
274 [ + - + - ]: 9 : result.resize(static_cast<uint32_t>(std::distance(result.cbegin(), end_it)));
275 : 18 : return result;
276 : : }
277 : :
278 : :
279 : : /// \brief Compute the intersection over union of two 2d convex polygons. If any of the polygons
280 : : /// span a zero area, the result is 0.0.
281 : : /// \tparam Iterable1T A container class that has stl style iterators defined.
282 : : /// \tparam Iterable2T A container class that has stl style iterators defined.
283 : : /// \tparam Point1T Point type that have the adapters for the x and y fields.
284 : : /// \tparam Point2T Point type that have the adapters for the x and y fields.
285 : : /// \param polygon1 A convex polygon
286 : : /// \param polygon2 A convex polygon
287 : : /// \return (Intersection / Union) between two given polygons.
288 : : /// \throws std::domain_error If there is any inconsistency on the undderlying geometrical
289 : : /// computation.
290 : : template<template<typename ...> class Iterable1T, template<typename ...> class Iterable2T,
291 : : typename PointT>
292 : 2 : common::types::float32_t convex_intersection_over_union_2d(
293 : : const Iterable1T<PointT> & polygon1,
294 : : const Iterable2T<PointT> & polygon2
295 : : )
296 : : {
297 : 2 : constexpr auto eps = std::numeric_limits<float32_t>::epsilon();
298 [ + - ]: 4 : const auto intersection = convex_polygon_intersection2d(polygon1, polygon2);
299 : :
300 : : const auto intersection_area =
301 : 2 : common::geometry::area_2d(intersection.begin(), intersection.end());
302 : :
303 [ + + ]: 2 : if (intersection_area < eps) {
304 : 1 : return 0.0F; // There's either no intersection or the points are collinear
305 : : }
306 : :
307 : : const auto polygon1_area =
308 : 1 : common::geometry::area_2d(polygon1.begin(), polygon1.end());
309 : : const auto polygon2_area =
310 : 1 : common::geometry::area_2d(polygon2.begin(), polygon2.end());
311 : :
312 : 1 : const auto union_area = polygon1_area + polygon2_area - intersection_area;
313 [ - + ]: 1 : if (union_area < eps) {
314 [ # # ]: 0 : throw std::domain_error("IoU is undefined for polygons with a zero union area");
315 : : }
316 : :
317 : 1 : return intersection_area / union_area;
318 : : }
319 : :
320 : : } // namespace geometry
321 : : } // namespace common
322 : : } // namespace autoware
323 : :
324 : : #endif // GEOMETRY__INTERSECTION_HPP_
|
