├── data ├── map.pgm └── basement.png ├── result ├── result.gif ├── basement_path.png ├── basement_contour.png └── basement_decomposition.png ├── CMakeLists.txt ├── README.md ├── include ├── tcd.h ├── visibility_polygon.h ├── weakly_monotone.h ├── cgal_definitions.h ├── bcd.h ├── cgal_comm.h ├── decomposition.h ├── sweep.h ├── visibility_graph.h ├── graph_base.h ├── coverage_planner.h └── graph_base_impl.h └── src ├── tcd.cc ├── weakly_monotone.cc ├── visibility_polygon.cc ├── decomposition.cc ├── cgal_comm.cc ├── visibility_graph.cc ├── sweep.cc ├── bcd.cc └── main.cpp /data/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicheyHuang/CoveragePlanner/HEAD/data/map.pgm -------------------------------------------------------------------------------- /data/basement.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicheyHuang/CoveragePlanner/HEAD/data/basement.png -------------------------------------------------------------------------------- /result/result.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicheyHuang/CoveragePlanner/HEAD/result/result.gif -------------------------------------------------------------------------------- /result/basement_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicheyHuang/CoveragePlanner/HEAD/result/basement_path.png -------------------------------------------------------------------------------- /result/basement_contour.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicheyHuang/CoveragePlanner/HEAD/result/basement_contour.png -------------------------------------------------------------------------------- /result/basement_decomposition.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RicheyHuang/CoveragePlanner/HEAD/result/basement_decomposition.png -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(CoveragePlanner) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(Eigen3 REQUIRED) 8 | find_package(OpenCV 4 REQUIRED) 9 | find_package(CGAL QUIET COMPONENTS Core) 10 | 11 | include_directories(${EIGEN_INCLUDE_DIRS}) 12 | include_directories(${OpenCV_INCLUDE_DIRS}) 13 | include_directories(${CGAL_INCLUDE_DIRS}) 14 | 15 | file(GLOB_RECURSE srcs "src/*.cc" "src/*.cpp") 16 | file(GLOB_RECURSE hdrs "include/*.h") 17 | 18 | message("find source files: ${srcs}") 19 | message("find headers: ${hdrs}") 20 | 21 | include_directories(include) 22 | 23 | add_executable(CoveragePlanner ${srcs} ${hdrs}) 24 | 25 | target_link_libraries(CoveragePlanner 26 | ${OpenCV_LIBS} 27 | CGAL::CGAL 28 | CGAL::CGAL_Core 29 | ) 30 | 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CoveragePlanner 2 | 3 | **Dependencies：** 4 | 5 | CGAL 5.0 6 | 7 | OpenCV 4.0 8 | 9 | Eigen 3.0 10 | 11 | **demo:** 12 | 13 | ![original pgm map](https://github.com/RicheyHuang/CoveragePlanner/blob/master/result/result.gif) 14 | 15 | **original pgm map:** 16 | 17 | ![original pgm map](https://github.com/RicheyHuang/CoveragePlanner/blob/master/data/basement.png) 18 | 19 | **acquire polygon boundary and obstacles:** 20 | 21 | ![acquire polygon boundary and obstacles](https://github.com/RicheyHuang/CoveragePlanner/blob/master/result/basement_contour.png) 22 | 23 | **boustrophedon cellular decomposition:** 24 | 25 | ![boustrophedon cellular decomposition](https://github.com/RicheyHuang/CoveragePlanner/blob/master/result/basement_decomposition.png) 26 | 27 | **path planning by solving TSP:** 28 | 29 | ![path planning by solving TSP](https://github.com/RicheyHuang/CoveragePlanner/blob/master/result/basement_path.png) 30 | -------------------------------------------------------------------------------- /include/tcd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_TCD_H_ 21 | #define COVERAGEPLANNER_TCD_H_ 22 | 23 | #include "cgal_definitions.h" 24 | 25 | // Wrapper functions to interface trapezoidal decomposition (TCD). 26 | namespace polygon_coverage_planning { 27 | 28 | std::vector computeTCD(const PolygonWithHoles& polygon_in, 29 | const Direction_2& dir); 30 | 31 | } // namespace polygon_coverage_planning 32 | 33 | #endif // COVERAGEPLANNER_TCD_H_ 34 | -------------------------------------------------------------------------------- /include/visibility_polygon.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_VISIBILITY_POLYGON_H_ 21 | #define COVERAGEPLANNER_VISIBILITY_POLYGON_H_ 22 | 23 | #include "cgal_definitions.h" 24 | 25 | namespace polygon_coverage_planning { 26 | 27 | // Compute the visibility polygon given a point inside a strictly simple 28 | // polygon. Francisc Bungiu, Michael Hemmer, John Hershberger, Kan Huang, and 29 | // Alexander Kröller. Efficient computation of visibility polygons. CoRR, 30 | // abs/1403.3905, 2014. 31 | bool computeVisibilityPolygon(const PolygonWithHoles& pwh, 32 | const Point_2& query_point, 33 | Polygon_2* visibility_polygon); 34 | 35 | } // namespace polygon_coverage_planning 36 | 37 | #endif // COVERAGEPLANNER_VISIBILITY_POLYGON_H_ 38 | -------------------------------------------------------------------------------- /include/weakly_monotone.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_WEAKLY_MONOTONE_H_ 21 | #define COVERAGEPLANNER_WEAKLY_MONOTONE_H_ 22 | 23 | #include "cgal_definitions.h" 24 | 25 | namespace polygon_coverage_planning { 26 | 27 | // Check whether polygon 'in' is weakly monotone perpendicular to 'x_axis'. 28 | bool isWeaklyMonotone(const Polygon_2& in, const Line_2& x_axis); 29 | // For all edges check whether polygon 'in' is weakly monotone perpendicular to 30 | // that edge. 31 | std::vector getAllSweepableEdgeDirections(const Polygon_2& in); 32 | 33 | VertexConstCirculator findSouth(const Polygon_2& in, const Line_2& x_axis); 34 | VertexConstCirculator findNorth(const Polygon_2& in, const Line_2& x_axis); 35 | 36 | } // namespace polygon_coverage_planning 37 | 38 | #endif // COVERAGEPLANNER_WEAKLY_MONOTONE_H_ 39 | -------------------------------------------------------------------------------- /src/tcd.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include "cgal_comm.h" 21 | #include "cgal_definitions.h" 22 | #include "tcd.h" 23 | 24 | #include 25 | 26 | namespace polygon_coverage_planning { 27 | 28 | std::vector computeTCD(const PolygonWithHoles& polygon_in, 29 | const Direction_2& dir) { 30 | // Rotate polygon to have direction aligned with x-axis. 31 | // TODO(rikba): Make this independent of rotation. 32 | PolygonWithHoles rotated_polygon = rotatePolygon(polygon_in, dir); 33 | 34 | // TCD 35 | std::vector traps; 36 | CGAL::Polygon_vertical_decomposition_2 decom; 37 | decom(rotated_polygon, std::back_inserter(traps)); 38 | 39 | // Rotate back all polygons. 40 | for (auto& p : traps) { 41 | CGAL::Aff_transformation_2 rotation(CGAL::ROTATION, dir, 1, 1e9); 42 | p = CGAL::transform(rotation, p); 43 | } 44 | 45 | return traps; 46 | } 47 | 48 | } // namespace polygon_coverage_planning 49 | -------------------------------------------------------------------------------- /include/cgal_definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_CGAL_DEFINITIONS_H_ 21 | #define COVERAGEPLANNER_CGAL_DEFINITIONS_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | typedef CGAL::Exact_predicates_exact_constructions_kernel K; 28 | typedef K::FT FT; 29 | typedef K::Point_2 Point_2; 30 | typedef K::Point_3 Point_3; 31 | typedef K::Vector_2 Vector_2; 32 | typedef K::Direction_2 Direction_2; 33 | typedef K::Line_2 Line_2; 34 | typedef K::Intersect_2 Intersect_2; 35 | typedef K::Plane_3 Plane_3; 36 | typedef K::Segment_2 Segment_2; 37 | typedef K::Triangle_2 Triangle_2; 38 | typedef CGAL::Polygon_2 Polygon_2; 39 | typedef Polygon_2::Vertex_const_iterator VertexConstIterator; 40 | typedef Polygon_2::Vertex_const_circulator VertexConstCirculator; 41 | typedef Polygon_2::Vertex_iterator VertexIterator; 42 | typedef Polygon_2::Vertex_circulator VertexCirculator; 43 | typedef Polygon_2::Edge_const_iterator EdgeConstIterator; 44 | typedef Polygon_2::Edge_const_circulator EdgeConstCirculator; 45 | typedef CGAL::Polygon_with_holes_2 PolygonWithHoles; 46 | typedef CGAL::Exact_predicates_inexact_constructions_kernel InexactKernel; 47 | 48 | #endif // COVERAGEPLANNER_CGAL_DEFINITIONS_H_ 49 | -------------------------------------------------------------------------------- /include/bcd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_BCD_H_ 21 | #define COVERAGEPLANNER_BCD_H_ 22 | 23 | #include "cgal_definitions.h" 24 | 25 | // Choset, Howie. "Coverage of known spaces: The boustrophedon cellular 26 | // decomposition." Autonomous Robots 9.3 (2000): 247-253. 27 | // https://www.cs.cmu.edu/~motionplanning/lecture/Chap6-CellDecomp_howie.pdf 28 | namespace polygon_coverage_planning { 29 | 30 | std::vector computeBCD(const PolygonWithHoles& polygon_in, 31 | const Direction_2& dir); 32 | void sortPolygon(PolygonWithHoles* pwh); 33 | std::vector getXSortedVertices( 34 | const PolygonWithHoles& p); 35 | void processEvent(const PolygonWithHoles& pwh, const VertexConstCirculator& v, 36 | std::vector* sorted_vertices, 37 | std::vector* processed_vertices, 38 | std::list* L, std::list* open_polygons, 39 | std::vector* closed_polygons); 40 | std::vector getIntersections(const std::list& L, 41 | const Line_2& l); 42 | bool outOfPWH(const PolygonWithHoles& pwh, const Point_2& p); 43 | // Removes duplicate vertices. Returns if resulting polygon is simple and has 44 | // some area. 45 | bool cleanupPolygon(Polygon_2* poly); 46 | 47 | } // namespace polygon_coverage_planning 48 | 49 | #endif // COVERAGEPLANNER_BCD_H_ 50 | -------------------------------------------------------------------------------- /include/cgal_comm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_CGAL_COMM_H_ 21 | #define COVERAGEPLANNER_CGAL_COMM_H_ 22 | 23 | #include "cgal_definitions.h" 24 | 25 | namespace polygon_coverage_planning { 26 | 27 | // Helper to check whether a point is inside or on the boundary of the 28 | // polygon. 29 | bool pointInPolygon(const PolygonWithHoles& pwh, const Point_2& p); 30 | inline bool pointInPolygon(const Polygon_2& poly, const Point_2& p) { 31 | return pointInPolygon(PolygonWithHoles(poly), p); 32 | } 33 | bool pointsInPolygon(const PolygonWithHoles& pwh, 34 | const std::vector::iterator& begin, 35 | const std::vector::iterator& end); 36 | 37 | // Definition according to 38 | // https://doc.cgal.org/latest/Straight_skeleton_2/index.html 39 | bool isStrictlySimple(const PolygonWithHoles& pwh); 40 | 41 | // Project a point on a polygon. 42 | Point_2 projectOnPolygon2(const Polygon_2& poly, const Point_2& p, 43 | FT* squared_distance); 44 | // Project a point on the polygon boundary. 45 | Point_2 projectPointOnHull(const PolygonWithHoles& pwh, const Point_2& p); 46 | 47 | FT computeArea(const PolygonWithHoles& pwh); 48 | inline FT computeArea(const Polygon_2& poly) { 49 | return computeArea(PolygonWithHoles(poly)); 50 | } 51 | 52 | // Remove collinear vertices. 53 | void simplifyPolygon(Polygon_2* polygon); 54 | void simplifyPolygon(PolygonWithHoles* pwh); 55 | 56 | PolygonWithHoles rotatePolygon(const PolygonWithHoles& polygon_in, 57 | const Direction_2& dir); 58 | 59 | // Sort boundary to be counter-clockwise and holes to be clockwise. 60 | void sortVertices(PolygonWithHoles* pwh); 61 | 62 | std::vector getHullVertices(const PolygonWithHoles& pwh); 63 | std::vector> getHoleVertices(const PolygonWithHoles& pwh); 64 | 65 | } // namespace polygon_coverage_planning 66 | 67 | #endif // COVERAGEPLANNER_CGAL_COMM_H_ 68 | -------------------------------------------------------------------------------- /include/decomposition.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_DECOMPOSITION_H_ 21 | #define COVERAGEPLANNER_DECOMPOSITION_H_ 22 | 23 | #include "cgal_definitions.h" 24 | 25 | namespace polygon_coverage_planning { 26 | 27 | // Get all unique polygon edge directions including opposite directions. 28 | std::vector findEdgeDirections(const PolygonWithHoles& pwh); 29 | 30 | // Get all directions that are perpendicular to the edges found with 31 | // findEdgeDirections. 32 | std::vector findPerpEdgeDirections(const PolygonWithHoles& pwh); 33 | 34 | // Find the best edge direction to sweep. The best direction is the direction 35 | // with the smallest polygon altitude. Returns the smallest altitude. 36 | double findBestSweepDir(const Polygon_2& cell, Direction_2* best_dir = nullptr); 37 | 38 | // Compute BCDs for every edge direction. Return any with the smallest possible 39 | // altitude sum. 40 | bool computeBestBCDFromPolygonWithHoles(const PolygonWithHoles& pwh, 41 | std::vector* bcd_polygons); 42 | 43 | // Compute TCDs for every edge direction. Return any with the smallest possible 44 | // altitude sum. 45 | bool computeBestTCDFromPolygonWithHoles(const PolygonWithHoles& pwh, 46 | std::vector* trap_polygons); 47 | 48 | enum DecompositionType { 49 | kBCD = 0, // Boustrophedon. 50 | kTCD // Trapezoidal. 51 | }; 52 | 53 | inline bool checkDecompositionTypeValid(const int type) { 54 | return (type == DecompositionType::kBCD) || (type == DecompositionType::kTCD); 55 | } 56 | 57 | inline std::string getDecompositionTypeName(const DecompositionType& type) { 58 | switch (type) { 59 | case DecompositionType::kBCD: 60 | return "Boustrophedon Cell Decomposition"; 61 | case DecompositionType::kTCD: 62 | return "Trapezoidal Cell Decomposition"; 63 | default: 64 | return "Unknown!"; 65 | } 66 | } 67 | 68 | } // namespace polygon_coverage_planning 69 | 70 | #endif // COVERAGEPLANNER_DECOMPOSITION_H_ 71 | -------------------------------------------------------------------------------- /include/sweep.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_SWEEP_H_ 21 | #define COVERAGEPLANNER_SWEEP_H_ 22 | 23 | #include "cgal_definitions.h" 24 | #include "visibility_graph.h" 25 | 26 | namespace polygon_coverage_planning { 27 | 28 | // Compute the sweep by moving from the bottom to the top of the polygon. 29 | bool computeSweep(const Polygon_2& in, 30 | const visibility_graph::VisibilityGraph& visibility_graph, 31 | const FT offset, const Direction_2& dir, 32 | bool counter_clockwise, std::vector* waypoints); 33 | 34 | // Compute sweeps in all sweepable directions, starting counter-clockwise, 35 | // clockwise, and reverse. 36 | bool computeAllSweeps(const Polygon_2& poly, const double max_sweep_offset, 37 | std::vector>* cluster_sweeps); 38 | 39 | // A segment is observable if all vertices between two sweeps are observable. 40 | void checkObservability( 41 | const Segment_2& prev_sweep, const Segment_2& sweep, 42 | const std::vector& sorted_pts, const FT max_sq_distance, 43 | std::vector::const_iterator* lowest_unobservable_point); 44 | 45 | // Find the intersections between a polygon and a line and sort them by the 46 | // distance to the perpendicular direction of the line. 47 | std::vector findIntersections(const Polygon_2& p, const Line_2& l); 48 | 49 | // Same as findIntersections but only return first and last intersection. 50 | bool findSweepSegment(const Polygon_2& p, const Line_2& l, 51 | Segment_2* sweep_segment); 52 | 53 | // Sort vertices of polygon based on signed distance to line l. 54 | std::vector sortVerticesToLine(const Polygon_2& p, const Line_2& l); 55 | 56 | // Connect to points in the polygon using the visibility graph. 57 | bool calculateShortestPath( 58 | const visibility_graph::VisibilityGraph& visibility_graph, 59 | const Point_2& start, const Point_2& goal, 60 | std::vector* shortest_path); 61 | 62 | } // namespace polygon_coverage_planning 63 | 64 | #endif // COVERAGEPLANNER_SWEEP_H_ 65 | -------------------------------------------------------------------------------- /src/weakly_monotone.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include "weakly_monotone.h" 21 | 22 | namespace polygon_coverage_planning { 23 | 24 | bool isWeaklyMonotone(const Polygon_2& in, const Line_2& x_axis) { 25 | // Find north and south. 26 | VertexConstCirculator north = findNorth(in, x_axis); 27 | VertexConstCirculator south = findSouth(in, x_axis); 28 | 29 | // Go from south to north vertex. 30 | VertexConstCirculator c = south; 31 | VertexConstCirculator c_prev = south; 32 | for (c++; c != north; c++) { 33 | if (CGAL::has_smaller_signed_distance_to_line(x_axis, *c, *c_prev)) 34 | return false; 35 | c_prev = c; 36 | } 37 | 38 | // Go opposite direction. 39 | c = south; 40 | c_prev = south; 41 | for (c--; c != north; c--) { 42 | if (CGAL::has_smaller_signed_distance_to_line(x_axis, *c, *c_prev)) 43 | return false; 44 | c_prev = c; 45 | } 46 | 47 | return true; 48 | } 49 | 50 | std::vector getAllSweepableEdgeDirections(const Polygon_2& in) { 51 | // Get all directions. 52 | std::vector dirs; 53 | for (EdgeConstIterator it = in.edges_begin(); it != in.edges_end(); ++it) { 54 | // Check if this edge direction is already in the set. 55 | std::vector::iterator last = 56 | std::find_if(dirs.begin(), dirs.end(), [&it](const Direction_2& dir) { 57 | return CGAL::orientation(dir.vector(), it->to_vector()) == 58 | CGAL::COLLINEAR; 59 | }); 60 | if (last != dirs.end()) continue; 61 | // Check if the polygon is monotone perpendicular to this edge direction. 62 | if (isWeaklyMonotone(in, it->supporting_line())) 63 | dirs.push_back(it->direction()); 64 | } 65 | 66 | return dirs; 67 | } 68 | 69 | VertexConstCirculator findSouth(const Polygon_2& in, const Line_2& x_axis) { 70 | VertexConstCirculator vc = in.vertices_circulator(); 71 | VertexConstCirculator v = vc; 72 | do { 73 | v = CGAL::has_smaller_signed_distance_to_line(x_axis, *vc, *v) ? vc : v; 74 | } while (++vc != in.vertices_circulator()); 75 | return v; 76 | } 77 | 78 | VertexConstCirculator findNorth(const Polygon_2& in, const Line_2& x_axis) { 79 | return findSouth(in, x_axis.opposite()); 80 | } 81 | 82 | } // namespace polygon_coverage_planning 83 | -------------------------------------------------------------------------------- /include/visibility_graph.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef COVERAGEPLANNER_VISIBILITY_GRAPH_H_ 21 | #define COVERAGEPLANNER_VISIBILITY_GRAPH_H_ 22 | 23 | #include 24 | 25 | #include "graph_base.h" 26 | 27 | #include "cgal_definitions.h" 28 | 29 | namespace polygon_coverage_planning { 30 | namespace visibility_graph { 31 | 32 | struct NodeProperty { 33 | NodeProperty() : coordinates(Point_2(CGAL::ORIGIN)) {} 34 | NodeProperty(const Point_2& coordinates, const Polygon_2& visibility) 35 | : coordinates(coordinates), visibility(visibility) {} 36 | Point_2 coordinates; // The 2D coordinates. 37 | Polygon_2 visibility; // The visibile polygon from the vertex. 38 | }; 39 | 40 | struct EdgeProperty {}; 41 | 42 | // Shortest path calculation in the reduced visibility graph. 43 | // https://www.david-gouveia.com/pathfinding-on-a-2d-polygonal-map 44 | class VisibilityGraph : public GraphBase { 45 | public: 46 | // Creates an undirected, weighted visibility graph. 47 | VisibilityGraph(const PolygonWithHoles& polygon); 48 | VisibilityGraph(const Polygon_2& polygon) 49 | : VisibilityGraph(PolygonWithHoles(polygon)) {} 50 | 51 | VisibilityGraph() : GraphBase() {} 52 | 53 | virtual bool create() override; 54 | 55 | // Compute the shortest path in a polygon with holes using A* and the 56 | // precomputed visibility graph. 57 | // If start or goal are outside the polygon, they are snapped (projected) back 58 | // into it. 59 | bool solve(const Point_2& start, const Point_2& goal, 60 | std::vector* waypoints) const; 61 | // Same as solve but provide a precomputed visibility graph for the polygon. 62 | // Note: Start and goal need to be contained in the polygon_. 63 | bool solve(const Point_2& start, const Polygon_2& start_visibility_polygon, 64 | const Point_2& goal, const Polygon_2& goal_visibility_polygon, 65 | std::vector* waypoints) const; 66 | 67 | // Convenience function: addtionally adds original start and goal to shortest 68 | // path, if they were outside of polygon. 69 | bool solveWithOutsideStartAndGoal(const Point_2& start, const Point_2& goal, 70 | std::vector* waypoints) const; 71 | // Given a solution, get the concatenated 2D waypoints. 72 | bool getWaypoints(const Solution& solution, 73 | std::vector* waypoints) const; 74 | 75 | inline PolygonWithHoles getPolygon() const { return polygon_; } 76 | 77 | private: 78 | // Adds all line of sight neighbors. 79 | // The graph is acyclic and undirected and thus forms a symmetric adjacency 80 | // matrix. 81 | virtual bool addEdges() override; 82 | 83 | // Calculate the Euclidean distance to goal for all given nodes. 84 | virtual bool calculateHeuristic(size_t goal, 85 | Heuristic* heuristic) const override; 86 | 87 | // Find and append concave outer boundary vertices. 88 | void findConcaveOuterBoundaryVertices( 89 | std::vector* concave_vertices) const; 90 | // Find and append convex hole vertices. 91 | void findConvexHoleVertices( 92 | std::vector* convex_vertices) const; 93 | 94 | // Given two waypoints, compute its euclidean distance. 95 | double computeEuclideanSegmentCost(const Point_2& from, 96 | const Point_2& to) const; 97 | 98 | PolygonWithHoles polygon_; 99 | }; 100 | 101 | } // namespace visibility_graph 102 | } // namespace polygon_coverage_planning 103 | 104 | #endif /* COVERAGEPLANNER_VISIBILITY_GRAPH_H_ */ 105 | -------------------------------------------------------------------------------- /src/visibility_polygon.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "cgal_comm.h" 26 | #include "visibility_polygon.h" 27 | 28 | namespace polygon_coverage_planning { 29 | 30 | bool computeVisibilityPolygon(const PolygonWithHoles& pwh, 31 | const Point_2& query_point, 32 | Polygon_2* visibility_polygon) { 33 | 34 | // Preconditions. 35 | if(!pointInPolygon(pwh, query_point)){ 36 | std::cout<<"Query point outside of polygon."< VisibilityTraits; 45 | typedef CGAL::Arrangement_2 VisibilityArrangement; 46 | VisibilityArrangement poly; 47 | CGAL::insert(poly, pwh.outer_boundary().edges_begin(), 48 | pwh.outer_boundary().edges_end()); 49 | // Store main face. 50 | 51 | VisibilityArrangement::Face_const_handle main_face = poly.faces_begin(); 52 | while (main_face->is_unbounded()) { 53 | main_face++; 54 | } 55 | 56 | for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin(); 57 | hit != pwh.holes_end(); ++hit) 58 | CGAL::insert(poly, hit->edges_begin(), hit->edges_end()); 59 | 60 | // Create Triangular Expansion Visibility object. 61 | typedef CGAL::Triangular_expansion_visibility_2 63 | TEV; 64 | TEV tev(poly); 65 | 66 | // We need to determine the halfedge or face to which the query point 67 | // corresponds. 68 | typedef CGAL::Arr_naive_point_location NaivePL; 69 | typedef CGAL::Arr_point_location_result::Type PLResult; 70 | NaivePL pl(poly); 71 | PLResult pl_result = pl.locate(query_point); 72 | 73 | VisibilityArrangement::Vertex_const_handle* v = nullptr; 74 | VisibilityArrangement::Halfedge_const_handle* e = nullptr; 75 | VisibilityArrangement::Face_const_handle* f = nullptr; 76 | 77 | typedef VisibilityArrangement::Face_handle VisibilityFaceHandle; 78 | VisibilityFaceHandle fh; 79 | VisibilityArrangement visibility_arr; 80 | if ((f = boost::get(&pl_result))) { 81 | // Located in face. 82 | fh = tev.compute_visibility(query_point, *f, visibility_arr); 83 | } else if ((v = boost::get( 84 | &pl_result))) { 85 | // Located on vertex. 86 | // Search the incident halfedge that contains the polygon face. 87 | VisibilityArrangement::Halfedge_const_handle he = poly.halfedges_begin(); 88 | while ((he->target()->point() != (*v)->point()) || 89 | (he->face() != main_face)) { 90 | he++; 91 | if (he == poly.halfedges_end()) { 92 | std::cout<<"Cannot find halfedge corresponding to vertex."<( 99 | &pl_result))) { 100 | // Located on halfedge. 101 | // Find halfedge that has polygon interior as face. 102 | VisibilityArrangement::Halfedge_const_handle he = 103 | (*e)->face() == main_face ? (*e) : (*e)->twin(); 104 | fh = tev.compute_visibility(query_point, he, visibility_arr); 105 | } else { 106 | std::cout<<"Cannot locate query point on arrangement."<is_fictitious()) { 112 | std::cout<<"Visibility polygon is fictitious."<is_unbounded()) { 116 | std::cout<<"Visibility polygon is unbounded."<outer_ccb(); 122 | *visibility_polygon = Polygon_2(); 123 | do { 124 | visibility_polygon->push_back(curr->source()->point()); 125 | } while (++curr != fh->outer_ccb()); 126 | 127 | simplifyPolygon(visibility_polygon); 128 | if (visibility_polygon->is_clockwise_oriented()) 129 | visibility_polygon->reverse_orientation(); 130 | 131 | return true; 132 | } 133 | 134 | } // namespace polygon_coverage_planning 135 | -------------------------------------------------------------------------------- /include/graph_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_H_ 21 | #define POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // Utilities to create graphs. 29 | namespace polygon_coverage_planning { 30 | 31 | const double kToMilli = 1000; 32 | const double kFromMilli = 1.0 / kToMilli; 33 | 34 | // A doubly linked list representing a directed graph. 35 | // idx: node id 36 | // map pair first: neighbor id 37 | // map pair second: cost to go to neighbor 38 | typedef std::vector> Graph; 39 | 40 | // An edge id. 41 | // first: from node 42 | // second: to node 43 | typedef std::pair EdgeId; 44 | 45 | // An edge. 46 | typedef std::pair Edge; 47 | 48 | // The solution. 49 | typedef std::vector Solution; 50 | 51 | // A heuristic. 52 | // first: node 53 | // second: heuristic cost to goal 54 | typedef std::map Heuristic; 55 | 56 | // The base graph class. 57 | template 58 | class GraphBase { 59 | public: 60 | // A map from graph node id to node properties. 61 | using NodeProperties = std::map; 62 | // A map from graph edge id to edge properties. 63 | using EdgeProperties = std::map; 64 | 65 | GraphBase() 66 | : start_idx_(std::numeric_limits::max()), 67 | goal_idx_(std::numeric_limits::max()), 68 | is_created_(false){}; 69 | 70 | // Add a node. 71 | bool addNode(const NodeProperty& node_property); 72 | // Add a start node, that often follows special construction details. 73 | virtual bool addStartNode(const NodeProperty& node_property); 74 | // Add a goal node, that often follows special construction details. 75 | virtual bool addGoalNode(const NodeProperty& node_property); 76 | // Clear data structures. 77 | virtual void clear(); 78 | void clearEdges(); 79 | // Create graph given the internal settings. 80 | virtual bool create() = 0; 81 | 82 | inline size_t size() const { return graph_.size(); } 83 | inline size_t getNumberOfEdges() const { return edge_properties_.size(); } 84 | inline void reserve(size_t size) { graph_.reserve(size); } 85 | inline size_t getStartIdx() const { return start_idx_; } 86 | inline size_t getGoalIdx() const { return goal_idx_; } 87 | inline size_t isInitialized() const { return is_created_; } 88 | 89 | bool nodeExists(size_t node_id) const; 90 | bool nodePropertyExists(size_t node_id) const; 91 | bool edgeExists(const EdgeId& edge_id) const; 92 | bool edgePropertyExists(const EdgeId& edge_id) const; 93 | 94 | bool getEdgeCost(const EdgeId& edge_id, double* cost) const; 95 | const NodeProperty* getNodeProperty(size_t node_id) const; 96 | const EdgeProperty* getEdgeProperty(const EdgeId& edge_id) const; 97 | 98 | // Solve the graph with Dijkstra using arbitrary start and goal index. 99 | bool solveDijkstra(size_t start, size_t goal, Solution* solution) const; 100 | // Solve the graph with Dijkstra using internal start and goal index. 101 | bool solveDijkstra(Solution* solution) const; 102 | // Solve the graph with A* using arbitrary start and goal index. 103 | bool solveAStar(size_t start, size_t goal, Solution* solution) const; 104 | // Solve the graph with A* using internal start and goal index. 105 | bool solveAStar(Solution* solution) const; 106 | 107 | // Create the adjacency matrix setting no connectings to INT_MAX and 108 | // transforming cost into milli int. 109 | std::vector> getAdjacencyMatrix() const; 110 | 111 | // Preserving three decimal digits. 112 | inline int doubleToMilliInt(double in) const { 113 | return static_cast(std::round(in * kToMilli)); 114 | } 115 | inline double milliIntToDouble(int in) const { 116 | return static_cast(in) * kFromMilli; 117 | } 118 | 119 | protected: 120 | // Called from addNode. Creates all edges to the node at the back of the 121 | // graph. 122 | virtual bool addEdges() = 0; 123 | // Given the goal, calculate and set the heuristic for all nodes in the graph. 124 | virtual bool calculateHeuristic(size_t goal, Heuristic* heuristic) const; 125 | 126 | bool addEdge(const EdgeId& edge_id, const EdgeProperty& edge_property, 127 | double cost); 128 | 129 | Solution reconstructSolution(const std::map& came_from, 130 | size_t current) const; 131 | 132 | Graph graph_; 133 | // Map to store all node properties. Key is the graph node id. 134 | NodeProperties node_properties_; 135 | // Map to store all edge properties. Key is the graph edge id. 136 | EdgeProperties edge_properties_; 137 | size_t start_idx_; 138 | size_t goal_idx_; 139 | bool is_created_; 140 | }; 141 | } // namespace polygon_coverage_planning 142 | 143 | #include "graph_base_impl.h" 144 | 145 | #endif // POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_H_ 146 | -------------------------------------------------------------------------------- /src/decomposition.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include "bcd.h" 21 | #include "decomposition.h" 22 | #include "tcd.h" 23 | #include "weakly_monotone.h" 24 | 25 | 26 | namespace polygon_coverage_planning { 27 | 28 | std::vector findEdgeDirections(const PolygonWithHoles& pwh) { 29 | // Get all possible polygon directions. 30 | std::vector directions; 31 | for (size_t i = 0; i < pwh.outer_boundary().size(); ++i) { 32 | directions.push_back(pwh.outer_boundary().edge(i).direction()); 33 | } 34 | for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin(); 35 | hit != pwh.holes_end(); ++hit) { 36 | for (size_t i = 0; i < hit->size(); i++) { 37 | directions.push_back(hit->edge(i).direction()); 38 | } 39 | } 40 | 41 | // Remove redundant directions. 42 | std::set to_remove; 43 | for (size_t i = 0; i < directions.size() - 1; ++i) { 44 | for (size_t j = i + 1; j < directions.size(); ++j) { 45 | if (CGAL::orientation(directions[i].vector(), directions[j].vector()) == 46 | CGAL::COLLINEAR) 47 | to_remove.insert(j); 48 | } 49 | } 50 | for (std::set::reverse_iterator rit = to_remove.rbegin(); 51 | rit != to_remove.rend(); ++rit) { 52 | directions.erase(std::next(directions.begin(), *rit)); 53 | } 54 | 55 | // Add opposite directions. 56 | std::vector temp_directions = directions; 57 | for (size_t i = 0; i < temp_directions.size(); ++i) { 58 | directions.push_back(-temp_directions[i]); 59 | } 60 | 61 | return directions; 62 | } 63 | 64 | std::vector findPerpEdgeDirections(const PolygonWithHoles& pwh) { 65 | std::vector directions = findEdgeDirections(pwh); 66 | for (auto& d : directions) { 67 | d = Direction_2(-d.dy(), d.dx()); 68 | } 69 | 70 | return directions; 71 | } 72 | 73 | double findBestSweepDir(const Polygon_2& cell, Direction_2* best_dir) { 74 | // Get all sweepable edges. 75 | PolygonWithHoles pwh(cell); 76 | std::vector edge_dirs = getAllSweepableEdgeDirections(cell); 77 | 78 | // Find minimum altitude. 79 | double min_altitude = std::numeric_limits::max(); 80 | for (const auto& dir : edge_dirs) { 81 | auto s = findSouth(cell, Line_2(Point_2(CGAL::ORIGIN), dir)); 82 | auto n = findNorth(cell, Line_2(Point_2(CGAL::ORIGIN), dir)); 83 | auto orthogonal_vec = 84 | dir.vector().perpendicular(CGAL::Orientation::POSITIVE); 85 | Line_2 line_through_n(*n, orthogonal_vec.direction()); 86 | auto s_proj = line_through_n.projection(*s); 87 | double altitude = 88 | std::sqrt(CGAL::to_double(CGAL::squared_distance(*n, s_proj))); 89 | 90 | if (altitude < min_altitude) { 91 | min_altitude = altitude; 92 | if (best_dir) *best_dir = dir; 93 | } 94 | } 95 | 96 | return min_altitude; 97 | } 98 | 99 | bool computeBestBCDFromPolygonWithHoles(const PolygonWithHoles& pwh, 100 | std::vector* bcd_polygons) { 101 | bcd_polygons->clear(); 102 | double min_altitude_sum = std::numeric_limits::max(); 103 | 104 | // Get all possible decomposition directions. 105 | std::vector directions = findPerpEdgeDirections(pwh); 106 | 107 | // For all possible rotations: 108 | for (const auto& dir : directions) { 109 | // Calculate decomposition. 110 | std::vector cells = computeBCD(pwh, dir); 111 | 112 | // Calculate minimum altitude sum for each cell. 113 | double min_altitude_sum_tmp = 0.0; 114 | for (const auto& cell : cells) { 115 | min_altitude_sum_tmp += findBestSweepDir(cell); 116 | } 117 | 118 | // Update best decomposition. 119 | if (min_altitude_sum_tmp < min_altitude_sum) { 120 | min_altitude_sum = min_altitude_sum_tmp; 121 | *bcd_polygons = cells; 122 | } 123 | } 124 | 125 | if (bcd_polygons->empty()) 126 | return false; 127 | else 128 | return true; 129 | } 130 | 131 | bool computeBestTCDFromPolygonWithHoles(const PolygonWithHoles& pwh, 132 | std::vector* tcd_polygons) { 133 | tcd_polygons->clear(); 134 | double min_altitude_sum = std::numeric_limits::max(); 135 | 136 | // Get all possible decomposition directions. 137 | std::vector directions = findPerpEdgeDirections(pwh); 138 | 139 | // For all possible rotations: 140 | for (const auto& dir : directions) { 141 | // Calculate decomposition. 142 | std::vector cells = computeTCD(pwh, dir); 143 | 144 | // Calculate minimum altitude sum for each cell. 145 | double min_altitude_sum_tmp = 0.0; 146 | for (const auto& cell : cells) { 147 | min_altitude_sum_tmp += findBestSweepDir(cell); 148 | } 149 | 150 | // Update best decomposition. 151 | if (min_altitude_sum_tmp < min_altitude_sum) { 152 | min_altitude_sum = min_altitude_sum_tmp; 153 | *tcd_polygons = cells; 154 | } 155 | } 156 | 157 | if (tcd_polygons->empty()) 158 | return false; 159 | else 160 | return true; 161 | } 162 | 163 | } // namespace polygon_coverage_planning 164 | -------------------------------------------------------------------------------- /src/cgal_comm.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include "cgal_comm.h" 21 | 22 | namespace polygon_coverage_planning { 23 | 24 | bool pointInPolygon(const PolygonWithHoles& pwh, const Point_2& p) { 25 | // Point inside outer boundary. 26 | CGAL::Bounded_side result = 27 | CGAL::bounded_side_2(pwh.outer_boundary().vertices_begin(), 28 | pwh.outer_boundary().vertices_end(), p, K()); 29 | if (result == CGAL::ON_UNBOUNDED_SIDE) return false; 30 | 31 | // Point outside hole. 32 | for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin(); 33 | hit != pwh.holes_end(); ++hit) { 34 | result = CGAL::bounded_side_2(hit->vertices_begin(), hit->vertices_end(), p, 35 | K()); 36 | if (result == CGAL::ON_BOUNDED_SIDE) return false; 37 | } 38 | 39 | return true; 40 | } 41 | 42 | bool pointsInPolygon(const PolygonWithHoles& pwh, 43 | const std::vector::iterator& begin, 44 | const std::vector::iterator& end) { 45 | for (std::vector::iterator it = begin; it != end; ++it) { 46 | if (!pointInPolygon(pwh, *it)) return false; 47 | } 48 | return true; 49 | } 50 | 51 | bool isStrictlySimple(const PolygonWithHoles& pwh) { 52 | for (PolygonWithHoles::Hole_const_iterator hi = pwh.holes_begin(); 53 | hi != pwh.holes_end(); ++hi) 54 | if (!hi->is_simple()) return false; 55 | return pwh.outer_boundary().is_simple(); 56 | } 57 | 58 | Point_2 projectOnPolygon2(const Polygon_2& poly, const Point_2& p, 59 | FT* squared_distance) { 60 | 61 | // Find the closest edge. 62 | std::vector> edge_distances(poly.size()); 63 | std::vector>::iterator dit = 64 | edge_distances.begin(); 65 | for (EdgeConstIterator eit = poly.edges_begin(); eit != poly.edges_end(); 66 | eit++, dit++) { 67 | dit->first = CGAL::squared_distance(*eit, p); 68 | dit->second = eit; 69 | } 70 | 71 | std::vector>::iterator closest_pair = 72 | std::min_element(edge_distances.begin(), edge_distances.end(), 73 | [](const std::pair& lhs, 74 | const std::pair& rhs) { 75 | return lhs.first < rhs.first; 76 | }); 77 | 78 | EdgeConstIterator closest_edge = closest_pair->second; 79 | *squared_distance = closest_pair->first; 80 | 81 | // Project p on supporting line of closest edge. 82 | Point_2 projection = closest_edge->supporting_line().projection(p); 83 | // Check if p is on edge. If not snap it to source or target. 84 | if (!closest_edge->has_on(projection)) { 85 | FT d_source = CGAL::squared_distance(p, closest_edge->source()); 86 | FT d_target = CGAL::squared_distance(p, closest_edge->target()); 87 | projection = 88 | d_source < d_target ? closest_edge->source() : closest_edge->target(); 89 | } 90 | 91 | return projection; 92 | } 93 | 94 | Point_2 projectPointOnHull(const PolygonWithHoles& pwh, const Point_2& p) { 95 | // Project point on outer boundary. 96 | FT min_distance; 97 | Point_2 projection = 98 | projectOnPolygon2(pwh.outer_boundary(), p, &min_distance); 99 | 100 | // Project on holes. 101 | for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin(); 102 | hit != pwh.holes_end(); ++hit) { 103 | FT temp_distance; 104 | Point_2 temp_projection = projectOnPolygon2(*hit, p, &temp_distance); 105 | if (temp_distance < min_distance) { 106 | min_distance = temp_distance; 107 | projection = temp_projection; 108 | } 109 | } 110 | 111 | return projection; 112 | } 113 | 114 | FT computeArea(const PolygonWithHoles& pwh) { 115 | FT area = 116 | CGAL::abs(CGAL::polygon_area_2(pwh.outer_boundary().vertices_begin(), 117 | pwh.outer_boundary().vertices_end(), K())); 118 | for (PolygonWithHoles::Hole_const_iterator hi = pwh.holes_begin(); 119 | hi != pwh.holes_end(); ++hi) 120 | area -= CGAL::abs( 121 | CGAL::polygon_area_2(hi->vertices_begin(), hi->vertices_end(), K())); 122 | return area; 123 | } 124 | 125 | void simplifyPolygon(Polygon_2* polygon) { 126 | 127 | std::vector v_to_erase; 128 | 129 | Polygon_2::Vertex_circulator vc = polygon->vertices_circulator(); 130 | // Find collinear vertices. 131 | do { 132 | if (CGAL::collinear(*std::prev(vc), *vc, *std::next(vc))) { 133 | v_to_erase.push_back(vc); 134 | } 135 | } while (++vc != polygon->vertices_circulator()); 136 | 137 | // Remove intermediate vertices. 138 | for (std::vector::reverse_iterator rit = 139 | v_to_erase.rbegin(); 140 | rit != v_to_erase.rend(); ++rit) { 141 | polygon->erase(*rit); 142 | } 143 | } 144 | 145 | void simplifyPolygon(PolygonWithHoles* pwh) { 146 | 147 | simplifyPolygon(&pwh->outer_boundary()); 148 | 149 | for (PolygonWithHoles::Hole_iterator hi = pwh->holes_begin(); 150 | hi != pwh->holes_end(); ++hi) 151 | simplifyPolygon(&*hi); 152 | } 153 | 154 | PolygonWithHoles rotatePolygon(const PolygonWithHoles& polygon_in, 155 | const Direction_2& dir) { 156 | CGAL::Aff_transformation_2 rotation(CGAL::ROTATION, dir, 1, 1e9); 157 | rotation = rotation.inverse(); 158 | PolygonWithHoles rotated_polygon = polygon_in; 159 | rotated_polygon.outer_boundary() = 160 | CGAL::transform(rotation, polygon_in.outer_boundary()); 161 | PolygonWithHoles::Hole_iterator hit_rot = rotated_polygon.holes_begin(); 162 | for (PolygonWithHoles::Hole_const_iterator hit = polygon_in.holes_begin(); 163 | hit != polygon_in.holes_end(); ++hit) { 164 | *(hit_rot++) = CGAL::transform(rotation, *hit); 165 | } 166 | 167 | return rotated_polygon; 168 | } 169 | 170 | void sortVertices(PolygonWithHoles* pwh) { 171 | if (pwh->outer_boundary().is_clockwise_oriented()) 172 | pwh->outer_boundary().reverse_orientation(); 173 | 174 | for (PolygonWithHoles::Hole_iterator hi = pwh->holes_begin(); 175 | hi != pwh->holes_end(); ++hi) 176 | if (hi->is_counterclockwise_oriented()) hi->reverse_orientation(); 177 | } 178 | 179 | std::vector getHullVertices(const PolygonWithHoles& pwh) { 180 | std::vector vec(pwh.outer_boundary().size()); 181 | std::vector::iterator vecit = vec.begin(); 182 | for (VertexConstIterator vit = pwh.outer_boundary().vertices_begin(); 183 | vit != pwh.outer_boundary().vertices_end(); ++vit, ++vecit) 184 | *vecit = *vit; 185 | return vec; 186 | } 187 | 188 | std::vector> getHoleVertices(const PolygonWithHoles& pwh) { 189 | std::vector> hole_vertices(pwh.number_of_holes()); 190 | std::vector>::iterator hvit = hole_vertices.begin(); 191 | for (PolygonWithHoles::Hole_const_iterator hi = pwh.holes_begin(); 192 | hi != pwh.holes_end(); ++hi, ++hvit) { 193 | hvit->resize(hi->size()); 194 | std::vector::iterator it = hvit->begin(); 195 | for (VertexConstIterator vit = hi->vertices_begin(); 196 | vit != hi->vertices_end(); ++vit, ++it) 197 | *it = *vit; 198 | } 199 | return hole_vertices; 200 | } 201 | 202 | } // namespace polygon_coverage_planning 203 | -------------------------------------------------------------------------------- /include/coverage_planner.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by huangxh on 20-7-31. 3 | // 4 | 5 | #ifndef COVERAGEPLANNER_COVERAGE_PLANNER_H 6 | #define COVERAGEPLANNER_COVERAGE_PLANNER_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "opencv2/core/core.hpp" 13 | #include "opencv2/highgui/highgui.hpp" 14 | #include "opencv2/imgproc/imgproc.hpp" 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "cgal_comm.h" 21 | #include "decomposition.h" 22 | #include "sweep.h" 23 | 24 | class MouseParams{ 25 | public: 26 | MouseParams(const cv::Mat& img_){ 27 | img = img_.clone(); 28 | point = Point_2(-1, -1); 29 | } 30 | Point_2 point; 31 | cv::Mat img; 32 | }; 33 | 34 | void onMouseHandle(int event, int x, int y, int flags, void* params) 35 | { 36 | MouseParams* mp = (MouseParams*)params; 37 | switch (event) 38 | { 39 | case cv::EVENT_LBUTTONDOWN: 40 | { 41 | cv::circle(mp->img, cv::Point(x,y), 3, cv::Scalar(0, 64, 255), -1); 42 | cv::imshow("select start point", mp->img); 43 | std::cout<<" x: "<point = Point_2(x, y); 45 | break; 46 | } 47 | default: 48 | { 49 | break; 50 | } 51 | } 52 | } 53 | 54 | Point_2 getStartingPoint(cv::Mat& img) 55 | { 56 | MouseParams params(img); 57 | params.img = img.clone(); 58 | 59 | cv::namedWindow("select start point", cv::WINDOW_AUTOSIZE); 60 | cv::imshow("select start point", params.img); 61 | cv::setMouseCallback("select start point", onMouseHandle, (void*)&(params)); 62 | cv::waitKey(); 63 | Point_2 point = params.point; 64 | 65 | img = params.img.clone(); 66 | 67 | cv::destroyWindow("select start point"); 68 | return point; 69 | } 70 | 71 | class CellNode 72 | { 73 | public: 74 | CellNode() 75 | { 76 | isVisited = false; 77 | isCleaned = false; 78 | parentIndex = INT_MAX; 79 | cellIndex = INT_MAX; 80 | } 81 | bool isVisited; 82 | bool isCleaned; 83 | 84 | int parentIndex; 85 | std::deque neighbor_indices; 86 | 87 | int cellIndex; 88 | }; 89 | 90 | std::vector calculateDecompositionAdjacency(const std::vector& decompositions) { 91 | 92 | std::vector polygon_adj_graph(decompositions.size()); 93 | for (size_t i = 0; i < decompositions.size() - 1; ++i) { 94 | polygon_adj_graph[i].cellIndex = i; 95 | for (size_t j = i + 1; j < decompositions.size(); ++j) { 96 | PolygonWithHoles joined; 97 | if (CGAL::join(decompositions[i], decompositions[j], joined)) { 98 | polygon_adj_graph[i].neighbor_indices.emplace_back(j); 99 | polygon_adj_graph[j].neighbor_indices.emplace_back(i); 100 | } 101 | } 102 | } 103 | polygon_adj_graph.back().cellIndex = decompositions.size()-1; 104 | 105 | return polygon_adj_graph; 106 | } 107 | 108 | // DFS 109 | void walkThroughGraph(std::vector& cell_graph, int cell_index, int& unvisited_counter, std::deque& path) 110 | { 111 | if(!cell_graph[cell_index].isVisited){ 112 | cell_graph[cell_index].isVisited = true; 113 | unvisited_counter--; 114 | } 115 | path.emplace_front(cell_graph[cell_index]); 116 | 117 | // for debugging 118 | // std::cout<< "cell: " < getTravellingPath(const std::vector& cell_graph, int first_cell_index) 151 | { 152 | std::deque travelling_path; 153 | 154 | std::deque _cell_path; 155 | std::vector _cell_graph = cell_graph; 156 | 157 | if(_cell_graph.size()==1){ 158 | travelling_path.emplace_back(0); 159 | }else{ 160 | int unvisited_counter = _cell_graph.size(); 161 | walkThroughGraph(_cell_graph, first_cell_index, unvisited_counter, _cell_path); 162 | std::reverse(_cell_path.begin(), _cell_path.end()); 163 | } 164 | 165 | for(auto& cell : _cell_path){ 166 | travelling_path.emplace_back(cell.cellIndex); 167 | } 168 | 169 | return travelling_path; 170 | } 171 | 172 | std::vector>> calculateCellIntersections(std::vector& decompositions, std::vector& cell_graph){ 173 | 174 | std::vector>> cell_intersections(cell_graph.size()); 175 | 176 | for(size_t i = 0; i < cell_graph.size(); ++i){ 177 | for(size_t j = 0; j < cell_graph[i].neighbor_indices.size(); ++j){ 178 | std::list pts; 179 | for(auto m = decompositions[i].edges_begin(); m != decompositions[i].edges_end(); ++m){ 180 | for(auto n = decompositions[cell_graph[i].neighbor_indices[j]].edges_begin(); 181 | n != decompositions[cell_graph[i].neighbor_indices[j]].edges_end(); 182 | ++n){ 183 | Segment_2 segments[] = {*m, *n}; 184 | CGAL::compute_intersection_points(segments, segments+2, std::back_inserter(pts)); 185 | } 186 | } 187 | 188 | for(auto p = decompositions[i].vertices_begin(); p != decompositions[i].vertices_end(); ++p){ 189 | for(auto q = decompositions[cell_graph[i].neighbor_indices[j]].vertices_begin(); q != decompositions[cell_graph[i].neighbor_indices[j]].vertices_end(); ++q){ 190 | if(CGAL::to_double(p->x())==CGAL::to_double(q->x()) && CGAL::to_double(p->y())==CGAL::to_double(q->y())){ 191 | pts.insert(pts.end(), *p); 192 | } 193 | } 194 | } 195 | 196 | auto verbose = std::unique(pts.begin(), pts.end()); 197 | pts.erase(verbose, pts.end()); 198 | cell_intersections[i].insert(std::make_pair(cell_graph[i].neighbor_indices[j], pts)); 199 | cell_intersections[cell_graph[i].neighbor_indices[j]].insert(std::make_pair(i, pts)); 200 | } 201 | } 202 | 203 | return cell_intersections; 204 | } 205 | 206 | Point_2 findNextGoal(const Point_2& start, const Point_2& goal, const std::list& candidates){ 207 | double min_cost = DBL_MAX; 208 | double cost; 209 | Point_2 next_point = start; 210 | Segment_2 seg_from_start, seg_to_goal; 211 | for(auto point = candidates.begin(); point != candidates.end(); ++point){ 212 | seg_from_start = Segment_2(start, *point); 213 | seg_to_goal = Segment_2(*point, goal); 214 | cost = CGAL::to_double(seg_from_start.squared_length())+CGAL::to_double(seg_to_goal.squared_length()); 215 | if(cost < min_cost){ 216 | min_cost = cost; 217 | next_point = *point; 218 | } 219 | } 220 | return next_point; 221 | } 222 | 223 | bool doReverseNextSweep(const Point_2& curr_point, const std::vector& next_sweep){ 224 | return CGAL::to_double(CGAL::squared_distance(curr_point, next_sweep.front())) > CGAL::to_double(CGAL::squared_distance(curr_point, next_sweep.back())); 225 | } 226 | 227 | std::vector getShortestPath(const Polygon_2& polygon, const Point_2& start, const Point_2& goal){ 228 | polygon_coverage_planning::visibility_graph::VisibilityGraph vis_graph(polygon); 229 | std::vector shortest_path; 230 | polygon_coverage_planning::calculateShortestPath(vis_graph, start, goal, &shortest_path); 231 | return shortest_path; 232 | } 233 | 234 | int getCellIndexOfPoint(const std::vector& decompositions, const Point_2& point){ 235 | int index = -1; 236 | for(int i = 0; i < decompositions.size(); i++){ 237 | if(polygon_coverage_planning::pointInPolygon(decompositions[i], point)){ 238 | index = i; 239 | break; 240 | } 241 | } 242 | return index; 243 | } 244 | 245 | #endif //COVERAGEPLANNER_COVERAGE_PLANNER_H 246 | -------------------------------------------------------------------------------- /src/visibility_graph.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include "cgal_comm.h" 21 | #include "visibility_graph.h" 22 | #include "visibility_polygon.h" 23 | 24 | 25 | namespace polygon_coverage_planning { 26 | namespace visibility_graph { 27 | 28 | VisibilityGraph::VisibilityGraph(const PolygonWithHoles& polygon) 29 | : GraphBase(), polygon_(polygon) { 30 | // Build visibility graph. 31 | is_created_ = create(); 32 | } 33 | 34 | bool VisibilityGraph::create() { 35 | clear(); 36 | // Sort vertices. 37 | sortVertices(&polygon_); 38 | // Select shortest path vertices. 39 | std::vector graph_vertices; 40 | findConcaveOuterBoundaryVertices(&graph_vertices); 41 | findConvexHoleVertices(&graph_vertices); 42 | 43 | for (const VertexConstCirculator& v : graph_vertices) { 44 | // Compute visibility polygon. 45 | Polygon_2 visibility; 46 | if (!computeVisibilityPolygon(polygon_, *v, &visibility)) { 47 | std::cout<<"Cannot compute visibility polygon."<* concave_vertices) const { 64 | 65 | VertexConstCirculator vit = polygon_.outer_boundary().vertices_circulator(); 66 | do { 67 | Triangle_2 triangle(*std::prev(vit), *vit, *std::next(vit)); 68 | CGAL::Orientation orientation = triangle.orientation(); 69 | 70 | if (orientation == CGAL::CLOCKWISE) concave_vertices->push_back(vit); 71 | } while (++vit != polygon_.outer_boundary().vertices_circulator()); 72 | } 73 | 74 | void VisibilityGraph::findConvexHoleVertices( 75 | std::vector* convex_vertices) const { 76 | 77 | 78 | for (PolygonWithHoles::Hole_const_iterator hit = polygon_.holes_begin(); 79 | hit != polygon_.holes_end(); ++hit) { 80 | 81 | VertexConstCirculator vit = hit->vertices_circulator(); 82 | do { 83 | Triangle_2 triangle(*std::prev(vit), *vit, *std::next(vit)); 84 | CGAL::Orientation orientation = triangle.orientation(); 85 | 86 | if (orientation == CGAL::CLOCKWISE) convex_vertices->push_back(vit); 87 | } while (++vit != hit->vertices_circulator()); 88 | } 89 | } 90 | 91 | bool VisibilityGraph::addEdges() { 92 | if (graph_.empty()) { 93 | std::cout<<"Cannot add edges to an empty graph."<visibility, 106 | adj_node_property->coordinates)) { 107 | EdgeId forwards_edge_id(new_id, adj_id); 108 | EdgeId backwards_edge_id(adj_id, new_id); 109 | const double cost = computeEuclideanSegmentCost( 110 | new_node_property->coordinates, 111 | adj_node_property->coordinates); // Symmetric cost. 112 | if (!addEdge(forwards_edge_id, EdgeProperty(), cost) || 113 | !addEdge(backwards_edge_id, EdgeProperty(), cost)) { 114 | return false; 115 | } 116 | } 117 | } 118 | return true; 119 | } 120 | 121 | bool VisibilityGraph::solve(const Point_2& start, const Point_2& goal, 122 | std::vector* waypoints) const { 123 | 124 | waypoints->clear(); 125 | 126 | // Make sure start and end are inside the polygon. 127 | const Point_2 start_new = pointInPolygon(polygon_, start) 128 | ? start 129 | : projectPointOnHull(polygon_, start); 130 | const Point_2 goal_new = pointInPolygon(polygon_, goal) 131 | ? goal 132 | : projectPointOnHull(polygon_, goal); 133 | 134 | // Compute start and goal visibility polygon. 135 | Polygon_2 start_visibility, goal_visibility; 136 | if (!computeVisibilityPolygon(polygon_, start_new, &start_visibility) || 137 | !computeVisibilityPolygon(polygon_, goal_new, &goal_visibility)) { 138 | return false; 139 | } 140 | 141 | // Find shortest path. 142 | return solve(start_new, start_visibility, goal_new, goal_visibility, 143 | waypoints); 144 | } 145 | 146 | bool VisibilityGraph::solve(const Point_2& start, 147 | const Polygon_2& start_visibility_polygon, 148 | const Point_2& goal, 149 | const Polygon_2& goal_visibility_polygon, 150 | std::vector* waypoints) const { 151 | 152 | waypoints->clear(); 153 | 154 | if (!is_created_) { 155 | std::cout<<"Visibility graph not initialized."<visibility, goal)) { 181 | waypoints->push_back(start); 182 | waypoints->push_back(goal); 183 | return true; 184 | } 185 | 186 | // Find shortest way using A*. 187 | Solution solution; 188 | if (!temp_visibility_graph.solveAStar(start_idx, goal_idx, &solution)) { 189 | std::cout<< 190 | "Could not find shortest path. Graph not fully connected."<* waypoints) const { 200 | 201 | waypoints->resize(solution.size()); 202 | for (size_t i = 0; i < solution.size(); i++) { 203 | const NodeProperty* node_property = getNodeProperty(solution[i]); 204 | if (node_property == nullptr) { 205 | std::cout<<"Cannot reconstruct solution."<coordinates; 209 | } 210 | return true; 211 | } 212 | 213 | bool VisibilityGraph::calculateHeuristic(size_t goal, 214 | Heuristic* heuristic) const { 215 | 216 | heuristic->clear(); 217 | 218 | const NodeProperty* goal_node_property = getNodeProperty(goal); 219 | if (goal_node_property == nullptr) { 220 | std::cout<<"Cannot find goal node property to calculate heuristic."<coordinates, goal_node_property->coordinates); 233 | } 234 | 235 | return true; 236 | } 237 | 238 | bool VisibilityGraph::solveWithOutsideStartAndGoal( 239 | const Point_2& start, const Point_2& goal, 240 | std::vector* waypoints) const { 241 | 242 | 243 | if (solve(start, goal, waypoints)) { 244 | if (!pointInPolygon(polygon_, start)) { 245 | waypoints->insert(waypoints->begin(), start); 246 | } 247 | if (!pointInPolygon(polygon_, goal)) { 248 | waypoints->push_back(goal); 249 | } 250 | return true; 251 | } else { 252 | return false; 253 | } 254 | } 255 | 256 | double VisibilityGraph::computeEuclideanSegmentCost(const Point_2& from, 257 | const Point_2& to) const { 258 | return std::sqrt(CGAL::to_double(Segment_2(from, to).squared_length())); 259 | } 260 | 261 | } // namespace visibility_graph 262 | } // namespace polygon_coverage_planning 263 | -------------------------------------------------------------------------------- /src/sweep.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include "sweep.h" 21 | #include "visibility_polygon.h" 22 | #include "weakly_monotone.h" 23 | 24 | namespace polygon_coverage_planning { 25 | 26 | bool computeSweep(const Polygon_2& in, 27 | const visibility_graph::VisibilityGraph& visibility_graph, 28 | const FT offset, const Direction_2& dir, 29 | bool counter_clockwise, std::vector* waypoints) { 30 | waypoints->clear(); 31 | const FT kSqOffset = offset * offset; 32 | 33 | // Assertions. 34 | // TODO(rikba): Check monotone perpendicular to dir. 35 | if (!in.is_counterclockwise_oriented()) return false; 36 | 37 | // Find start sweep. 38 | Line_2 sweep(Point_2(0.0, 0.0), dir); 39 | std::vector sorted_pts = sortVerticesToLine(in, sweep); 40 | sweep = Line_2(sorted_pts.front(), dir); 41 | // rotate sweep by 90 degree 42 | Vector_2 offset_vector = sweep.perpendicular(sorted_pts.front()).to_vector(); 43 | offset_vector = offset * offset_vector / 44 | std::sqrt(CGAL::to_double(offset_vector.squared_length())); 45 | const CGAL::Aff_transformation_2 kOffset(CGAL::TRANSLATION, offset_vector); 46 | 47 | Segment_2 sweep_segment; 48 | bool has_sweep_segment = findSweepSegment(in, sweep, &sweep_segment); 49 | while (has_sweep_segment) { 50 | // Align sweep segment. 51 | if (counter_clockwise) sweep_segment = sweep_segment.opposite(); 52 | // Connect previous sweep. 53 | if (!waypoints->empty()) { 54 | std::vector shortest_path; 55 | if (!calculateShortestPath(visibility_graph, waypoints->back(), 56 | sweep_segment.source(), &shortest_path)) 57 | return false; 58 | for (std::vector::iterator it = std::next(shortest_path.begin()); 59 | it != std::prev(shortest_path.end()); ++it) { 60 | waypoints->push_back(*it); 61 | } 62 | } 63 | // Traverse sweep. 64 | waypoints->push_back(sweep_segment.source()); 65 | if (!sweep_segment.is_degenerate()) 66 | waypoints->push_back(sweep_segment.target()); 67 | 68 | // Offset sweep. 69 | sweep = sweep.transform(kOffset); 70 | // Find new sweep segment. 71 | Segment_2 prev_sweep_segment = 72 | counter_clockwise ? sweep_segment.opposite() : sweep_segment; 73 | has_sweep_segment = findSweepSegment(in, sweep, &sweep_segment); 74 | // Add a final sweep. 75 | // 当前的slice没切到polygon，同时在polygon中的最后一个slice已经遍历过（两个交点），到达终止条件 76 | if (!has_sweep_segment && 77 | !((!waypoints->empty() && 78 | *std::prev(waypoints->end(), 1) == sorted_pts.back()) || 79 | (waypoints->size() > 1 && 80 | *std::prev(waypoints->end(), 2) == sorted_pts.back()))) { 81 | // 取polygon中的最后一个slice 82 | sweep = Line_2(sorted_pts.back(), dir); 83 | has_sweep_segment = findSweepSegment(in, sweep, &sweep_segment); 84 | // 没切到polygon的不要 85 | if (!has_sweep_segment) { 86 | std::cout<<"Failed to calculate final sweep."<::const_iterator unobservable_point = 97 | sorted_pts.end(); 98 | checkObservability(prev_sweep_segment, sweep_segment, sorted_pts, 99 | kSqOffset, &unobservable_point); 100 | if (unobservable_point != sorted_pts.end()) { 101 | // 102 | sweep = Line_2(*unobservable_point, dir); 103 | has_sweep_segment = findSweepSegment(in, sweep, &sweep_segment); 104 | if (!has_sweep_segment) { 105 | std::cout<<"Failed to calculate extra sweep at point: " 106 | << *unobservable_point< intersections = findIntersections(p, l); 122 | if (intersections.empty()) return false; 123 | *sweep_segment = Segment_2(intersections.front(), intersections.back()); 124 | return true; 125 | } 126 | 127 | void checkObservability( 128 | const Segment_2& prev_sweep, const Segment_2& sweep, 129 | const std::vector& sorted_pts, const FT max_sq_distance, 130 | std::vector::const_iterator* lowest_unobservable_point) { 131 | *lowest_unobservable_point = sorted_pts.end(); 132 | 133 | // Find first point that is between prev_sweep and sweep and unobservable. 134 | for (std::vector::const_iterator it = sorted_pts.begin(); 135 | it != sorted_pts.end(); ++it) { 136 | // 左边/逆时针 = 正； 右边/顺时针 = 负 137 | if (prev_sweep.supporting_line().has_on_positive_side(*it)) continue; 138 | if (sweep.supporting_line().has_on_negative_side(*it)) { 139 | break; 140 | } 141 | // 在sweep和presweep之间才计算，要是没有符合条件的，it就是初始化状态，指向polygon的最后一个slice 142 | FT sq_distance_prev = CGAL::squared_distance(prev_sweep, *it); 143 | FT sq_distance_curr = CGAL::squared_distance(sweep, *it); 144 | if (sq_distance_prev > max_sq_distance && 145 | sq_distance_curr > max_sq_distance) { 146 | *lowest_unobservable_point = it; 147 | return; 148 | } 149 | } 150 | } 151 | 152 | bool computeAllSweeps(const Polygon_2& poly, const double max_sweep_offset, 153 | std::vector>* cluster_sweeps) { 154 | cluster_sweeps->clear(); 155 | cluster_sweeps->reserve(2 * poly.size()); 156 | 157 | // Find all sweepable directions. 158 | std::vector dirs = getAllSweepableEdgeDirections(poly); 159 | 160 | // Compute all possible sweeps. 161 | visibility_graph::VisibilityGraph vis_graph(poly); 162 | for (const Direction_2& dir : dirs) { 163 | bool counter_clockwise = true; 164 | std::vector sweep; 165 | if (!computeSweep(poly, vis_graph, max_sweep_offset, dir, counter_clockwise, 166 | &sweep)) { 167 | std::cout<<"Cannot compute counter-clockwise sweep."<push_back(sweep); 171 | std::reverse(sweep.begin(), sweep.end()); 172 | cluster_sweeps->push_back(sweep); 173 | } 174 | 175 | if (!computeSweep(poly, vis_graph, max_sweep_offset, dir, 176 | !counter_clockwise, &sweep)) { 177 | std::cout<<"Cannot compute clockwise sweep."<push_back(sweep); 181 | std::reverse(sweep.begin(), sweep.end()); 182 | cluster_sweeps->push_back(sweep); 183 | } 184 | } 185 | return true; 186 | } 187 | 188 | bool calculateShortestPath( 189 | const visibility_graph::VisibilityGraph& visibility_graph, 190 | const Point_2& start, const Point_2& goal, 191 | std::vector* shortest_path) { 192 | shortest_path->clear(); 193 | 194 | Polygon_2 start_visibility, goal_visibility; 195 | if (!computeVisibilityPolygon(visibility_graph.getPolygon(), start, 196 | &start_visibility)) { 197 | std::cout<<"Cannot compute visibility polygon from start query point " 198 | << start 199 | << " in polygon: " << visibility_graph.getPolygon()<size() < 2) { 218 | std::cout<<"Shortest path too short."< sortVerticesToLine(const Polygon_2& p, const Line_2& l) { 226 | // Copy points. 227 | std::vector pts(p.size()); 228 | std::vector::iterator pts_it = pts.begin(); 229 | for (VertexConstIterator it = p.vertices_begin(); it != p.vertices_end(); 230 | ++it) { 231 | *(pts_it++) = *it; 232 | } 233 | 234 | // Sort. 235 | std::sort(pts.begin(), pts.end(), 236 | [&l](const Point_2& a, const Point_2& b) -> bool { 237 | return CGAL::has_smaller_signed_distance_to_line(l, a, b); 238 | }); 239 | 240 | return pts; 241 | } 242 | 243 | std::vector findIntersections(const Polygon_2& p, const Line_2& l) { 244 | std::vector intersections; 245 | typedef CGAL::cpp11::result_of::type 246 | Intersection; 247 | 248 | for (EdgeConstIterator it = p.edges_begin(); it != p.edges_end(); ++it) { 249 | Intersection result = CGAL::intersection(*it, l); 250 | if (result) { 251 | if (const Segment_2* s = boost::get(&*result)) { 252 | intersections.push_back(s->source()); 253 | intersections.push_back(s->target()); 254 | } else { 255 | intersections.push_back(*boost::get(&*result)); 256 | } 257 | } 258 | } 259 | 260 | // Sort. 261 | Line_2 perp_l = l.perpendicular(l.point(0)); 262 | std::sort(intersections.begin(), intersections.end(), 263 | [&perp_l](const Point_2& a, const Point_2& b) -> bool { 264 | return CGAL::has_smaller_signed_distance_to_line(perp_l, a, b); 265 | }); 266 | 267 | return intersections; 268 | } 269 | 270 | } // namespace polygon_coverage_planning 271 | -------------------------------------------------------------------------------- /include/graph_base_impl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #ifndef POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_IMPL_H_ 21 | #define POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_IMPL_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace polygon_coverage_planning { 28 | 29 | template 30 | bool GraphBase::addNode( 31 | const NodeProperty& node_property) { 32 | graph_.push_back(std::map()); // Add node. 33 | 34 | // Add node properties. 35 | const size_t idx = graph_.size() - 1; 36 | node_properties_.insert(std::make_pair(idx, node_property)); 37 | // Create all adjacent edges. 38 | if (!addEdges()) { 39 | graph_.pop_back(); 40 | node_properties_.erase(node_properties_.find(idx)); 41 | return false; 42 | } 43 | return true; 44 | } 45 | 46 | template 47 | bool GraphBase::addStartNode( 48 | const NodeProperty& node_property) { 49 | // Add start node like any other node. 50 | start_idx_ = graph_.size(); 51 | if (addNode(node_property)) { 52 | return true; 53 | } else { 54 | std::cout<<"Failed adding start node."< 60 | bool GraphBase::addGoalNode( 61 | const NodeProperty& node_property) { 62 | // Add goal node like any other node. 63 | goal_idx_ = graph_.size(); 64 | if (addNode(node_property)) { 65 | return true; 66 | } else { 67 | std::cout<<"Failed adding goal node."< 73 | void GraphBase::clear() { 74 | graph_.clear(); 75 | node_properties_.clear(); 76 | edge_properties_.clear(); 77 | start_idx_ = std::numeric_limits::max(); 78 | goal_idx_ = std::numeric_limits::max(); 79 | is_created_ = false; 80 | } 81 | 82 | template 83 | void GraphBase::clearEdges() { 84 | edge_properties_.clear(); 85 | for (std::map& neighbors : graph_) { 86 | neighbors.clear(); 87 | } 88 | } 89 | 90 | template 91 | bool GraphBase::nodeExists(size_t node_id) const { 92 | return node_id < graph_.size(); 93 | } 94 | template 95 | bool GraphBase::nodePropertyExists( 96 | size_t node_id) const { 97 | return node_properties_.count(node_id) > 0; 98 | } 99 | 100 | template 101 | bool GraphBase::edgeExists( 102 | const EdgeId& edge_id) const { 103 | return nodeExists(edge_id.first) && 104 | graph_[edge_id.first].count(edge_id.second) > 0; 105 | } 106 | 107 | template 108 | bool GraphBase::edgePropertyExists( 109 | const EdgeId& edge_id) const { 110 | return edge_properties_.count(edge_id) > 0; 111 | } 112 | 113 | template 114 | bool GraphBase::getEdgeCost(const EdgeId& edge_id, 115 | double* cost) const { 116 | 117 | if (edgeExists(edge_id)) { 118 | *cost = graph_.at(edge_id.first).at(edge_id.second); 119 | return true; 120 | } else { 121 | std::cout<<"Edge from " << edge_id.first << " to " << edge_id.second 122 | << " does not exist."< 129 | const NodeProperty* GraphBase::getNodeProperty( 130 | size_t node_id) const { 131 | if (nodePropertyExists(node_id)) { 132 | return &(node_properties_.at(node_id)); 133 | } else { 134 | std::cout<<"Cannot access node property " << node_id << "."< 140 | const EdgeProperty* 141 | GraphBase::GraphBase::getEdgeProperty( 142 | const EdgeId& edge_id) const { 143 | if (edgePropertyExists(edge_id)) { 144 | return &(edge_properties_.at(edge_id)); 145 | } else { 146 | std::cout<<"Cannot access edge property from " 147 | << edge_id.first << " to " << edge_id.second << "."< 153 | bool GraphBase::solveDijkstra( 154 | size_t start, size_t goal, Solution* solution) const { 155 | solution->clear(); 156 | if (!nodeExists(start) || !nodeExists(goal)) { 157 | return false; 158 | } 159 | 160 | // https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm 161 | // Initialization. 162 | std::set open_set = {start}; // Nodes to evaluate. 163 | std::set closed_set; // Nodes already evaluated. 164 | std::map came_from; // Get previous node on optimal path. 165 | std::map cost; // Optimal cost from start. 166 | for (size_t i = 0; i < graph_.size(); i++) { 167 | cost[i] = std::numeric_limits::max(); 168 | } 169 | cost[start] = 0.0; 170 | 171 | while (!open_set.empty()) { 172 | // Pop vertex with lowest score from open set. 173 | size_t current = *std::min_element( 174 | open_set.begin(), open_set.end(), 175 | [&cost](size_t i, size_t j) { return cost[i] < cost[j]; }); 176 | if (current == goal) { // Reached goal. 177 | *solution = reconstructSolution(came_from, current); 178 | return true; 179 | } 180 | open_set.erase(current); 181 | closed_set.insert(current); 182 | 183 | // Check all neighbors. 184 | for (const std::pair& n : graph_[current]) { 185 | if (closed_set.count(n.first) > 0) { 186 | continue; // Ignore already evaluated neighbors. 187 | } 188 | open_set.insert(n.first); // Add to open set if not already in. 189 | 190 | // The distance from start to a neighbor. 191 | const double tentative_cost = cost[current] + n.second; 192 | if (tentative_cost >= cost[n.first]) { 193 | continue; // This is not a better path to n. 194 | } else { 195 | // This path is the best path to n until now. 196 | came_from[n.first] = current; 197 | cost[n.first] = tentative_cost; 198 | } 199 | } 200 | } 201 | 202 | return false; 203 | } 204 | 205 | template 206 | bool GraphBase::solveDijkstra( 207 | Solution* solution) const { 208 | return GraphBase::solveDijkstra(start_idx_, goal_idx_, solution); 209 | } 210 | 211 | template 212 | bool GraphBase::calculateHeuristic( 213 | size_t goal, Heuristic* heuristic) const { 214 | std::cout<<"Heuristic not implemented."< 219 | bool GraphBase::solveAStar( 220 | size_t start, size_t goal, Solution* solution) const { 221 | if (!nodeExists(start) || !nodeExists(goal)) { 222 | return false; 223 | } 224 | 225 | Heuristic heuristic; 226 | if (!calculateHeuristic(goal, &heuristic)) { 227 | return false; 228 | } 229 | 230 | // https://en.wikipedia.org/wiki/A*_search_algorithm 231 | // Initialization. 232 | std::set open_set = {start}; // Nodes to evaluate. 233 | std::set closed_set; // Nodes already evaluated. 234 | std::map came_from; // Get previous node on optimal path. 235 | std::map cost; // Optimal cost from start. 236 | std::map cost_with_heuristic; // cost + heuristic 237 | for (size_t i = 0; i < graph_.size(); i++) { 238 | cost[i] = std::numeric_limits::max(); 239 | cost_with_heuristic[i] = std::numeric_limits::max(); 240 | } 241 | cost[start] = 0.0; 242 | const Heuristic::const_iterator start_heuristic_it = heuristic.find(start); 243 | if (start_heuristic_it == heuristic.end()) { 244 | return false; // Heuristic not found. 245 | } 246 | cost_with_heuristic[start] = start_heuristic_it->second; 247 | 248 | while (!open_set.empty()) { 249 | // Pop vertex with lowest cost with heuristic from open set. 250 | size_t current = *std::min_element( 251 | open_set.begin(), open_set.end(), 252 | [&cost_with_heuristic](size_t i, size_t j) { 253 | return cost_with_heuristic[i] < cost_with_heuristic[j]; 254 | }); 255 | if (current == goal) { // Reached goal. 256 | *solution = reconstructSolution(came_from, current); 257 | return true; 258 | } 259 | open_set.erase(current); 260 | closed_set.insert(current); 261 | 262 | // Check all neighbors. 263 | for (const std::pair& n : graph_[current]) { 264 | if (closed_set.count(n.first) > 0) { 265 | continue; // Ignore already evaluated neighbors. 266 | } 267 | open_set.insert(n.first); // Add to open set if not already in. 268 | 269 | // The distance from start to a neighbor. 270 | const double tentative_cost = cost[current] + n.second; 271 | if (tentative_cost >= cost[n.first]) { 272 | continue; // This is not a better path to n. 273 | } else { 274 | // This path is the best path to n until now. 275 | came_from[n.first] = current; 276 | cost[n.first] = tentative_cost; 277 | const Heuristic::const_iterator heuristic_it = heuristic.find(n.first); 278 | if (heuristic_it == heuristic.end()) { 279 | return false; // Heuristic not found. 280 | } 281 | cost_with_heuristic[n.first] = cost[n.first] + heuristic_it->second; 282 | } 283 | } 284 | } 285 | 286 | return false; 287 | } 288 | 289 | template 290 | bool GraphBase::solveAStar( 291 | Solution* solution) const { 292 | return GraphBase::solveAStar(start_idx_, goal_idx_, solution); 293 | } 294 | 295 | template 296 | bool GraphBase::addEdge( 297 | const EdgeId& edge_id, const EdgeProperty& edge_property, double cost) { 298 | if (cost >= 0.0 && nodeExists(edge_id.first)) { 299 | graph_[edge_id.first][edge_id.second] = cost; 300 | edge_properties_.insert(std::make_pair(edge_id, edge_property)); 301 | return true; 302 | } else { 303 | return false; 304 | } 305 | } 306 | 307 | template 308 | Solution GraphBase::reconstructSolution( 309 | const std::map& came_from, size_t current) const { 310 | Solution solution = {current}; 311 | while (came_from.find(current) != came_from.end()) { 312 | current = came_from.at(current); 313 | solution.push_back(current); 314 | } 315 | std::reverse(solution.begin(), solution.end()); 316 | return solution; 317 | } 318 | 319 | template 320 | std::vector> 321 | GraphBase::getAdjacencyMatrix() const { 322 | std::vector> m(graph_.size(), 323 | std::vector(graph_.size())); 324 | 325 | for (size_t i = 0; i < m.size(); ++i) { 326 | for (size_t j = 0; j < m[i].size(); ++j) { 327 | const EdgeId edge(i, j); 328 | double cost; 329 | if (edgeExists(edge) && getEdgeCost(edge, &cost)) { 330 | m[i][j] = doubleToMilliInt(cost); 331 | } else { 332 | m[i][j] = std::numeric_limits::max(); 333 | } 334 | } 335 | } 336 | 337 | return m; 338 | } 339 | 340 | } // namespace polygon_coverage_planning 341 | 342 | #endif // POLYGON_COVERAGE_SOLVERS_GRAPH_BASE_IMPL_H_ 343 | -------------------------------------------------------------------------------- /src/bcd.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * polygon_coverage_planning implements algorithms for coverage planning in 3 | * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous 4 | * Systems Lab, ETH Zürich 5 | * 6 | * This program is free software: you can redistribute it and/or modify it under 7 | * the terms of the GNU General Public License as published by the Free Software 8 | * Foundation, either version 3 of the License, or (at your option) any later 9 | * version. 10 | * 11 | * This program is distributed in the hope that it will be useful, but WITHOUT 12 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 13 | * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 14 | * details. 15 | * 16 | * You should have received a copy of the GNU General Public License along with 17 | * this program. If not, see . 18 | */ 19 | 20 | #include 21 | 22 | #include "bcd.h" 23 | #include "cgal_comm.h" 24 | 25 | namespace polygon_coverage_planning { 26 | 27 | std::vector computeBCD(const PolygonWithHoles& polygon_in, 28 | const Direction_2& dir) { 29 | // Rotate polygon to have direction aligned with x-axis. 30 | // TODO(rikba): Make this independent of rotation. 31 | PolygonWithHoles rotated_polygon = rotatePolygon(polygon_in, dir); 32 | sortPolygon(&rotated_polygon); 33 | simplifyPolygon(&rotated_polygon); 34 | 35 | // Sort vertices by x value. 36 | std::vector sorted_vertices = 37 | getXSortedVertices(rotated_polygon); 38 | 39 | // Initialize edge list. 40 | std::list L; 41 | std::list open_polygons; 42 | std::vector closed_polygons; 43 | std::vector processed_vertices; 44 | for (size_t i = 0; i < sorted_vertices.size(); ++i) { 45 | const VertexConstCirculator& v = sorted_vertices[i]; 46 | // v already processed. 47 | if (std::find(processed_vertices.begin(), processed_vertices.end(), *v) != 48 | processed_vertices.end()) 49 | continue; 50 | processEvent(rotated_polygon, v, &sorted_vertices, &processed_vertices, &L, 51 | &open_polygons, &closed_polygons); 52 | } 53 | 54 | // Rotate back all polygons. 55 | for (Polygon_2& p : closed_polygons) { 56 | CGAL::Aff_transformation_2 rotation(CGAL::ROTATION, dir, 1, 1e9); 57 | p = CGAL::transform(rotation, p); 58 | } 59 | 60 | return closed_polygons; 61 | } 62 | 63 | std::vector getXSortedVertices( 64 | const PolygonWithHoles& p) { 65 | std::vector sorted_vertices; 66 | 67 | // Get boundary vertices. 68 | VertexConstCirculator v = p.outer_boundary().vertices_circulator(); 69 | do { 70 | sorted_vertices.push_back(v); 71 | } while (++v != p.outer_boundary().vertices_circulator()); 72 | // Get hole vertices. 73 | for (PolygonWithHoles::Hole_const_iterator hit = p.holes_begin(); 74 | hit != p.holes_end(); ++hit) { 75 | VertexConstCirculator vh = hit->vertices_circulator(); 76 | do { 77 | sorted_vertices.push_back(vh); 78 | } while (++vh != hit->vertices_circulator()); 79 | } 80 | // Sort x,y. 81 | Polygon_2::Traits::Less_xy_2 less_xy_2; 82 | std::sort(sorted_vertices.begin(), sorted_vertices.end(), 83 | [&less_xy_2](const VertexConstCirculator& a, 84 | const VertexConstCirculator& b) -> bool { 85 | return less_xy_2(*a, *b); 86 | }); 87 | 88 | return sorted_vertices; 89 | } 90 | 91 | void processEvent(const PolygonWithHoles& pwh, const VertexConstCirculator& v, 92 | std::vector* sorted_vertices, 93 | std::vector* processed_vertices, 94 | std::list* L, std::list* open_polygons, 95 | std::vector* closed_polygons) { 96 | 97 | Polygon_2::Traits::Equal_2 eq_2; 98 | 99 | // Compute intersection. 100 | Line_2 l(*v, Direction_2(0, 1)); 101 | std::vector intersections = getIntersections(*L, l); 102 | 103 | // Get e_lower and e_upper. 104 | Segment_2 e_prev(*v, *std::prev(v)); 105 | Segment_2 e_next(*v, *std::next(v)); 106 | // Catch vertical edges. 107 | Polygon_2::Traits::Equal_x_2 eq_x_2; 108 | if (eq_x_2(e_prev.source(), e_prev.target())) { 109 | e_prev = Segment_2(*std::prev(v), *std::prev(v, 2)); 110 | } else if (eq_x_2(e_next.source(), e_next.target())) { 111 | e_next = Segment_2(*std::next(v), *std::next(v, 2)); 112 | } 113 | 114 | Polygon_2::Traits::Less_y_2 less_y_2; 115 | Polygon_2::Traits::Less_x_2 less_x_2; 116 | Segment_2 e_lower = e_prev; 117 | Segment_2 e_upper = e_next; 118 | if (less_x_2(e_prev.target(), e_prev.source()) && 119 | less_x_2(e_next.target(), e_next.source())) { // OUT 120 | 121 | Point_2 p_on_upper = eq_2(e_lower.source(), e_upper.source()) 122 | ? e_upper.target() 123 | : e_upper.source(); 124 | if (e_lower.supporting_line().has_on_positive_side(p_on_upper)) 125 | std::swap(e_lower, e_upper); 126 | 127 | // Determine whether we close one or close two and open one. 128 | // TODO(rikba): instead of looking at unbounded side, look at adjacent edge 129 | // angle 130 | bool close_one = outOfPWH(pwh, *v + Vector_2(1e-6, 0)); 131 | 132 | // Find edges to remove. 133 | std::list::iterator e_lower_it = L->begin(); 134 | size_t e_lower_id = 0; 135 | for (; e_lower_it != L->end(); ++e_lower_it) { 136 | if (*e_lower_it == e_lower || *e_lower_it == e_lower.opposite()) { 137 | break; 138 | } 139 | e_lower_id++; 140 | } 141 | 142 | std::list::iterator e_upper_it = std::next(e_lower_it); 143 | size_t e_upper_id = e_lower_id + 1; 144 | size_t lower_cell_id = e_lower_id / 2; 145 | size_t upper_cell_id = e_upper_id / 2; 146 | 147 | if (close_one) { 148 | std::list::iterator cell = 149 | std::next(open_polygons->begin(), lower_cell_id); 150 | cell->push_back(e_lower.source()); 151 | Polygon_2::Traits::Equal_2 eq_2; 152 | if (!eq_2(e_lower.source(), e_upper.source())) { 153 | cell->push_back(e_upper.source()); 154 | } 155 | if (cleanupPolygon(&*cell)) closed_polygons->push_back(*cell); 156 | L->erase(e_lower_it); 157 | L->erase(e_upper_it); 158 | open_polygons->erase(cell); 159 | } else { 160 | // Close two cells, open one. 161 | // Close lower cell. 162 | 163 | std::list::iterator lower_cell = 164 | std::next(open_polygons->begin(), lower_cell_id); 165 | lower_cell->push_back(intersections[e_lower_id - 1]); 166 | lower_cell->push_back(intersections[e_lower_id]); 167 | if (cleanupPolygon(&*lower_cell)) closed_polygons->push_back(*lower_cell); 168 | // Close upper cell. 169 | std::list::iterator upper_cell = 170 | std::next(open_polygons->begin(), upper_cell_id); 171 | upper_cell->push_back(intersections[e_upper_id]); 172 | upper_cell->push_back(intersections[e_upper_id + 1]); 173 | if (cleanupPolygon(&*upper_cell)) closed_polygons->push_back(*upper_cell); 174 | 175 | // Delete e_lower and e_upper from list. 176 | L->erase(e_lower_it); 177 | L->erase(e_upper_it); 178 | 179 | // Open one new cell. 180 | std::list::iterator new_polygon = 181 | open_polygons->insert(lower_cell, Polygon_2()); 182 | new_polygon->push_back(intersections[e_upper_id + 1]); 183 | new_polygon->push_back(intersections[e_lower_id - 1]); 184 | 185 | open_polygons->erase(lower_cell); 186 | open_polygons->erase(upper_cell); 187 | } 188 | processed_vertices->push_back(e_lower.source()); 189 | if (!eq_2(e_lower.source(), e_upper.source())) { 190 | processed_vertices->push_back(e_upper.source()); 191 | } 192 | } else if (!less_x_2(e_lower.target(), e_lower.source()) && 193 | !less_x_2(e_upper.target(), e_upper.source())) { 194 | // IN 195 | Polygon_2::Traits::Equal_2 eq_2; 196 | Point_2 p_on_lower = eq_2(e_lower.source(), e_upper.source()) 197 | ? e_lower.target() 198 | : e_lower.source(); 199 | if (e_upper.supporting_line().has_on_positive_side(p_on_lower)) 200 | std::swap(e_lower, e_upper); 201 | 202 | // Determine whether we open one or close one and open two. 203 | bool open_one = outOfPWH(pwh, *v - Vector_2(1e-6, 0)); 204 | 205 | // Find edge to update. 206 | size_t e_LOWER_id = 0; 207 | bool found_e_lower_id = false; 208 | for (size_t i = 0; i < intersections.size() - 1; i = i + 2) { 209 | if (intersections.empty()) break; 210 | if (open_one) { 211 | if (less_y_2(intersections[i], e_lower.source()) && 212 | less_y_2(intersections[i + 1], e_upper.source())) { 213 | e_LOWER_id = i; 214 | found_e_lower_id = true; 215 | } 216 | } else { 217 | if (less_y_2(intersections[i], e_lower.source()) && 218 | less_y_2(e_upper.source(), intersections[i + 1])) { 219 | e_LOWER_id = i; 220 | } 221 | } 222 | } 223 | if (open_one) { 224 | // Add one new cell above e_UPPER. 225 | std::list::iterator e_UPPER = L->begin(); 226 | std::list::iterator open_cell = open_polygons->begin(); 227 | if (!L->empty() && found_e_lower_id) { 228 | e_UPPER = std::next(e_UPPER, e_LOWER_id + 1); 229 | open_cell = std::next(open_cell, e_LOWER_id / 2 + 1); 230 | } 231 | // Update edge list. 232 | if (L->empty()) { 233 | L->insert(L->end(), e_lower); 234 | L->insert(L->end(), e_upper); 235 | } else if (!L->empty() && !found_e_lower_id) { 236 | L->insert(L->begin(), e_upper); 237 | L->insert(L->begin(), e_lower); 238 | } else { 239 | std::list::iterator inserter = std::next(e_UPPER); 240 | L->insert(inserter, e_lower); 241 | L->insert(inserter, e_upper); 242 | } 243 | 244 | // Create new polygon. 245 | std::list::iterator open_polygon = 246 | open_polygons->insert(open_cell, Polygon_2()); 247 | open_polygon->push_back(e_upper.source()); 248 | if (!eq_2(e_lower.source(), e_upper.source())) { 249 | open_polygon->push_back(e_lower.source()); 250 | } 251 | } else { 252 | // Add new polygon between e_LOWER and e_UPPER. 253 | std::list::iterator e_LOWER = 254 | std::next(L->begin(), e_LOWER_id); 255 | std::list::iterator cell = 256 | std::next(open_polygons->begin(), e_LOWER_id / 2); 257 | 258 | // Add e_lower and e_upper 259 | std::list::iterator e_lower_it = 260 | L->insert(std::next(e_LOWER), e_lower); 261 | L->insert(std::next(e_lower_it), e_upper); 262 | 263 | // Add new cell. 264 | std::list::iterator new_polygon = 265 | open_polygons->insert(cell, Polygon_2()); 266 | 267 | // Close one cell. 268 | cell->push_back(intersections[e_LOWER_id]); 269 | cell->push_back(intersections[e_LOWER_id + 1]); 270 | if (cleanupPolygon(&*cell)) closed_polygons->push_back(*cell); 271 | // Open two new cells 272 | // Lower polygon. 273 | new_polygon->push_back(e_lower.source()); 274 | new_polygon->push_back(intersections[e_LOWER_id]); 275 | 276 | // Upper polygon. 277 | new_polygon = open_polygons->insert(cell, Polygon_2()); 278 | new_polygon->push_back(intersections[e_LOWER_id + 1]); 279 | new_polygon->push_back(e_upper.source()); 280 | // Close old cell. 281 | open_polygons->erase(cell); 282 | } 283 | processed_vertices->push_back(e_lower.source()); 284 | if (!eq_2(e_lower.source(), e_upper.source())) { 285 | processed_vertices->push_back(e_upper.source()); 286 | } 287 | } else { 288 | // TODO(rikba): Sort vertices correctly in the first place. 289 | // Check if v exits among edges. 290 | VertexConstCirculator v_middle = v; 291 | std::list::iterator it = L->end(); 292 | while (it == L->end()) { 293 | for (it = L->begin(); it != L->end(); it++) { 294 | if (*v_middle == it->source() || *v_middle == it->target()) { 295 | // Swap v in sorted vertices. 296 | if (!eq_2(*v, *v_middle)) { 297 | std::vector::iterator i_v = 298 | sorted_vertices->end(); 299 | std::vector::iterator i_v_middle = 300 | sorted_vertices->end(); 301 | for (std::vector::iterator it = 302 | sorted_vertices->begin(); 303 | it != sorted_vertices->end(); ++it) { 304 | if (*it == v) i_v = it; 305 | if (*it == v_middle) i_v_middle = it; 306 | } 307 | 308 | std::iter_swap(i_v, i_v_middle); 309 | } 310 | break; 311 | } 312 | } 313 | if (it == L->end()) { 314 | VertexConstCirculator v_prev = std::prev(v_middle); 315 | VertexConstCirculator v_next = std::next(v_middle); 316 | 317 | if (v_prev->x() == v_middle->x()) 318 | v_middle = v_prev; 319 | else 320 | v_middle = v_next; 321 | } 322 | } 323 | 324 | // Correct vertical edges. 325 | e_prev = Segment_2(*v_middle, *std::prev(v_middle)); 326 | e_next = Segment_2(*v_middle, *std::next(v_middle)); 327 | 328 | // Find edge to update. 329 | std::list::iterator old_e_it = L->begin(); 330 | Segment_2 new_edge; 331 | size_t edge_id = 0; 332 | for (; old_e_it != L->end(); ++old_e_it) { 333 | if (*old_e_it == e_next || *old_e_it == e_next.opposite()) { 334 | new_edge = e_prev; 335 | break; 336 | } else if (*old_e_it == e_prev || *old_e_it == e_prev.opposite()) { 337 | new_edge = e_next; 338 | break; 339 | } 340 | edge_id++; 341 | } 342 | 343 | // Update cell with new vertex. 344 | size_t cell_id = edge_id / 2; 345 | std::list::iterator cell = 346 | std::next(open_polygons->begin(), cell_id); 347 | 348 | if ((edge_id % 2) == 0) { 349 | // Case 1: Insert new vertex at end. 350 | cell->push_back(new_edge.source()); 351 | } else { 352 | // Case 2: Insert new vertex at begin. 353 | cell->insert(cell->vertices_begin(), new_edge.source()); 354 | } 355 | // Update edge. 356 | L->insert(old_e_it, new_edge); 357 | L->erase(old_e_it); 358 | 359 | processed_vertices->push_back(*v_middle); 360 | } 361 | } 362 | std::vector getIntersections(const std::list& L, 363 | const Line_2& l) { 364 | typedef CGAL::cpp11::result_of::type 365 | Intersection; 366 | 367 | std::vector intersections(L.size()); 368 | std::vector::iterator intersection = intersections.begin(); 369 | for (std::list::const_iterator it = L.begin(); it != L.end(); 370 | ++it) { 371 | Intersection result = CGAL::intersection(*it, l); 372 | if (result) { 373 | if (boost::get(&*result)) { 374 | *(intersection++) = it->target(); 375 | } else { 376 | const Point_2* p = boost::get(&*result); 377 | *(intersection++) = *p; 378 | } 379 | } else { 380 | std::cout<<"No intersection found!"<outer_boundary().is_clockwise_oriented()) 389 | pwh->outer_boundary().reverse_orientation(); 390 | 391 | for (PolygonWithHoles::Hole_iterator hi = pwh->holes_begin(); 392 | hi != pwh->holes_end(); ++hi) 393 | if (hi->is_counterclockwise_oriented()) hi->reverse_orientation(); 394 | } 395 | 396 | bool cleanupPolygon(Polygon_2* poly) { 397 | Polygon_2::Traits::Equal_2 eq_2; 398 | bool erase_one = true; 399 | while (erase_one) { 400 | Polygon_2::Vertex_circulator vit = poly->vertices_circulator(); 401 | erase_one = false; 402 | do { 403 | if (eq_2(*vit, *std::next(vit))) { 404 | poly->erase(vit++); 405 | erase_one = true; 406 | } else 407 | vit++; 408 | } while (vit != poly->vertices_circulator()); 409 | } 410 | 411 | return poly->is_simple() && poly->area() != 0.0; 412 | } 413 | 414 | bool outOfPWH(const PolygonWithHoles& pwh, const Point_2& p) { 415 | if (pwh.outer_boundary().has_on_unbounded_side(p)) return true; 416 | 417 | for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin(); 418 | hit != pwh.holes_end(); ++hit) { 419 | if (hit->has_on_bounded_side(p)) { 420 | return true; 421 | } 422 | } 423 | 424 | return false; 425 | } 426 | 427 | } // namespace polygon_coverage_planning 428 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "coverage_planner.h" 2 | 3 | #define DENSE_PATH 4 | 5 | int main(){ 6 | 7 | cv::Mat img = cv::imread("../data/basement.png"); 8 | 9 | cv::Mat gray; 10 | cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); 11 | 12 | cv::Mat img_ = gray.clone(); 13 | 14 | cv::threshold(img_, img_, 250, 255, 0); 15 | 16 | cv::Mat erode_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(10,10), cv::Point(-1,-1)); // size: robot radius 17 | cv::morphologyEx(img_, img_, cv::MORPH_ERODE, erode_kernel); 18 | 19 | cv::Mat open_kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5), cv::Point(-1,-1)); 20 | cv::morphologyEx(img_, img_, cv::MORPH_OPEN, open_kernel); 21 | 22 | // cv::imshow("preprocess", img_); 23 | 24 | std::vector> cnts; 25 | std::vector hierarchy; // index: next, prev, first_child, parent 26 | cv::findContours(img_, cnts, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE); 27 | 28 | std::vector cnt_indices(cnts.size()); 29 | std::iota(cnt_indices.begin(), cnt_indices.end(), 0); 30 | std::sort(cnt_indices.begin(), cnt_indices.end(), [&cnts](int lhs, int rhs){return cv::contourArea(cnts[lhs]) > cv::contourArea(cnts[rhs]);}); 31 | int ext_cnt_idx = cnt_indices.front(); 32 | 33 | cv::Mat cnt_canvas = img.clone(); 34 | cv::drawContours(cnt_canvas, cnts, ext_cnt_idx, cv::Scalar(0,0,255)); 35 | std::vector> contours; 36 | contours.emplace_back(cnts[ext_cnt_idx]); 37 | 38 | // find all the contours of obstacle 39 | for(int i = 0; i < hierarchy.size(); i++){ 40 | if(hierarchy[i][3]==ext_cnt_idx){ // parent contour's index equals to external contour's index 41 | contours.emplace_back(cnts[i]); 42 | cv::drawContours(cnt_canvas, cnts, i, cv::Scalar(255,0,0)); 43 | } 44 | } 45 | // cv::imshow("contours", cnt_canvas); 46 | 47 | cv::Mat cnt_img = cv::Mat(img.rows, img.cols, CV_8UC3); 48 | cnt_img.setTo(255); 49 | for(int i = 0; i < contours.size(); i++){ 50 | cv::drawContours(cnt_img, contours, i, cv::Scalar(0,0,0)); 51 | } 52 | // cv::imshow("only contours", cnt_img); 53 | 54 | cv::Mat poly_canvas = img.clone(); 55 | std::vector poly; 56 | std::vector> polys; 57 | for(auto& contour : contours){ 58 | cv::approxPolyDP(contour, poly, 3, true); 59 | polys.emplace_back(poly); 60 | poly.clear(); 61 | } 62 | for(int i = 0; i < polys.size(); i++){ 63 | cv::drawContours(poly_canvas, polys, i, cv::Scalar(255,0,255)); 64 | } 65 | // cv::imshow("polygons", poly_canvas); 66 | 67 | cv::Mat poly_img = cv::Mat(img.rows, img.cols, CV_8UC3); 68 | poly_img.setTo(255); 69 | for(int i = 0; i < polys.size(); i++){ 70 | cv::drawContours(poly_img, polys, i, cv::Scalar(0,0,0)); 71 | } 72 | // cv::imshow("only polygons", poly_img); 73 | 74 | // compute main direction 75 | 76 | // [0,180) 77 | std::vector line_deg_histogram(180); 78 | double line_len; // weight 79 | double line_deg; 80 | int line_deg_idx; 81 | 82 | cv::Mat line_canvas = img.clone(); 83 | auto ext_poly = polys.front(); 84 | ext_poly.emplace_back(ext_poly.front()); 85 | for(int i = 1; i < ext_poly.size(); i++){ 86 | line_len = std::sqrt(std::pow((ext_poly[i].x-ext_poly[i-1].x),2)+std::pow((ext_poly[i].y-ext_poly[i-1].y),2)); 87 | // y-axis towards up, x-axis towards right, theta is from x-axis to y-axis 88 | line_deg = std::round(atan2(-(ext_poly[i].y-ext_poly[i-1].y),(ext_poly[i].x)-ext_poly[i-1].x)/M_PI*180.0); // atan2: (-180, 180] 89 | line_deg_idx = (int(line_deg) + 180) % 180; // [0, 180) 90 | line_deg_histogram[line_deg_idx] += int(line_len); 91 | 92 | // std::cout<<"deg: "< outer_poly = polys.front(); 108 | polys.erase(polys.begin()); 109 | std::vector> inner_polys = polys; 110 | 111 | 112 | Polygon_2 outer_polygon; 113 | for(const auto& point : outer_poly){ 114 | outer_polygon.push_back(Point_2(point.x, point.y)); 115 | } 116 | 117 | int num_holes = inner_polys.size(); 118 | std::vector holes(num_holes); 119 | for(int i = 0; i < inner_polys.size(); i++){ 120 | for(const auto& point : inner_polys[i]){ 121 | holes[i].push_back(Point_2(point.x, point.y)); 122 | } 123 | } 124 | 125 | PolygonWithHoles pwh(outer_polygon, holes.begin(), holes.end()); 126 | 127 | // cell decomposition 128 | 129 | std::vector bcd_cells; 130 | 131 | // polygon_coverage_planning::computeBestTCDFromPolygonWithHoles(pwh, &bcd_cells); 132 | polygon_coverage_planning::computeBestBCDFromPolygonWithHoles(pwh, &bcd_cells); 133 | 134 | // test decomposition 135 | // std::vector> bcd_polys; 136 | // std::vector bcd_poly; 137 | // 138 | // for(const auto& cell:bcd_cells){ 139 | // for(int i = 0; i < cell.size(); i++){ 140 | // bcd_poly.emplace_back(cv::Point(CGAL::to_double(cell[i].x()), CGAL::to_double(cell[i].y()))); 141 | // } 142 | // bcd_polys.emplace_back(bcd_poly); 143 | // bcd_poly.clear(); 144 | // } 145 | 146 | // for(int i = 0; i < bcd_polys.size(); i++){ 147 | // cv::drawContours(poly_img, bcd_polys, i, cv::Scalar(255,0,255)); 148 | // cv::imshow("bcd", poly_img); 149 | // cv::waitKey(); 150 | // } 151 | // cv::imshow("bcd", poly_img); 152 | // cv::waitKey(); 153 | 154 | 155 | // construct adjacent graph 156 | // std::map> cell_graph; 157 | // bool succeed = calculateDecompositionAdjacency(bcd_cells, &cell_graph); 158 | // 159 | // if(succeed){ 160 | // std::cout<<"cell graph was constructed"<first<<" 's neighbors: "<second.begin(); it!=cell_it->second.end(); it++){ 168 | // std::cout<<*it<<" "; 169 | // } 170 | // std::cout<"<> cells_sweeps; 194 | 195 | for (size_t i = 0; i < bcd_cells.size(); ++i) { 196 | // Compute all cluster sweeps. 197 | std::vector cell_sweep; 198 | Direction_2 best_dir; 199 | polygon_coverage_planning::findBestSweepDir(bcd_cells[i], &best_dir); 200 | polygon_coverage_planning::visibility_graph::VisibilityGraph vis_graph(bcd_cells[i]); 201 | 202 | bool counter_clockwise = true; 203 | polygon_coverage_planning::computeSweep(bcd_cells[i], vis_graph, sweep_step, best_dir, counter_clockwise, &cell_sweep); 204 | cells_sweeps.emplace_back(cell_sweep); 205 | } 206 | 207 | // cv::Point prev, curr; 208 | // cv::Mat poly_img_ = poly_img.clone(); 209 | // for(size_t i = 0; i < cells_sweeps.size(); ++i){ 210 | // for(size_t j = 1; j < cells_sweeps[i].size(); ++j){ 211 | // prev = cv::Point(CGAL::to_double(cells_sweeps[i][j-1].x()),CGAL::to_double(cells_sweeps[i][j-1].y())); 212 | // curr = cv::Point(CGAL::to_double(cells_sweeps[i][j].x()),CGAL::to_double(cells_sweeps[i][j].y())); 213 | // cv::line(poly_img_, prev, curr, cv::Scalar(0, 0, 255)); 214 | // cv::namedWindow("way",cv::WINDOW_NORMAL); 215 | // cv::imshow("way", poly_img_); 216 | // cv::waitKey(0); 217 | // } 218 | // poly_img_ = poly_img.clone(); 219 | // } 220 | // cv::waitKey(0); 221 | 222 | 223 | // for(int i = 0; i < cell_path.size(); i++){ 224 | // cv::drawContours(poly_img, bcd_polys, cell_path[i].cellIndex, cv::Scalar(0, 255, 255)); 225 | // cv::namedWindow("path cell", cv::WINDOW_NORMAL); 226 | // cv::imshow("path cell", poly_img); 227 | // cv::waitKey(500); 228 | // cv::drawContours(poly_img, bcd_polys, cell_path[i].cellIndex, cv::Scalar(0, 0, 255)); 229 | // } 230 | // cv::waitKey(); 231 | 232 | auto cell_intersections = calculateCellIntersections(bcd_cells, cell_graph); 233 | 234 | // for(size_t i = 0; i < cell_intersections.size(); ++i){ 235 | // for(auto j = cell_intersections[i].begin(); j != cell_intersections[i].end(); ++j){ 236 | // std::cout<<"cell "<first<<": "; 237 | // for(auto k = j->second.begin(); k != j->second.end(); ++k){ 238 | // std::cout<<"("<x())<<", "<y())<<")"<<" "; 239 | // } 240 | // std::cout< way_points; 246 | 247 | #ifdef DENSE_PATH 248 | Point_2 point = start; 249 | std::list next_candidates; 250 | Point_2 next_point; 251 | std::vector shortest_path; 252 | 253 | if(doReverseNextSweep(start, cells_sweeps[cell_idx_path.front()])){ 254 | shortest_path = getShortestPath(bcd_cells[cell_idx_path.front()], start, cells_sweeps[cell_idx_path.front()].back()); 255 | way_points.insert(way_points.end(), shortest_path.begin(), std::prev(shortest_path.end())); 256 | } else{ 257 | shortest_path = getShortestPath(bcd_cells[cell_idx_path.front()], start, cells_sweeps[cell_idx_path.front()].front()); 258 | way_points.insert(way_points.end(), shortest_path.begin(), std::prev(shortest_path.end())); 259 | } 260 | 261 | point = way_points.back(); 262 | 263 | for(size_t i = 0; i < cell_idx_path.size(); ++i){ 264 | // has been cleaned? 265 | if(!cell_graph[cell_idx_path[i]].isCleaned){ 266 | // need to reverse? 267 | if(doReverseNextSweep(point, cells_sweeps[cell_idx_path[i]])){ 268 | way_points.insert(way_points.end(), cells_sweeps[cell_idx_path[i]].rbegin(), cells_sweeps[cell_idx_path[i]].rend()); 269 | }else{ 270 | way_points.insert(way_points.end(), cells_sweeps[cell_idx_path[i]].begin(), cells_sweeps[cell_idx_path[i]].end()); 271 | } 272 | // now cleaned 273 | cell_graph[cell_idx_path[i]].isCleaned = true; 274 | // update current point 275 | point = way_points.back(); 276 | // find shortest path to next cell 277 | if((i+1) 0; --i){ 365 | p1 = cv::Point(CGAL::to_double(cells_sweeps[idx][i].x()),CGAL::to_double(cells_sweeps[idx][i].y())); 366 | p2 = cv::Point(CGAL::to_double(cells_sweeps[idx][i-1].x()),CGAL::to_double(cells_sweeps[idx][i-1].y())); 367 | cv::line(img, p1, p2, cv::Scalar(0, 64, 255)); 368 | cv::namedWindow("cover",cv::WINDOW_NORMAL); 369 | cv::imshow("cover", img); 370 | // cv::waitKey(50); 371 | cv::line(img, p1, p2, cv::Scalar(200, 200, 200)); 372 | } 373 | 374 | }else{ 375 | way_points.insert(way_points.end(), cells_sweeps[idx].begin(), cells_sweeps[idx].end()); 376 | 377 | temp_img = img.clone(); 378 | cv::line(img, 379 | cv::Point(CGAL::to_double(point.x()),CGAL::to_double(point.y())), 380 | cv::Point(CGAL::to_double(cells_sweeps[idx].front().x()),CGAL::to_double(cells_sweeps[idx].front().y())), 381 | cv::Scalar(255, 0, 0), 382 | 1); 383 | cv::namedWindow("cover",cv::WINDOW_NORMAL); 384 | cv::imshow("cover", img); 385 | // cv::waitKey(500); 386 | img = temp_img.clone(); 387 | 388 | for(size_t i = 1; i < cells_sweeps[idx].size(); ++i){ 389 | p1 = cv::Point(CGAL::to_double(cells_sweeps[idx][i-1].x()),CGAL::to_double(cells_sweeps[idx][i-1].y())); 390 | p2 = cv::Point(CGAL::to_double(cells_sweeps[idx][i].x()),CGAL::to_double(cells_sweeps[idx][i].y())); 391 | cv::line(img, p1, p2, cv::Scalar(0, 64, 255)); 392 | cv::namedWindow("cover",cv::WINDOW_NORMAL); 393 | cv::imshow("cover", img); 394 | // cv::waitKey(50); 395 | cv::line(img, p1, p2, cv::Scalar(200, 200, 200)); 396 | } 397 | } 398 | 399 | cell_graph[idx].isCleaned = true; 400 | point = way_points.back(); 401 | } 402 | } 403 | 404 | cv::waitKey(1000); 405 | 406 | #endif 407 | 408 | return 0; 409 | } 410 | --------------------------------------------------------------------------------