86 approx_polygon.clear();
87 if (polygon.size() < 3)
90 std::vector<std::pair<unsigned, unsigned>> intervals;
91 std::pair<unsigned, unsigned> interval(0, 0);
94 float max_distance = .0f;
95 for (std::size_t idx = 1; idx < polygon.size(); ++idx) {
97 (polygon[0].x - polygon[idx].x) * (polygon[0].x - polygon[idx].x) +
98 (polygon[0].y - polygon[idx].y) * (polygon[0].y - polygon[idx].y);
100 if (distance > max_distance) {
101 max_distance = distance;
102 interval.second = idx;
106 for (std::size_t idx = 1; idx < polygon.size(); ++idx) {
107 float distance = (polygon[interval.second].x - polygon[idx].x) *
108 (polygon[interval.second].x - polygon[idx].x) +
109 (polygon[interval.second].y - polygon[idx].y) *
110 (polygon[interval.second].y - polygon[idx].y);
112 if (distance > max_distance) {
113 max_distance = distance;
114 interval.first = idx;
118 if (max_distance < threshold * threshold)
121 intervals.push_back(interval);
122 std::swap(interval.first, interval.second);
123 intervals.push_back(interval);
127 interval.second =
static_cast<unsigned int>(polygon.size()) - 1;
128 intervals.push_back(interval);
131 std::vector<unsigned> result;
133 while (!intervals.empty()) {
134 std::pair<unsigned, unsigned>& currentInterval = intervals.back();
135 float line_x = polygon[currentInterval.first].y - polygon[currentInterval.second].y;
136 float line_y = polygon[currentInterval.second].x - polygon[currentInterval.first].x;
138 polygon[currentInterval.first].x * polygon[currentInterval.second].y -
139 polygon[currentInterval.first].y * polygon[currentInterval.second].x;
141 float linelen = 1.0f / std::sqrt(line_x * line_x + line_y * line_y);
147 float max_distance = 0.0;
148 unsigned first_index = currentInterval.first + 1;
149 unsigned max_index = 0;
152 if (currentInterval.first > currentInterval.second) {
153 for (std::size_t idx = first_index; idx < polygon.size(); idx++) {
155 std::abs(line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
156 if (distance > max_distance) {
157 max_distance = distance;
164 for (
unsigned int idx = first_index; idx < currentInterval.second; idx++) {
166 std::abs(line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
167 if (distance > max_distance) {
168 max_distance = distance;
173 if (max_distance > threshold) {
174 std::pair<unsigned, unsigned> interval(max_index, currentInterval.second);
175 currentInterval.second = max_index;
176 intervals.push_back(interval);
179 result.push_back(currentInterval.second);
180 intervals.pop_back();
184 approx_polygon.reserve(result.size());
186 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> lines(
188 std::reverse(result.begin(), result.end());
189 for (std::size_t rIdx = 0; rIdx < result.size(); ++rIdx) {
190 std::size_t nIdx = rIdx + 1;
191 if (nIdx == result.size())
194 Eigen::Vector2f centroid = Eigen::Vector2f::Zero();
195 Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero();
196 std::size_t pIdx = result[rIdx];
197 unsigned num_points = 0;
198 if (pIdx > result[nIdx]) {
199 num_points =
static_cast<unsigned>(polygon.size()) - pIdx;
200 for (; pIdx < polygon.size(); ++pIdx) {
201 covariance.coeffRef(0) += polygon[pIdx].x * polygon[pIdx].x;
202 covariance.coeffRef(1) += polygon[pIdx].x * polygon[pIdx].y;
203 covariance.coeffRef(3) += polygon[pIdx].y * polygon[pIdx].y;
204 centroid[0] += polygon[pIdx].x;
205 centroid[1] += polygon[pIdx].y;
210 num_points += result[nIdx] - pIdx;
211 for (; pIdx < result[nIdx]; ++pIdx) {
212 covariance.coeffRef(0) += polygon[pIdx].x * polygon[pIdx].x;
213 covariance.coeffRef(1) += polygon[pIdx].x * polygon[pIdx].y;
214 covariance.coeffRef(3) += polygon[pIdx].y * polygon[pIdx].y;
215 centroid[0] += polygon[pIdx].x;
216 centroid[1] += polygon[pIdx].y;
219 covariance.coeffRef(2) = covariance.coeff(1);
221 float norm = 1.0f / float(num_points);
224 covariance.coeffRef(0) -= centroid[0] * centroid[0];
225 covariance.coeffRef(1) -= centroid[0] * centroid[1];
226 covariance.coeffRef(3) -= centroid[1] * centroid[1];
229 Eigen::Vector2f normal;
230 eigen22(covariance, eval, normal);
233 Eigen::Vector2f direction;
234 direction[0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
235 direction[1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
236 direction.normalize();
238 if (std::abs(direction.dot(normal)) >
float(
M_SQRT1_2)) {
239 std::swap(normal[0], normal[1]);
240 normal[0] = -normal[0];
244 if (direction[0] * normal[1] < direction[1] * normal[0])
247 lines[rIdx].head<2>().matrix() = normal;
248 lines[rIdx][2] = -normal.dot(centroid);
251 float threshold2 = threshold * threshold;
252 for (std::size_t rIdx = 0; rIdx < lines.size(); ++rIdx) {
253 std::size_t nIdx = rIdx + 1;
254 if (nIdx == result.size())
257 Eigen::Vector3f vertex = lines[rIdx].cross(lines[nIdx]);
264 Eigen::Vector3f pq = polygon[result[nIdx]].getVector3fMap() - vertex;
267 float distance = pq.squaredNorm();
268 if (distance > threshold2) {
270 if ((pq[0] * lines[rIdx][0] + pq[1] * lines[rIdx][1] < 0.0) &&
271 (pq[0] * lines[nIdx][0] + pq[1] * lines[nIdx][1] < 0.0)) {
272 float distance1 = lines[rIdx][0] * polygon[result[nIdx]].x +
273 lines[rIdx][1] * polygon[result[nIdx]].y + lines[rIdx][2];
274 float distance2 = lines[nIdx][0] * polygon[result[nIdx]].x +
275 lines[nIdx][1] * polygon[result[nIdx]].y + lines[nIdx][2];
277 point.x = polygon[result[nIdx]].x - distance1 * lines[rIdx][0];
278 point.y = polygon[result[nIdx]].y - distance1 * lines[rIdx][1];
280 approx_polygon.push_back(point);
282 vertex[0] = polygon[result[nIdx]].x - distance2 * lines[nIdx][0];
283 vertex[1] = polygon[result[nIdx]].y - distance2 * lines[nIdx][1];
286 point.getVector3fMap() = vertex;
287 approx_polygon.push_back(point);
292 for (std::vector<unsigned>::reverse_iterator it = result.rbegin();
295 approx_polygon.push_back(polygon[*it]);