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__LFIT_HPP_
22 : : #define GEOMETRY__BOUNDING_BOX__LFIT_HPP_
23 : :
24 : : #include <geometry/bounding_box/eigenbox_2d.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 A representation of the M matrix for the L-fit algorithm
40 : : struct LFitWs
41 : : {
42 : : /// \brief Number of points in the first partition
43 : : std::size_t p;
44 : : /// \brief Number of points in the second partition
45 : : std::size_t q;
46 : : // assume matrix of form: [a b; c d]
47 : : /// \brief Sum of x values in first partition
48 : : float32_t m12a;
49 : : /// \brief Sum of y values in first partition
50 : : float32_t m12b;
51 : : /// \brief Sum of y values in second partition
52 : : float32_t m12c;
53 : : /// \brief Negative sum of x values in second partition
54 : : float32_t m12d;
55 : : // m22 is a symmetric matrix
56 : : /// \brief Sum_p x_2 + sum_q y_2
57 : : float32_t m22a;
58 : : /// \brief Sum_p x*y - sum_q x*y
59 : : float32_t m22b;
60 : : /// \brief Sum_p y_2 + sum_x y_2
61 : : float32_t m22d;
62 : : }; // struct LFitWs
63 : :
64 : : /// \brief Initialize M matrix for L-fit algorithm
65 : : /// \param[in] begin An iterator pointing to the first point in a point list
66 : : /// \param[in] end An iterator one past the last point in the point list
67 : : /// \param[in] size The number of points in the point list
68 : : /// \param[out] ws A representation of the M matrix to get initialized
69 : : /// \tparam IT The iterator type, should dereference to a point type with float members x and y
70 : : template<typename IT>
71 : 12 : void init_lfit_ws(const IT begin, const IT end, const std::size_t size, LFitWs & ws)
72 : : {
73 : 12 : ws.p = 1UL;
74 : 12 : ws.q = size - 1UL;
75 : : // init P terms (first partition)
76 : : using point_adapter::x_;
77 : : using point_adapter::y_;
78 : 12 : const auto & pt = *begin;
79 : 12 : const float32_t px = x_(pt);
80 : 12 : const float32_t py = y_(pt);
81 : : // assume matrix of form: [a b; c d]
82 : 12 : ws.m12a = px;
83 : 12 : ws.m12b = py;
84 : 12 : ws.m12c = 0.0F;
85 : 12 : ws.m12d = 0.0F;
86 : : // m22 is a symmetric matrix
87 : 12 : ws.m22a = px * px;
88 : 12 : ws.m22b = px * py;
89 : 12 : ws.m22d = py * py;
90 : 12 : auto it = begin;
91 : 12 : ++it;
92 [ + + ]: 116 : for (; it != end; ++it) {
93 : 104 : const auto & qt = *it;
94 : 104 : const float32_t qx = x_(qt);
95 : 104 : const float32_t qy = y_(qt);
96 : 104 : ws.m12c += qy;
97 : 104 : ws.m12d -= qx;
98 : 104 : ws.m22a += qy * qy;
99 : 104 : ws.m22b -= qx * qy;
100 : 104 : ws.m22d += qx * qx;
101 : : }
102 : 12 : }
103 : :
104 : : /// \brief Solves the L fit problem for a given M matrix
105 : : /// \tparam PointT The point type of the cluster being L-fitted
106 : : /// \param[in] ws A representation of the M Matrix
107 : : /// \param[out] dir The best fit direction for this partition/M matrix
108 : : /// \return The L2 residual for this fit (the score, lower is better)
109 : : template<typename PointT>
110 : 104 : float32_t solve_lfit(const LFitWs & ws, PointT & dir)
111 : : {
112 : 104 : const float32_t pi = 1.0F / static_cast<float32_t>(ws.p);
113 : 104 : const float32_t qi = 1.0F / static_cast<float32_t>(ws.q);
114 : 104 : const Covariance2d M{ // matrix of form [x, z; z y]
115 : 104 : ws.m22a - (((ws.m12a * ws.m12a) * pi) + ((ws.m12c * ws.m12c) * qi)),
116 : 104 : ws.m22d - (((ws.m12b * ws.m12b) * pi) + ((ws.m12d * ws.m12d) * qi)),
117 : 104 : ws.m22b - (((ws.m12a * ws.m12b) * pi) + ((ws.m12c * ws.m12d) * qi)),
118 : : 0UL
119 : : };
120 : 104 : PointT eig1;
121 [ + - ]: 104 : const auto eigv = eig_2d(M, eig1, dir);
122 : 104 : return eigv.second;
123 : : }
124 : :
125 : : /// \brief Increments L fit M matrix with information in the point
126 : : /// \tparam PointT The point type
127 : : /// \param[in] pt The point to increment the M matrix with
128 : : /// \param[inout] ws A representation of the M matrix
129 : : template<typename PointT>
130 : 92 : void increment_lfit_ws(const PointT & pt, LFitWs & ws)
131 : : {
132 : 92 : const float32_t px = point_adapter::x_(pt);
133 : 92 : const float32_t py = point_adapter::y_(pt);
134 : 92 : ws.m12a += px;
135 : 92 : ws.m12b += py;
136 : 92 : ws.m12c -= py;
137 : 92 : ws.m12d += px;
138 : 92 : ws.m22b += 2.0F * px * py;
139 : 92 : const float32_t px2y2 = (px - py) * (px + py);
140 : 92 : ws.m22a += px2y2;
141 : 92 : ws.m22d -= px2y2;
142 : 92 : }
143 : :
144 : : /// \tparam IT An iterator type that should dereference into a point type with float members x and y
145 : : template<typename PointT>
146 : : class LFitCompare
147 : : {
148 : : public:
149 : : /// \brief Constructor, initializes normal direction
150 : : /// \param[in] hint A 2d vector with the direction that will be used to order the
151 : : /// point list
152 : 12 : explicit LFitCompare(const PointT & hint)
153 : 12 : : m_nx(point_adapter::x_(hint)),
154 : 12 : m_ny(point_adapter::y_(hint))
155 : : {
156 : 12 : }
157 : :
158 : : /// \brief Comparator operation, returns true if the projection of a the tangent line
159 : : /// is smaller than the projection of b
160 : : /// \param[in] p The first point for comparison
161 : : /// \param[in] q The second point for comparison
162 : : /// \return True if a has a smaller projection than b on the tangent line
163 : 376 : bool8_t operator()(const PointT & p, const PointT & q) const
164 : : {
165 : : using point_adapter::x_;
166 : : using point_adapter::y_;
167 : 376 : return ((x_(p) * m_nx) + (y_(p) * m_ny)) < ((x_(q) * m_nx) + (y_(q) * m_ny));
168 : : }
169 : :
170 : : private:
171 : : const float32_t m_nx;
172 : : const float32_t m_ny;
173 : : }; // class LFitCompare
174 : :
175 : : /// \brief The main implementation of L-fitting a bounding box to a list of points.
176 : : /// Assumes sufficiently valid, large enough, and appropriately ordered point list
177 : : /// \param[in] begin An iterator pointing to the first point in a point list
178 : : /// \param[in] end An iterator pointing to one past the last point in the point list
179 : : /// \param[in] size The number of points in the point list
180 : : /// \return A bounding box that minimizes the LFit residual
181 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
182 : : template<typename IT>
183 : 12 : BoundingBox lfit_bounding_box_2d_impl(const IT begin, const IT end, const std::size_t size)
184 : : {
185 : : // initialize M
186 : 12 : LFitWs ws{};
187 : 12 : init_lfit_ws(begin, end, size, ws);
188 : : // solve initial problem
189 : 12 : details::base_type<decltype(*begin)> best_normal;
190 [ + - ]: 12 : float32_t min_eig = solve_lfit(ws, best_normal);
191 : : // solve subsequent problems
192 : 12 : auto it = begin;
193 : 12 : ++it;
194 [ + - ]: 104 : for (; it != end; ++it) {
195 : : // update M
196 : 104 : ws.p += 1U;
197 : 104 : ws.q -= 1U;
198 [ + + ]: 104 : if (ws.q == 0U) { // checks for q = 0 case
199 : 12 : break;
200 : : }
201 : 92 : increment_lfit_ws(*it, ws);
202 : : // solve incremented problem
203 : 92 : decltype(best_normal) dir;
204 [ + - ]: 92 : const float32_t score = solve_lfit(ws, dir);
205 : : // update optima
206 [ + + ]: 92 : if (score < min_eig) {
207 : 22 : min_eig = score;
208 : 22 : best_normal = dir;
209 : : }
210 : : }
211 : : // can recover best corner point, but don't care, need to cover all points
212 : 12 : const auto inorm = 1.0F / norm_2d(best_normal);
213 [ - + ]: 12 : if (!std::isnormal(inorm)) {
214 [ # # ]: 0 : throw std::runtime_error{"LFit: Abnormal norm"};
215 : : }
216 : 12 : best_normal = times_2d(best_normal, inorm);
217 : 12 : auto best_tangent = get_normal(best_normal);
218 : : // find extreme points
219 [ + + ]: 36 : Point4<IT> supports;
220 : 12 : const bool8_t is_ccw = details::compute_supports(begin, end, best_normal, best_tangent, supports);
221 [ + - ]: 12 : if (is_ccw) {
222 : 12 : std::swap(best_normal, best_tangent);
223 : : }
224 [ + - ]: 12 : BoundingBox bbox = details::compute_bounding_box(best_normal, best_tangent, supports);
225 : 12 : bbox.value = min_eig;
226 : :
227 : 24 : return bbox;
228 : : }
229 : : } // namespace details
230 : :
231 : : /// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed
232 : : /// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation"
233 : : /// \return An oriented bounding box in x-y. This bounding box has no height information
234 : : /// \param[in] begin An iterator pointing to the first point in a point list
235 : : /// \param[in] end An iterator pointing to one past the last point in the point list
236 : : /// \param[in] size The number of points in the point list
237 : : /// \param[in] hint An iterator pointing to the point whose normal will be used to sort the list
238 : : /// \return A pair where the first element is an iterator pointing to the nearest point, and the
239 : : /// second element is the size of the list
240 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
241 : : /// \throw std::domain_error If the number of points is too few
242 : : template<typename IT, typename PointT>
243 : 12 : BoundingBox lfit_bounding_box_2d(
244 : : const IT begin,
245 : : const IT end,
246 : : const PointT hint,
247 : : const std::size_t size)
248 : : {
249 [ - + ]: 12 : if (size <= 1U) {
250 [ # # ]: 0 : throw std::domain_error("LFit requires >= 2 points!");
251 : : }
252 : : // NOTE: assumes points are in base_link/sensor frame!
253 : : // sort along tangent wrt sensor origin
254 : : //lint -e522 NOLINT Not a pure function: data structure iterators are pointing to is modified
255 [ + - ]: 12 : std::partial_sort(begin, end, end, details::LFitCompare<PointT>{hint});
256 : :
257 : 12 : return details::lfit_bounding_box_2d_impl(begin, end, size);
258 : : }
259 : :
260 : : /// \brief Compute bounding box which best fits an L-shaped cluster. Uses the method proposed
261 : : /// in "Efficient L-shape fitting of laser scanner data for vehicle pose estimation".
262 : : /// This implementation sorts the list using std::sort
263 : : /// \return An oriented bounding box in x-y. This bounding box has no height information
264 : : /// \param[in] begin An iterator pointing to the first point in a point list
265 : : /// \param[in] end An iterator pointing to one past the last point in the point list
266 : : /// \tparam IT An iterator type dereferencable into a point with float members x and y
267 : : template<typename IT>
268 : 12 : BoundingBox lfit_bounding_box_2d(const IT begin, const IT end)
269 : : {
270 : : // use the principal component as the sorting direction
271 : 12 : const auto cov = details::covariance_2d(begin, end);
272 : : using PointT = details::base_type<decltype(*begin)>;
273 : 12 : PointT eig1;
274 : 12 : PointT eig2;
275 [ + - ]: 12 : (void)details::eig_2d(cov, eig1, eig2);
276 : : (void)eig2;
277 [ + - ]: 24 : return lfit_bounding_box_2d(begin, end, eig1, cov.num_points);
278 : : }
279 : : } // namespace bounding_box
280 : : } // namespace geometry
281 : : } // namespace common
282 : : } // namespace autoware
283 : :
284 : : #endif // GEOMETRY__BOUNDING_BOX__LFIT_HPP_
|
