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 2D pca on a linked list of points to estimate an oriented
19 : : /// bounding box
20 : :
21 : : #ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
22 : : #define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
23 : :
24 : : #include <geometry/bounding_box/bounding_box_common.hpp>
25 : : #include <limits>
26 : : #include <utility>
27 : :
28 : : namespace autoware
29 : : {
30 : : namespace common
31 : : {
32 : : namespace geometry
33 : : {
34 : : namespace bounding_box
35 : : {
36 : : namespace details
37 : : {
38 : :
39 : : /// \brief Simplified 2d covariance matrix
40 : : struct Covariance2d
41 : : {
42 : : /// \brief Variance in the x direction
43 : : float32_t xx;
44 : : /// \brief Variance in the y direction
45 : : float32_t yy;
46 : : /// \brief x-y covariance
47 : : float32_t xy;
48 : : /// \brief Number of points
49 : : std::size_t num_points;
50 : : }; // struct Covariance2d
51 : :
52 : : /// \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm
53 : : /// \param[in] begin An iterator pointing to the first point in a point list
54 : : /// \param[in] end An iterator pointing to one past the last point in the point list
55 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
56 : : /// \return A 2d covariance matrix for all points inthe list
57 : : template<typename IT>
58 : 16 : Covariance2d covariance_2d(const IT begin, const IT end)
59 : : {
60 : 16 : Covariance2d ret{0.0F, 0.0F, 0.0F, 0U};
61 : 16 : float32_t ux = 0.0F;
62 : 16 : float32_t uy = 0.0F;
63 : 16 : float32_t num_points = 0.0F;
64 : : using point_adapter::x_;
65 : : using point_adapter::y_;
66 [ + + ]: 168 : for (auto it = begin; it != end; ++it) {
67 : 152 : ++ret.num_points;
68 : 152 : num_points = static_cast<float32_t>(ret.num_points);
69 : 152 : const auto & pt = *it;
70 : : // update mean x
71 : 152 : const float32_t dx = x_(pt) - ux;
72 : 152 : ux = ux + (dx / num_points);
73 : : // update cov
74 : 152 : const float32_t dy = y_(pt) - uy;
75 : 152 : ret.xy += (x_(pt) - ux) * (dy);
76 : : // update mean y
77 : 152 : uy = uy + (dy / num_points);
78 : : // update M2
79 : 152 : ret.xx += dx * (x_(pt) - ux);
80 : 152 : ret.yy += dy * (y_(pt) - uy);
81 : : }
82 : : // finalize sample (co-)variance
83 [ + - ]: 16 : if (ret.num_points > 1U) {
84 : 16 : num_points = num_points - 1.0F;
85 : : }
86 : 16 : ret.xx /= num_points;
87 : 16 : ret.yy /= num_points;
88 : 16 : ret.xy /= num_points;
89 : :
90 : 16 : return ret;
91 : : }
92 : :
93 : : /// \brief Compute eigenvectors and eigenvalues
94 : : /// \param[in] cov 2d Covariance matrix
95 : : /// \param[out] eigvec1 First eigenvector
96 : : /// \param[out] eigvec2 Second eigenvector
97 : : /// \tparam PointT Point type that has at least float members x and y
98 : : /// \return A pairt of eigenvalues: The first is the larger eigenvalue
99 : : /// \throw std::runtime error if you would get degenerate covariance
100 : : template<typename PointT>
101 : 132 : std::pair<float32_t, float32_t> eig_2d(
102 : : const Covariance2d & cov, PointT & eigvec1, PointT & eigvec2)
103 : : {
104 : 132 : const float32_t tr_2 = (cov.xx + cov.yy) * 0.5F;
105 : 132 : const float32_t det = (cov.xx * cov.yy) - (cov.xy * cov.xy);
106 : : // Add a small fudge to alleviate floating point errors
107 : 132 : float32_t disc = ((tr_2 * tr_2) - det) + std::numeric_limits<float32_t>::epsilon();
108 [ - + ]: 132 : if (disc < 0.0F) {
109 : : throw std::runtime_error(
110 : : "pca_2d: negative discriminant! Should never happen for well formed "
111 [ # # ]: 0 : "covariance matrix");
112 : : }
113 : 132 : disc = sqrtf(disc);
114 : : // compute eigenvalues
115 [ + - ]: 132 : const auto ret = std::make_pair(tr_2 + disc, tr_2 - disc);
116 : : // compute eigenvectors
117 : : using point_adapter::xr_;
118 : : using point_adapter::yr_;
119 : : // We compare squared value against floating epsilon to make sure that eigen vectors
120 : : // are persistent against further calculations.
121 : : // (e.g. taking cross product of two eigen vectors)
122 [ + + ]: 132 : if (fabsf(cov.xy * cov.xy) > std::numeric_limits<float32_t>::epsilon()) {
123 : 92 : xr_(eigvec1) = cov.xy;
124 : 92 : yr_(eigvec1) = ret.first - cov.xx;
125 : 92 : xr_(eigvec2) = cov.xy;
126 : 92 : yr_(eigvec2) = ret.second - cov.xx;
127 : : } else {
128 [ + + ]: 40 : if (cov.xx > cov.yy) {
129 : 14 : xr_(eigvec1) = 1.0F;
130 : 14 : yr_(eigvec1) = 0.0F;
131 : 14 : xr_(eigvec2) = 0.0F;
132 : 14 : yr_(eigvec2) = 1.0F;
133 : : } else {
134 : 26 : xr_(eigvec1) = 0.0F;
135 : 26 : yr_(eigvec1) = 1.0F;
136 : 26 : xr_(eigvec2) = 1.0F;
137 : 26 : yr_(eigvec2) = 0.0F;
138 : : }
139 : : }
140 : 132 : return ret;
141 : : }
142 : :
143 : :
144 : : /// \brief Given eigenvectors, compute support (furthest) point in each direction
145 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
146 : : /// \tparam PointT type of a point with float members x and y
147 : : /// \param[in] begin An iterator pointing to the first point in a point list
148 : : /// \param[in] end An iterator pointing to one past the last point in the point list
149 : : /// \param[in] eig1 First principal component of cluster
150 : : /// \param[in] eig2 Second principal component of cluster
151 : : /// \param[out] support Array to get filled with extreme points in each of the principal
152 : : /// components
153 : : /// \return whether or not eig2 is ccw wrt eig1
154 : : template<typename IT, typename PointT>
155 : 16 : bool8_t compute_supports(
156 : : const IT begin,
157 : : const IT end,
158 : : const PointT & eig1,
159 : : const PointT & eig2,
160 : : Point4<IT> & support)
161 : : {
162 : 16 : const bool8_t ret = cross_2d(eig1, eig2) >= 0.0F;
163 : 16 : std::array<float32_t, 4U> metrics{{
164 : : -std::numeric_limits<float32_t>::max(),
165 : : -std::numeric_limits<float32_t>::max(),
166 : : std::numeric_limits<float32_t>::max(),
167 : : std::numeric_limits<float32_t>::max()
168 : : }};
169 [ + + ]: 168 : for (auto it = begin; it != end; ++it) {
170 : 152 : const PointT & pt = *it;
171 [ + + ]: 152 : float32_t val = dot_2d(ret ? eig1 : eig2, pt);
172 [ + + ]: 152 : if (val > metrics[0U]) {
173 : 70 : metrics[0U] = val;
174 : 70 : support[0U] = it;
175 : : }
176 [ + + ]: 152 : if (val < metrics[2U]) {
177 : 58 : metrics[2U] = val;
178 : 58 : support[2U] = it;
179 : : }
180 [ + + ]: 152 : val = dot_2d(ret ? eig2 : eig1, pt);
181 [ + + ]: 152 : if (val > metrics[1U]) {
182 : 32 : metrics[1U] = val;
183 : 32 : support[1U] = it;
184 : : }
185 [ + + ]: 152 : if (val < metrics[3U]) {
186 : 64 : metrics[3U] = val;
187 : 64 : support[3U] = it;
188 : : }
189 : : }
190 : 16 : return ret;
191 : : }
192 : :
193 : : /// \brief Compute bounding box given a pair of basis directions
194 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
195 : : /// \tparam PointT Point type of the lists, must have float members x and y
196 : : /// \param[in] ax1 First basis direction, assumed to be normal to ax2
197 : : /// \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1
198 : : /// \param[in] supports Array of iterators referring to points that are most extreme in each basis
199 : : /// direction. Should be result of compute_supports function
200 : : /// \return A bounding box based on the basis axes and the support points
201 : : template<typename IT, typename PointT>
202 : 16 : BoundingBox compute_bounding_box(
203 : : const PointT & ax1,
204 : : const PointT & ax2,
205 : : const Point4<IT> & supports)
206 : : {
207 [ # # ]: 16 : decltype(BoundingBox::corners) corners;
208 : 16 : const Point4<PointT> directions{{ax1, ax2, minus_2d(ax1), minus_2d(ax2)}};
209 [ + - ]: 16 : compute_corners(corners, supports, directions);
210 : :
211 : : // build box
212 [ + - ]: 16 : BoundingBox bbox;
213 [ + - ]: 16 : finalize_box(corners, bbox);
214 [ + - ]: 16 : size_2d(corners, bbox.size);
215 : 32 : return bbox;
216 : : }
217 : : } // namespace details
218 : :
219 : : /// \brief Compute oriented bounding box using PCA. This uses all points in a list, and does not
220 : : /// modify the list. The resulting bounding box is not necessarily minimum in any way
221 : : /// \param[in] begin An iterator pointing to the first point in a point list
222 : : /// \param[in] end An iterator pointing to one past the last point in the point list
223 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
224 : : /// \return An oriented bounding box in x-y. This bounding box has no height information
225 : : template<typename IT>
226 : 4 : BoundingBox eigenbox_2d(const IT begin, const IT end)
227 : : {
228 : : // compute covariance
229 : 4 : const details::Covariance2d cov = details::covariance_2d(begin, end);
230 : :
231 : : // compute eigenvectors
232 : : using PointT = details::base_type<decltype(*begin)>;
233 : 4 : PointT eig1;
234 : 4 : PointT eig2;
235 [ + - ]: 4 : const auto eigv = details::eig_2d(cov, eig1, eig2);
236 : :
237 : : // find extreme points
238 [ + + ]: 12 : details::Point4<IT> supports;
239 : 4 : const bool8_t is_ccw = details::compute_supports(begin, end, eig1, eig2, supports);
240 : : // build box
241 [ + + ]: 4 : if (is_ccw) {
242 : 2 : std::swap(eig1, eig2);
243 : : }
244 [ + - ]: 4 : BoundingBox bbox = details::compute_bounding_box(eig1, eig2, supports);
245 : 4 : bbox.value = eigv.first;
246 : :
247 : 8 : return bbox;
248 : : }
249 : : } // namespace bounding_box
250 : : } // namespace geometry
251 : : } // namespace common
252 : : } // namespace autoware
253 : :
254 : : #endif // GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
|
