Branch data Line data Source code
1 : : // Copyright 2017-2019 the Autoware Foundation
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 : : /// \file
18 : : /// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes
19 : :
20 : : #ifndef GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
21 : : #define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
22 : : #include <geometry/convex_hull.hpp>
23 : : #include <geometry/common_2d.hpp>
24 : : #include <geometry/bounding_box/bounding_box_common.hpp>
25 : :
26 : : #include <algorithm>
27 : : #include <cstring>
28 : : #include <limits>
29 : : #include <list>
30 : :
31 : : namespace autoware
32 : : {
33 : : namespace common
34 : : {
35 : : namespace geometry
36 : : {
37 : : namespace bounding_box
38 : : {
39 : : namespace details
40 : : {
41 : : /// \brief Find which (next) edge has smallest angle delta to directions, rotate directions
42 : : /// \param[inout] edges Array of edges on polygon after each anchor point (in ccw order).
43 : : /// E.g. if anchor point = p_i, edge = P[i+1] - P[i]
44 : : /// \param[inout] directions Array of directions of current bounding box (in ccw order)
45 : : /// \tparam PointT Point type of the lists, must have float members x and y
46 : : /// \return index of array to advance, e.g. the one with the smallest angle between edge and dir
47 : : template<typename PointT>
48 : 2180 : uint32_t update_angles(const Point4<PointT> & edges, Point4<PointT> & directions)
49 : : {
50 : : // find smallest angle to next
51 : 2180 : float32_t best_cos_th = -std::numeric_limits<float32_t>::max();
52 : 2180 : float32_t best_edge_dir_mag = 0.0F;
53 : 2180 : uint32_t advance_idx = 0U;
54 [ + + ]: 10900 : for (uint32_t idx = 0U; idx < edges.size(); ++idx) {
55 : 8720 : const float32_t edge_dir_mag =
56 : 8720 : std::max(
57 : 17440 : norm_2d(edges[idx]) * norm_2d(
58 : 17440 : directions[idx]), std::numeric_limits<float32_t>::epsilon());
59 : 8720 : const float32_t cos_th = dot_2d(edges[idx], directions[idx]) / edge_dir_mag;
60 [ + + ]: 8720 : if (cos_th > best_cos_th) {
61 : 4376 : best_cos_th = cos_th;
62 : 4376 : best_edge_dir_mag = edge_dir_mag;
63 : 4376 : advance_idx = idx;
64 : : }
65 : : }
66 : :
67 : : // update directions by smallest angle
68 : 2180 : const float32_t sin_th =
69 : 2180 : cross_2d(directions[advance_idx], edges[advance_idx]) / best_edge_dir_mag;
70 [ + + ]: 10900 : for (uint32_t idx = 0U; idx < edges.size(); ++idx) {
71 : 8720 : rotate_2d(directions[idx], best_cos_th, sin_th);
72 : : }
73 : :
74 : 2180 : return advance_idx;
75 : : }
76 : :
77 : : /// \brief Given support points i, find direction of edge: e = P[i+1] - P[i]
78 : : /// \tparam PointT Point type of the lists, must have float members x and y
79 : : /// \tparam IT The iterator type, should dereference to a point type with float members x and y
80 : : /// \param[in] begin An iterator pointing to the first point in a point list
81 : : /// \param[in] end An iterator pointing to one past the last point in the point list
82 : : /// \param[in] support Array of points that are most extreme in 4 directions for current OBB
83 : : /// \param[out] edges An array to be filled with the polygon edges next from support points
84 : : template<typename IT, typename PointT>
85 : 38 : void init_edges(
86 : : const IT begin,
87 : : const IT end,
88 : : const Point4<IT> & support,
89 : : Point4<PointT> & edges)
90 : : {
91 [ + + ]: 190 : for (uint32_t idx = 0U; idx < support.size(); ++idx) {
92 : 152 : auto tmp_it = support[idx];
93 : 152 : ++tmp_it;
94 [ + + ]: 152 : const PointT & q = (tmp_it == end) ? *begin : *tmp_it;
95 : 152 : edges[idx] = minus_2d(q, *support[idx]);
96 : : }
97 : 38 : }
98 : :
99 : : /// \brief Scan through list to find support points for bounding box oriented in direction of
100 : : /// u = P[1] - P[0]
101 : : /// \tparam IT The iterator type, should dereference to a point type with float members x and y
102 : : /// \param[in] begin An iterator pointing to the first point in a point list
103 : : /// \param[in] end An iterator pointing to one past the last point in the point list
104 : : /// \param[out] support Array that gets filled with pointers to points that are most extreme in
105 : : /// each direction aligned with and orthogonal to the first polygon edge.
106 : : /// Most extreme = max/min wrt u = P[1]-P[0] (in the dot/cross product sense)
107 : : template<typename IT>
108 : 38 : void init_bbox(const IT begin, const IT end, Point4<IT> & support)
109 : : {
110 : : // compute initial orientation using first two points
111 : 38 : auto pt_it = begin;
112 : 38 : ++pt_it;
113 : 38 : const auto u = minus_2d(*pt_it, *begin);
114 : 38 : support[0U] = begin;
115 : 38 : std::array<float32_t, 3U> metric{{
116 : : -std::numeric_limits<float32_t>::max(),
117 : : -std::numeric_limits<float32_t>::max(),
118 : : std::numeric_limits<float32_t>::max()
119 : : }};
120 : : // track u * p, fabsf(u x p), and -u * p
121 [ + + ]: 2218 : for (pt_it = begin; pt_it != end; ++pt_it) {
122 : : // check points against orientation for run_ptr
123 : 2180 : const auto & pt = *pt_it;
124 : : // u * p
125 : 2180 : const float32_t up = dot_2d(u, pt);
126 [ + + ]: 2180 : if (up > metric[0U]) {
127 : 604 : metric[0U] = up;
128 : 604 : support[1U] = pt_it;
129 : : }
130 : : // -u * p
131 [ + + ]: 2180 : if (up < metric[2U]) {
132 : 558 : metric[2U] = up;
133 : 558 : support[3U] = pt_it;
134 : : }
135 : : // u x p
136 : 2180 : const float32_t uxp = cross_2d(u, pt);
137 [ + + ]: 2180 : if (uxp > metric[1U]) {
138 : 1100 : metric[1U] = uxp;
139 : 1100 : support[2U] = pt_it;
140 : : }
141 : : }
142 : 38 : }
143 : : /// \brief Compute the minimum bounding box for a convex hull using the rotating calipers method.
144 : : /// This function may possibly also be used for computing the width of a convex hull, as it uses the
145 : : /// external supports of a single convex hull.
146 : : /// \param[in] begin An iterator to the first point on a convex hull
147 : : /// \param[in] end An iterator to one past the last point on a convex hull
148 : : /// \param[in] metric_fn A functor determining what measure the bounding box is minimum with respect
149 : : /// to
150 : : /// \tparam IT An iterator type dereferencable into a point type with float members x and y
151 : : /// \tparam MetricF A functor that computes a float measure from the x and y size (width and length)
152 : : /// of a bounding box, assumed to be packed in a Point32 message.
153 : : /// \throw std::domain_error if the list of points is too small to reasonable generate a bounding
154 : : /// box
155 : : template<typename IT, typename MetricF>
156 : 38 : BoundingBox rotating_calipers_impl(const IT begin, const IT end, const MetricF metric_fn)
157 : : {
158 : : using PointT = base_type<decltype(*begin)>;
159 : : // sanity check TODO(c.ho) more checks (up to n = 2?)
160 [ - + ]: 38 : if (begin == end) {
161 [ # # # # : 0 : throw std::domain_error("Malformed list, size = " + std::to_string(std::distance(begin, end)));
# # # # ]
162 : : }
163 : : // initial scan to get anchor points
164 : 38 : Point4<IT> support;
165 [ # # ]: 38 : init_bbox(begin, end, support);
166 : : // initialize directions accordingly
167 [ # # ]: 38 : Point4<PointT> directions;
168 : : { // I just don't want the temporary variable floating around
169 : 38 : auto tmp = support[0U];
170 : 38 : ++tmp;
171 : 38 : directions[0U] = minus_2d(*tmp, *support[0U]);
172 : 38 : directions[1U] = get_normal(directions[0U]);
173 : 38 : directions[2U] = minus_2d(directions[0U]);
174 : 38 : directions[3U] = minus_2d(directions[1U]);
175 : : }
176 : : // initialize edges
177 [ # # ]: 38 : Point4<PointT> edges;
178 : 38 : init_edges(begin, end, support, edges);
179 : : // size of initial guess
180 [ + - ]: 38 : BoundingBox bbox;
181 [ # # ]: 38 : decltype(BoundingBox::corners) best_corners;
182 [ + - ]: 38 : compute_corners(best_corners, support, directions);
183 [ + - ]: 38 : size_2d(best_corners, bbox.size);
184 : 38 : bbox.value = metric_fn(bbox.size);
185 : : // rotating calipers step: incrementally advance, update angles, points, compute area
186 [ + + ]: 2218 : for (auto it = begin; it != end; ++it) {
187 : : // find smallest angle to next, update directions
188 [ + - ]: 2180 : const uint32_t advance_idx = update_angles(edges, directions);
189 : : // recompute area
190 [ # # ]: 2180 : decltype(BoundingBox::corners) corners;
191 [ + - ]: 2180 : compute_corners(corners, support, directions);
192 : 2180 : decltype(BoundingBox::size) tmp_size;
193 [ + - ]: 2180 : size_2d(corners, tmp_size);
194 : 2180 : const float32_t tmp_value = metric_fn(tmp_size);
195 : : // update best if necessary
196 [ + + ]: 2180 : if (tmp_value < bbox.value) {
197 : : // corners: memcpy is fine since I know corners and best_corners are distinct
198 : 30 : (void)std::memcpy(&best_corners[0U], &corners[0U], sizeof(corners));
199 : : // size
200 : 30 : bbox.size = tmp_size;
201 : 30 : bbox.value = tmp_value;
202 : : }
203 : : // Step to next iteration of calipers
204 : : {
205 : : // update smallest support
206 : 2180 : auto next_it = support[advance_idx];
207 : 2180 : ++next_it;
208 [ + + ]: 2180 : const auto run_it = (end == next_it) ? begin : next_it;
209 : 2180 : support[advance_idx] = run_it;
210 : : // update edges
211 : 2180 : next_it = run_it;
212 : 2180 : ++next_it;
213 [ + + ]: 2180 : const PointT & next = (end == next_it) ? (*begin) : (*next_it);
214 : 2180 : edges[advance_idx] = minus_2d(next, *run_it);
215 : : }
216 : : }
217 : :
218 [ + - ]: 38 : finalize_box(best_corners, bbox);
219 : :
220 : : // TODO(christopher.ho) check if one of the sizes is too small, fuzz corner 1 and 2
221 : : // This doesn't cause any issues now, it shouldn't happen in practice, and even if it did,
222 : : // it would probably get smoothed out by prediction. But, this could cause issues with the
223 : : // collision detection algorithms (i.e GJK), but probably not.
224 : :
225 : 76 : return bbox;
226 : : }
227 : : } // namespace details
228 : :
229 : : /// \brief Compute the minimum area bounding box given a convex hull of points.
230 : : /// This function is exposed in case a user might already have a convex hull via other methods.
231 : : /// \param[in] begin An iterator to the first point on a convex hull
232 : : /// \param[in] end An iterator to one past the last point on a convex hull
233 : : /// \return A minimum area bounding box, value field is the area
234 : : /// \tparam IT An iterator type dereferencable into a point type with float members x and y
235 : : template<typename IT>
236 : 26 : BoundingBox minimum_area_bounding_box(const IT begin, const IT end)
237 : : {
238 : 2160 : const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t
239 : : {
240 : 2160 : return pt.x * pt.y;
241 : : };
242 [ + - ]: 52 : return details::rotating_calipers_impl(begin, end, metric_fn);
243 : : }
244 : :
245 : : /// \brief Compute the minimum perimeter bounding box given a convex hull of points
246 : : /// This function is exposed in case a user might already have a convex hull via other methods.
247 : : /// \param[in] begin An iterator to the first point on a convex hull
248 : : /// \param[in] end An iterator to one past the last point on a convex hull
249 : : /// \return A minimum perimeter bounding box, value field is half the perimeter
250 : : /// \tparam IT An iterator type dereferencable into a point type with float members x and y
251 : : template<typename IT>
252 : 12 : BoundingBox minimum_perimeter_bounding_box(const IT begin, const IT end)
253 : : {
254 : 58 : const auto metric_fn = [](const decltype(BoundingBox::size) & pt) -> float32_t
255 : : {
256 : 58 : return pt.x + pt.y;
257 : : };
258 [ + - ]: 24 : return details::rotating_calipers_impl(begin, end, metric_fn);
259 : : }
260 : :
261 : : /// \brief Compute the minimum area bounding box given an unstructured list of points.
262 : : /// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and
263 : : /// without memory allocation.
264 : : /// \param[inout] list A list of points to form a hull around, gets reordered
265 : : /// \return A minimum area bounding box, value field is the area
266 : : /// \tparam PointT Point type of the lists, must have float members x and y
267 : : template<typename PointT>
268 : 26 : BoundingBox minimum_area_bounding_box(std::list<PointT> & list)
269 : : {
270 [ + - ]: 26 : const auto last = convex_hull(list);
271 [ + - ]: 52 : return minimum_area_bounding_box(list.cbegin(), last);
272 : : }
273 : :
274 : : /// \brief Compute the minimum perimeter bounding box given an unstructured list of points
275 : : /// Only a list is supported as it enables the convex hull to be formed in O(n log n) time and
276 : : /// without memory allocation.
277 : : /// \param[inout] list A list of points to form a hull around, gets reordered
278 : : /// \return A minimum perimeter bounding box, value field is half the perimeter
279 : : /// \tparam PointT Point type of the lists, must have float members x and y
280 : : template<typename PointT>
281 : 12 : BoundingBox minimum_perimeter_bounding_box(std::list<PointT> & list)
282 : : {
283 [ + - ]: 12 : const auto last = convex_hull(list);
284 [ + - ]: 24 : return minimum_perimeter_bounding_box(list.cbegin(), last);
285 : : }
286 : : } // namespace bounding_box
287 : : } // namespace geometry
288 : : } // namespace common
289 : : } // namespace autoware
290 : : #endif // GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_
|
