Polygon.cpp
Go to the documentation of this file.
1 /*
2  * Polygon.cpp
3  *
4  * Created on: Nov 7, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
11 #include <Eigen/Core>
12 #include <Eigen/Geometry>
13 
14 #include <limits>
15 #include <algorithm>
16 
17 namespace grid_map {
18 
20  : timestamp_(0)
21 {
22 }
23 
24 Polygon::Polygon(std::vector<Position> vertices)
25  : Polygon()
26 {
27  vertices_ = vertices;
28 }
29 
31 
32 bool Polygon::isInside(const Position& point) const
33 {
34  int cross = 0;
35  for (int i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
36  if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
37  && (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) /
38  (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) )
39  {
40  cross++;
41  }
42  }
43  return bool(cross % 2);
44 }
45 
46 void Polygon::addVertex(const Position& vertex)
47 {
48  vertices_.push_back(vertex);
49 }
50 
51 const Position& Polygon::getVertex(const size_t index) const
52 {
53  return vertices_.at(index);
54 }
55 
57 {
58  vertices_.clear();
59 }
60 
61 const Position& Polygon::operator [](const size_t index) const
62 {
63  return getVertex(index);
64 }
65 
66 const std::vector<Position>& Polygon::getVertices() const
67 {
68  return vertices_;
69 }
70 
71 size_t Polygon::nVertices() const
72 {
73  return vertices_.size();
74 }
75 
76 const std::string& Polygon::getFrameId() const
77 {
78  return frameId_;
79 }
80 
81 void Polygon::setFrameId(const std::string& frameId)
82 {
83  frameId_ = frameId;
84 }
85 
86 uint64_t Polygon::getTimestamp() const
87 {
88  return timestamp_;
89 }
90 
91 void Polygon::setTimestamp(const uint64_t timestamp)
92 {
93  timestamp_ = timestamp;
94 }
95 
97 {
98  timestamp_ = 0.0;
99 }
100 
101 double Polygon::getArea() const
102 {
103  double area = 0.0;
104  int j = vertices_.size() - 1;
105  for (int i = 0; i < vertices_.size(); i++) {
106  area += (vertices_.at(j).x() + vertices_.at(i).x())
107  * (vertices_.at(j).y() - vertices_.at(i).y());
108  j = i;
109  }
110  return std::abs(area / 2.0);
111 }
112 
114 {
115  Position centroid = Position::Zero();
116  std::vector<Position> vertices = getVertices();
117  vertices.push_back(vertices.at(0));
118  double area = 0.0;
119  for (int i = 0; i < vertices.size() - 1; i++) {
120  const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
121  area += a;
122  centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
123  centroid.y() += a * (vertices[i].y() + vertices[i+1].y());
124  }
125  area *= 0.5;
126  centroid /= (6.0 * area);
127  return centroid;
128 }
129 
130 void Polygon::getBoundingBox(Position& center, Length& length) const
131 {
132  double minX = std::numeric_limits<double>::infinity();
133  double maxX = -std::numeric_limits<double>::infinity();
134  double minY = std::numeric_limits<double>::infinity();
135  double maxY = -std::numeric_limits<double>::infinity();
136  for (const auto& vertex : vertices_) {
137  if (vertex.x() > maxX) maxX = vertex.x();
138  if (vertex.y() > maxY) maxY = vertex.y();
139  if (vertex.x() < minX) minX = vertex.x();
140  if (vertex.y() < minY) minY = vertex.y();
141  }
142  center.x() = (minX + maxX) / 2.0;
143  center.y() = (minY + maxY) / 2.0;
144  length.x() = (maxX - minX);
145  length.y() = (maxY - minY);
146 }
147 
148 bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
149 {
150  Eigen::MatrixXd V(nVertices(), 2);
151  for (unsigned int i = 0; i < nVertices(); ++i)
152  V.row(i) = vertices_[i];
153 
154  // Create k, a list of indices from V forming the convex hull.
155  // TODO: Assuming counter-clockwise ordered convex polygon.
156  // MATLAB: k = convhulln(V);
157  Eigen::MatrixXi k;
158  k.resizeLike(V);
159  for (unsigned int i = 0; i < V.rows(); ++i)
160  k.row(i) << i, (i+1) % V.rows();
161  Eigen::RowVectorXd c = V.colwise().mean();
162  V.rowwise() -= c;
163  A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);
164 
165  unsigned int rc = 0;
166  for (unsigned int ix = 0; ix < k.rows(); ++ix) {
167  Eigen::MatrixXd F(2, V.cols());
168  F.row(0) << V.row(k(ix, 0));
169  F.row(1) << V.row(k(ix, 1));
170  Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
171  if (luDecomp.rank() == F.rows()) {
172  A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
173  ++rc;
174  }
175  }
176 
177  A = A.topRows(rc);
178  b = Eigen::VectorXd::Ones(A.rows());
179  b = b + A * c.transpose();
180 
181  return true;
182 }
183 
184 bool Polygon::thickenLine(const double thickness)
185 {
186  if (vertices_.size() != 2) return false;
187  const Vector connection(vertices_[1] - vertices_[0]);
188  const Vector orthogonal = thickness * Vector(connection.y(), -connection.x()).normalized();
189  std::vector<Position> newVertices;
190  newVertices.reserve(4);
191  newVertices.push_back(vertices_[0] + orthogonal);
192  newVertices.push_back(vertices_[0] - orthogonal);
193  newVertices.push_back(vertices_[1] - orthogonal);
194  newVertices.push_back(vertices_[1] + orthogonal);
195  vertices_ = newVertices;
196  return true;
197 }
198 
199 bool Polygon::offsetInward(const double margin)
200 {
201  // Create a list of indices of the neighbours of each vertex.
202  // TODO: Assuming counter-clockwise ordered convex polygon.
203  std::vector<Eigen::Array2i> neighbourIndices;
204  const unsigned int n = nVertices();
205  neighbourIndices.resize(n);
206  for (unsigned int i = 0; i < n; ++i) {
207  neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n;
208  }
209 
210  std::vector<Position> copy(vertices_);
211  for (unsigned int i = 0; i < neighbourIndices.size(); ++i) {
212  Eigen::Vector2d v1 = vertices_[neighbourIndices[i](0)] - vertices_[i];
213  Eigen::Vector2d v2 = vertices_[neighbourIndices[i](1)] - vertices_[i];
214  v1.normalize();
215  v2.normalize();
216  const double angle = acos(v1.dot(v2));
217  copy[i] += margin / sin(angle) * (v1 + v2);
218  }
219  vertices_ = copy;
220  return true;
221 }
222 
223 std::vector<Polygon> Polygon::triangulate(const TriangulationMethods& method) const
224 {
225  // TODO Add more triangulation methods.
226  // https://en.wikipedia.org/wiki/Polygon_triangulation
227  std::vector<Polygon> polygons;
228  if (vertices_.size() < 3)
229  return polygons;
230 
231  size_t nPolygons = vertices_.size() - 2;
232  polygons.reserve(nPolygons);
233 
234  if (nPolygons < 1) {
235  // Special case.
236  polygons.push_back(*this);
237  } else {
238  // General case.
239  for (size_t i = 0; i < nPolygons; ++i) {
240  Polygon polygon({vertices_[0], vertices_[i + 1], vertices_[i + 2]});
241  polygons.push_back((polygon));
242  }
243  }
244 
245  return polygons;
246 }
247 
248 Polygon Polygon::fromCircle(const Position center, const double radius,
249  const int nVertices)
250 {
251  Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp;
252 
253  Polygon polygon;
254  for (int j = 0; j < nVertices; j++) {
255  double theta = j * 2 * M_PI / (nVertices - 1);
256  Eigen::Rotation2D<double> rot2d(theta);
257  centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
258  polygon.addVertex(center + centerToVertexTemp);
259  }
260  return polygon;
261 }
262 
264  const Position center2, const double radius,
265  const int nVertices)
266 {
267  if (center1 == center2) return fromCircle(center1, radius, nVertices);
268  Eigen::Vector2d centerToVertex, centerToVertexTemp;
269  centerToVertex = center2 - center1;
270  centerToVertex.normalize();
271  centerToVertex *= radius;
272 
273  grid_map::Polygon polygon;
274  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
275  double theta = M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
276  Eigen::Rotation2D<double> rot2d(theta);
277  centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
278  polygon.addVertex(center1 + centerToVertexTemp);
279  }
280  for (int j = 0; j < ceil(nVertices / 2.0); j++) {
281  double theta = 3 * M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
282  Eigen::Rotation2D<double> rot2d(theta);
283  centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
284  polygon.addVertex(center2 + centerToVertexTemp);
285  }
286  return polygon;
287 }
288 
290 {
291  std::vector<Position> vertices;
292  vertices.reserve(polygon1.nVertices() + polygon2.nVertices());
293  vertices.insert(vertices.end(), polygon1.getVertices().begin(), polygon1.getVertices().end());
294  vertices.insert(vertices.end(), polygon2.getVertices().begin(), polygon2.getVertices().end());
295 
296  return monotoneChainConvexHullOfPoints(vertices);
297 }
298 
299 Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
300 {
301  // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain
302  if (points.size() <= 3) {
303  return Polygon(points);
304  }
305  std::vector<Position> pointsConvexHull(2 * points.size());
306 
307  // Sort points lexicographically.
308  auto sortedPoints(points);
309  std::sort(sortedPoints.begin(), sortedPoints.end(), sortVertices);
310 
311 
312  int k = 0;
313  // Build lower hull
314  for (int i = 0; i < sortedPoints.size(); ++i) {
315  while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
316  k--;
317  }
318  pointsConvexHull.at(k++) = sortedPoints.at(i);
319  }
320 
321  // Build upper hull.
322  for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
323  while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
324  k--;
325  }
326  pointsConvexHull.at(k++) = sortedPoints.at(i);
327  }
328  pointsConvexHull.resize(k - 1);
329 
330  Polygon polygon(pointsConvexHull);
331  return polygon;
332 }
333 
334 bool Polygon::sortVertices(const Eigen::Vector2d& vector1,
335  const Eigen::Vector2d& vector2)
336 {
337  return (vector1.x() < vector2.x()
338  || (vector1.x() == vector2.x() && vector1.y() < vector2.y()));
339 }
340 
341 double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1,
342  const Eigen::Vector2d& vector2)
343 {
344  return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
345 }
346 
347 double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin,
348  const Eigen::Vector2d &pointA,
349  const Eigen::Vector2d &pointB)
350 {
351  return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0;
352 }
353 
354 } /* namespace grid_map */
static Polygon convexHullOfTwoCircles(const Position center1, const Position center2, const double radius, const int nVertices=20)
Definition: Polygon.cpp:263
std::vector< Polygon > triangulate(const TriangulationMethods &method=TriangulationMethods::FAN) const
Definition: Polygon.cpp:223
Eigen::Vector2d Vector
Definition: TypeDefs.hpp:19
static double computeCrossProduct2D(const Eigen::Vector2d &vector1, const Eigen::Vector2d &vector2)
Definition: Polygon.cpp:341
const std::string & getFrameId() const
Definition: Polygon.cpp:76
Position getCentroid() const
Definition: Polygon.cpp:113
static Polygon monotoneChainConvexHullOfPoints(const std::vector< Position > &points)
Definition: Polygon.cpp:299
uint64_t timestamp_
Timestamp of the polygon (nanoseconds).
Definition: Polygon.hpp:245
bool convertToInequalityConstraints(Eigen::MatrixXd &A, Eigen::VectorXd &b) const
Definition: Polygon.cpp:148
static Polygon fromCircle(const Position center, const double radius, const int nVertices=20)
Definition: Polygon.cpp:248
const Position & operator[](const size_t index) const
Definition: Polygon.cpp:61
bool thickenLine(const double thickness)
Definition: Polygon.cpp:184
void resetTimestamp()
Definition: Polygon.cpp:96
const Position & getVertex(const size_t index) const
Definition: Polygon.cpp:51
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
void removeVertices()
Definition: Polygon.cpp:56
bool isInside(const Position &point) const
Definition: Polygon.cpp:32
size_t nVertices() const
Definition: Polygon.cpp:71
std::string frameId_
Frame id of the polygon.
Definition: Polygon.hpp:242
void addVertex(const Position &vertex)
Definition: Polygon.cpp:46
std::vector< Position > vertices_
Vertices of the polygon.
Definition: Polygon.hpp:248
static Polygon convexHull(Polygon &polygon1, Polygon &polygon2)
Definition: Polygon.cpp:289
static bool sortVertices(const Eigen::Vector2d &vector1, const Eigen::Vector2d &vector2)
Definition: Polygon.cpp:334
void getBoundingBox(Position &center, Length &length) const
Definition: Polygon.cpp:130
static double vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointO, const Eigen::Vector2d &pointA, const Eigen::Vector2d &pointB)
Definition: Polygon.cpp:347
void setFrameId(const std::string &frameId)
Definition: Polygon.cpp:81
virtual ~Polygon()
Definition: Polygon.cpp:30
uint64_t getTimestamp() const
Definition: Polygon.cpp:86
bool offsetInward(const double margin)
Definition: Polygon.cpp:199
const std::vector< Position > & getVertices() const
Definition: Polygon.cpp:66
void setTimestamp(const uint64_t timestamp)
Definition: Polygon.cpp:91
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
double getArea() const
Definition: Polygon.cpp:101


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Sun Nov 22 2020 03:24:45