18
18
// / \brief This file implements 2D pca on a linked list of points to estimate an oriented
19
19
// / bounding box
20
20
21
+ // cspell: ignore eigenbox, EIGENBOX
21
22
#ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
22
23
#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_
23
24
@@ -50,11 +51,12 @@ struct Covariance2d
50
51
std::size_t num_points;
51
52
}; // struct Covariance2d
52
53
54
+ // cspell: ignore Welford
53
55
// / \brief Compute 2d covariance matrix of a list of points using Welford's online algorithm
54
56
// / \param[in] begin An iterator pointing to the first point in a point list
55
57
// / \param[in] end An iterator pointing to one past the last point in the point list
56
- // / \tparam IT An iterator type dereferencable into a point with float members x and y
57
- // / \return A 2d covariance matrix for all points inthe list
58
+ // / \tparam IT An iterator type dereferenceable into a point with float members x and y
59
+ // / \return A 2d covariance matrix for all points in the list
58
60
template <typename IT>
59
61
Covariance2d covariance_2d (const IT begin, const IT end)
60
62
{
@@ -93,13 +95,14 @@ Covariance2d covariance_2d(const IT begin, const IT end)
93
95
94
96
// / \brief Compute eigenvectors and eigenvalues
95
97
// / \param[in] cov 2d Covariance matrix
96
- // / \param[out] eigvec1 First eigenvector
97
- // / \param[out] eigvec2 Second eigenvector
98
+ // / \param[out] eig_vec1 First eigenvector
99
+ // / \param[out] eig_vec2 Second eigenvector
98
100
// / \tparam PointT Point type that has at least float members x and y
99
101
// / \return A pairt of eigenvalues: The first is the larger eigenvalue
100
102
// / \throw std::runtime error if you would get degenerate covariance
101
103
template <typename PointT>
102
- std::pair<float32_t , float32_t > eig_2d (const Covariance2d & cov, PointT & eigvec1, PointT & eigvec2)
104
+ std::pair<float32_t , float32_t > eig_2d (
105
+ const Covariance2d & cov, PointT & eig_vec1, PointT & eig_vec2)
103
106
{
104
107
const float32_t tr_2 = (cov.xx + cov.yy ) * 0 .5F ;
105
108
const float32_t det = (cov.xx * cov.yy ) - (cov.xy * cov.xy );
@@ -120,28 +123,28 @@ std::pair<float32_t, float32_t> eig_2d(const Covariance2d & cov, PointT & eigvec
120
123
// are persistent against further calculations.
121
124
// (e.g. taking cross product of two eigen vectors)
122
125
if (fabsf (cov.xy * cov.xy ) > std::numeric_limits<float32_t >::epsilon ()) {
123
- xr_ (eigvec1 ) = cov.xy ;
124
- yr_ (eigvec1 ) = ret.first - cov.xx ;
125
- xr_ (eigvec2 ) = cov.xy ;
126
- yr_ (eigvec2 ) = ret.second - cov.xx ;
126
+ xr_ (eig_vec1 ) = cov.xy ;
127
+ yr_ (eig_vec1 ) = ret.first - cov.xx ;
128
+ xr_ (eig_vec2 ) = cov.xy ;
129
+ yr_ (eig_vec2 ) = ret.second - cov.xx ;
127
130
} else {
128
131
if (cov.xx > cov.yy ) {
129
- xr_ (eigvec1 ) = 1 .0F ;
130
- yr_ (eigvec1 ) = 0 .0F ;
131
- xr_ (eigvec2 ) = 0 .0F ;
132
- yr_ (eigvec2 ) = 1 .0F ;
132
+ xr_ (eig_vec1 ) = 1 .0F ;
133
+ yr_ (eig_vec1 ) = 0 .0F ;
134
+ xr_ (eig_vec2 ) = 0 .0F ;
135
+ yr_ (eig_vec2 ) = 1 .0F ;
133
136
} else {
134
- xr_ (eigvec1 ) = 0 .0F ;
135
- yr_ (eigvec1 ) = 1 .0F ;
136
- xr_ (eigvec2 ) = 1 .0F ;
137
- yr_ (eigvec2 ) = 0 .0F ;
137
+ xr_ (eig_vec1 ) = 0 .0F ;
138
+ yr_ (eig_vec1 ) = 1 .0F ;
139
+ xr_ (eig_vec2 ) = 1 .0F ;
140
+ yr_ (eig_vec2 ) = 0 .0F ;
138
141
}
139
142
}
140
143
return ret;
141
144
}
142
145
143
146
// / \brief Given eigenvectors, compute support (furthest) point in each direction
144
- // / \tparam IT An iterator type dereferencable into a point with float members x and y
147
+ // / \tparam IT An iterator type dereferenceable into a point with float members x and y
145
148
// / \tparam PointT type of a point with float members x and y
146
149
// / \param[in] begin An iterator pointing to the first point in a point list
147
150
// / \param[in] end An iterator pointing to one past the last point in the point list
@@ -183,7 +186,7 @@ bool8_t compute_supports(
183
186
}
184
187
185
188
// / \brief Compute bounding box given a pair of basis directions
186
- // / \tparam IT An iterator type dereferencable into a point with float members x and y
189
+ // / \tparam IT An iterator type dereferenceable into a point with float members x and y
187
190
// / \tparam PointT Point type of the lists, must have float members x and y
188
191
// / \param[in] ax1 First basis direction, assumed to be normal to ax2
189
192
// / \param[in] ax2 Second basis direction, assumed to be normal to ax1, assumed to be ccw wrt ax1
@@ -210,7 +213,7 @@ BoundingBox compute_bounding_box(
210
213
// / modify the list. The resulting bounding box is not necessarily minimum in any way
211
214
// / \param[in] begin An iterator pointing to the first point in a point list
212
215
// / \param[in] end An iterator pointing to one past the last point in the point list
213
- // / \tparam IT An iterator type dereferencable into a point with float members x and y
216
+ // / \tparam IT An iterator type dereferenceable into a point with float members x and y
214
217
// / \return An oriented bounding box in x-y. This bounding box has no height information
215
218
template <typename IT>
216
219
BoundingBox eigenbox_2d (const IT begin, const IT end)
@@ -222,7 +225,7 @@ BoundingBox eigenbox_2d(const IT begin, const IT end)
222
225
using PointT = details::base_type<decltype (*begin)>;
223
226
PointT eig1;
224
227
PointT eig2;
225
- const auto eigv = details::eig_2d (cov, eig1, eig2);
228
+ const auto eig_v = details::eig_2d (cov, eig1, eig2);
226
229
227
230
// find extreme points
228
231
details::Point4<IT> supports;
@@ -232,7 +235,7 @@ BoundingBox eigenbox_2d(const IT begin, const IT end)
232
235
std::swap (eig1, eig2);
233
236
}
234
237
BoundingBox bbox = details::compute_bounding_box (eig1, eig2, supports);
235
- bbox.value = eigv .first ;
238
+ bbox.value = eig_v .first ;
236
239
237
240
return bbox;
238
241
}
0 commit comments