Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <grid_map_core/Polygon.hpp>
00010
00011 #include <Eigen/Core>
00012 #include <Eigen/Geometry>
00013
00014 #include <limits>
00015 #include <algorithm>
00016
00017 namespace grid_map {
00018
00019 Polygon::Polygon()
00020 : timestamp_(0)
00021 {
00022 }
00023
00024 Polygon::Polygon(std::vector<Position> vertices)
00025 : Polygon()
00026 {
00027 vertices_ = vertices;
00028 }
00029
00030 Polygon::~Polygon() {}
00031
00032 bool Polygon::isInside(const Position& point) const
00033 {
00034 int cross = 0;
00035 for (int i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
00036 if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
00037 && (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) /
00038 (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) )
00039 {
00040 cross++;
00041 }
00042 }
00043 return bool(cross % 2);
00044 }
00045
00046 void Polygon::addVertex(const Position& vertex)
00047 {
00048 vertices_.push_back(vertex);
00049 }
00050
00051 const Position& Polygon::getVertex(const size_t index) const
00052 {
00053 return vertices_.at(index);
00054 }
00055
00056 void Polygon::removeVertices()
00057 {
00058 vertices_.clear();
00059 }
00060
00061 const Position& Polygon::operator [](const size_t index) const
00062 {
00063 return getVertex(index);
00064 }
00065
00066 const std::vector<Position>& Polygon::getVertices() const
00067 {
00068 return vertices_;
00069 }
00070
00071 size_t Polygon::nVertices() const
00072 {
00073 return vertices_.size();
00074 }
00075
00076 const std::string& Polygon::getFrameId() const
00077 {
00078 return frameId_;
00079 }
00080
00081 void Polygon::setFrameId(const std::string& frameId)
00082 {
00083 frameId_ = frameId;
00084 }
00085
00086 uint64_t Polygon::getTimestamp() const
00087 {
00088 return timestamp_;
00089 }
00090
00091 void Polygon::setTimestamp(const uint64_t timestamp)
00092 {
00093 timestamp_ = timestamp;
00094 }
00095
00096 void Polygon::resetTimestamp()
00097 {
00098 timestamp_ = 0.0;
00099 }
00100
00101 double Polygon::getArea() const
00102 {
00103 double area = 0.0;
00104 int j = vertices_.size() - 1;
00105 for (int i = 0; i < vertices_.size(); i++) {
00106 area += (vertices_.at(j).x() + vertices_.at(i).x())
00107 * (vertices_.at(j).y() - vertices_.at(i).y());
00108 j = i;
00109 }
00110 return std::abs(area / 2.0);
00111 }
00112
00113 Position Polygon::getCentroid() const
00114 {
00115 Position centroid = Position::Zero();
00116 std::vector<Position> vertices = getVertices();
00117 vertices.push_back(vertices.at(0));
00118 double area = 0.0;
00119 for (int i = 0; i < vertices.size() - 1; i++) {
00120 const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
00121 area += a;
00122 centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
00123 centroid.y() += a * (vertices[i].y() + vertices[i+1].y());
00124 }
00125 area *= 0.5;
00126 centroid /= (6.0 * area);
00127 return centroid;
00128 }
00129
00130 void Polygon::getBoundingBox(Position& center, Length& length) const
00131 {
00132 double minX = std::numeric_limits<double>::infinity();
00133 double maxX = -std::numeric_limits<double>::infinity();
00134 double minY = std::numeric_limits<double>::infinity();
00135 double maxY = -std::numeric_limits<double>::infinity();
00136 for (const auto& vertex : vertices_) {
00137 if (vertex.x() > maxX) maxX = vertex.x();
00138 if (vertex.y() > maxY) maxY = vertex.y();
00139 if (vertex.x() < minX) minX = vertex.x();
00140 if (vertex.y() < minY) minY = vertex.y();
00141 }
00142 center.x() = (minX + maxX) / 2.0;
00143 center.y() = (minY + maxY) / 2.0;
00144 length.x() = (maxX - minX);
00145 length.y() = (maxY - minY);
00146 }
00147
00148 bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
00149 {
00150 Eigen::MatrixXd V(nVertices(), 2);
00151 for (unsigned int i = 0; i < nVertices(); ++i)
00152 V.row(i) = vertices_[i];
00153
00154
00155
00156
00157 Eigen::MatrixXi k;
00158 k.resizeLike(V);
00159 for (unsigned int i = 0; i < V.rows(); ++i)
00160 k.row(i) << i, (i+1) % V.rows();
00161 Eigen::RowVectorXd c = V.colwise().mean();
00162 V.rowwise() -= c;
00163 A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);
00164
00165 unsigned int rc = 0;
00166 for (unsigned int ix = 0; ix < k.rows(); ++ix) {
00167 Eigen::MatrixXd F(2, V.cols());
00168 F.row(0) << V.row(k(ix, 0));
00169 F.row(1) << V.row(k(ix, 1));
00170 Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
00171 if (luDecomp.rank() == F.rows()) {
00172 A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
00173 ++rc;
00174 }
00175 }
00176
00177 A = A.topRows(rc);
00178 b = Eigen::VectorXd::Ones(A.rows());
00179 b = b + A * c.transpose();
00180
00181 return true;
00182 }
00183
00184 bool Polygon::thickenLine(const double thickness)
00185 {
00186 if (vertices_.size() != 2) return false;
00187 const Vector connection(vertices_[1] - vertices_[0]);
00188 const Vector orthogonal = thickness * Vector(connection.y(), -connection.x()).normalized();
00189 std::vector<Position> newVertices;
00190 newVertices.reserve(4);
00191 newVertices.push_back(vertices_[0] + orthogonal);
00192 newVertices.push_back(vertices_[0] - orthogonal);
00193 newVertices.push_back(vertices_[1] - orthogonal);
00194 newVertices.push_back(vertices_[1] + orthogonal);
00195 vertices_ = newVertices;
00196 return true;
00197 }
00198
00199 bool Polygon::offsetInward(const double margin)
00200 {
00201
00202
00203 std::vector<Eigen::Array2i> neighbourIndices;
00204 const unsigned int n = nVertices();
00205 neighbourIndices.resize(n);
00206 for (unsigned int i = 0; i < n; ++i) {
00207 neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n;
00208 }
00209
00210 std::vector<Position> copy(vertices_);
00211 for (unsigned int i = 0; i < neighbourIndices.size(); ++i) {
00212 Eigen::Vector2d v1 = vertices_[neighbourIndices[i](0)] - vertices_[i];
00213 Eigen::Vector2d v2 = vertices_[neighbourIndices[i](1)] - vertices_[i];
00214 v1.normalize();
00215 v2.normalize();
00216 const double angle = acos(v1.dot(v2));
00217 copy[i] += margin / sin(angle) * (v1 + v2);
00218 }
00219 vertices_ = copy;
00220 return true;
00221 }
00222
00223 std::vector<Polygon> Polygon::triangulate(const TriangulationMethods& method) const
00224 {
00225
00226
00227 std::vector<Polygon> polygons;
00228 if (vertices_.size() < 3)
00229 return polygons;
00230
00231 size_t nPolygons = vertices_.size() - 2;
00232 polygons.reserve(nPolygons);
00233
00234 if (nPolygons < 1) {
00235
00236 polygons.push_back(*this);
00237 } else {
00238
00239 for (size_t i = 0; i < nPolygons; ++i) {
00240 Polygon polygon({vertices_[0], vertices_[i + 1], vertices_[i + 2]});
00241 polygons.push_back((polygon));
00242 }
00243 }
00244
00245 return polygons;
00246 }
00247
00248 Polygon Polygon::fromCircle(const Position center, const double radius,
00249 const int nVertices)
00250 {
00251 Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp;
00252
00253 Polygon polygon;
00254 for (int j = 0; j < nVertices; j++) {
00255 double theta = j * 2 * M_PI / (nVertices - 1);
00256 Eigen::Rotation2D<double> rot2d(theta);
00257 centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
00258 polygon.addVertex(center + centerToVertexTemp);
00259 }
00260 return polygon;
00261 }
00262
00263 Polygon Polygon::convexHullOfTwoCircles(const Position center1,
00264 const Position center2, const double radius,
00265 const int nVertices)
00266 {
00267 if (center1 == center2) return fromCircle(center1, radius, nVertices);
00268 Eigen::Vector2d centerToVertex, centerToVertexTemp;
00269 centerToVertex = center2 - center1;
00270 centerToVertex.normalize();
00271 centerToVertex *= radius;
00272
00273 grid_map::Polygon polygon;
00274 for (int j = 0; j < ceil(nVertices / 2.0); j++) {
00275 double theta = M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
00276 Eigen::Rotation2D<double> rot2d(theta);
00277 centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
00278 polygon.addVertex(center1 + centerToVertexTemp);
00279 }
00280 for (int j = 0; j < ceil(nVertices / 2.0); j++) {
00281 double theta = 3 * M_PI_2 + j * M_PI / (ceil(nVertices / 2.0) - 1);
00282 Eigen::Rotation2D<double> rot2d(theta);
00283 centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
00284 polygon.addVertex(center2 + centerToVertexTemp);
00285 }
00286 return polygon;
00287 }
00288
00289 Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2)
00290 {
00291 std::vector<Position> vertices;
00292 vertices.reserve(polygon1.nVertices() + polygon2.nVertices());
00293 vertices.insert(vertices.end(), polygon1.getVertices().begin(), polygon1.getVertices().end());
00294 vertices.insert(vertices.end(), polygon2.getVertices().begin(), polygon2.getVertices().end());
00295
00296 return monotoneChainConvexHullOfPoints(vertices);
00297 }
00298
00299 Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
00300 {
00301
00302 if (points.size() <= 3) {
00303 return Polygon(points);
00304 }
00305 std::vector<Position> pointsConvexHull(2 * points.size());
00306
00307
00308 auto sortedPoints(points);
00309 std::sort(sortedPoints.begin(), sortedPoints.end(), sortVertices);
00310
00311
00312 int k = 0;
00313
00314 for (int i = 0; i < sortedPoints.size(); ++i) {
00315 while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
00316 k--;
00317 }
00318 pointsConvexHull.at(k++) = sortedPoints.at(i);
00319 }
00320
00321
00322 for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) {
00323 while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
00324 k--;
00325 }
00326 pointsConvexHull.at(k++) = sortedPoints.at(i);
00327 }
00328 pointsConvexHull.resize(k - 1);
00329
00330 Polygon polygon(pointsConvexHull);
00331 return polygon;
00332 }
00333
00334 bool Polygon::sortVertices(const Eigen::Vector2d& vector1,
00335 const Eigen::Vector2d& vector2)
00336 {
00337 return (vector1.x() < vector2.x()
00338 || (vector1.x() == vector2.x() && vector1.y() < vector2.y()));
00339 }
00340
00341 double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1,
00342 const Eigen::Vector2d& vector2)
00343 {
00344 return (vector1.x() * vector2.y() - vector1.y() * vector2.x());
00345 }
00346
00347 double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin,
00348 const Eigen::Vector2d &pointA,
00349 const Eigen::Vector2d &pointB)
00350 {
00351 return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0;
00352 }
00353
00354 }