├── .gitignore ├── CMakeLists.txt ├── README.md ├── doc ├── origin_01.png ├── result_02.png ├── result_03.png ├── result_04.png └── 凹多边形凸分解算法在快速原型中的应用_朱传敏.pdf ├── include ├── bow_shaped_planner.h └── decomposition.h └── src ├── .decomposition.cpp.swp ├── bow_shaped_planner.cpp ├── decomposition.cpp └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .vscode 3 | 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(BowShapedPlanner) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | # set(Glog_DIR /usr/lib/cmake/ceres) 7 | 8 | find_package(OpenCV REQUIRED) 9 | find_package(Eigen3 REQUIRED) 10 | # find_package(Glog REQUIRED) 11 | 12 | include_directories( 13 | include/ 14 | /opt/ros/kinetic/include 15 | ${OpenCV_INCLUDE_DIRS} 16 | ${EIGEN3_INCLUDE_DIR} 17 | # ${GLOG_INCLUDE_DIR} 18 | ) 19 | 20 | add_executable(planner 21 | src/main.cpp 22 | src/bow_shaped_planner.cpp 23 | src/decomposition.cpp 24 | ) 25 | target_link_libraries(planner 26 | # glog 27 | ${OpenCV_LIBS} 28 | ) 29 | 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Coverage Planning 2 | 3 | Recommend use CGAL library to finish this coverage planning due the bugs of this repository. 4 | 5 | ## 0. Overview 6 | 7 | **BUG when confronted with complicated polygons** 8 | 9 | Here is the good one 10 | 11 | ![good_one](./doc/result_03.png) 12 | 13 | Then we have got a bad one 14 | 15 | ![bad_one](./doc/result_04.png) 16 | 17 | **the bug lies in PolygonDecomposition::DecomposeIt() where the processing of erasing polygons and adding polygons** 18 | 19 | Decompose the given polygon if concave 20 | 21 | Compute a bow-shape complete coverage path for every polygon 22 | 23 | ![origin_polygon](./doc/origin_01.png) 24 | 25 | ![result_coverage](./doc/result_02.png) 26 | 27 | ## 1. Dependency 28 | 29 | - Eigen 30 | 31 | - OpenCV 32 | 33 | 34 | ## 2. Reference 35 | 36 | The decomposition algorithm is proposed by ZHU chuanmin, TANG jun and XU tiangui 37 | 38 | from College of Mechanical Engineering, Tongji University, Shanghai, China 39 | 40 | The paper link : https://wenku.baidu.com/view/a3ccf9abf705cc1755270974.html 41 | 42 | The reason I take this algorithm cause I came from Tongji 43 | 44 | 45 | 46 | ## 3. Run 47 | 48 | ```cmake 49 | # build 50 | mkdir build 51 | cd build 52 | cmake .. 53 | make 54 | # run 55 | ./planner 56 | ``` 57 | 58 | -------------------------------------------------------------------------------- /doc/origin_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RJJxp/CoveragePlanning/697bad42d3da8585938401766603e82db9677990/doc/origin_01.png -------------------------------------------------------------------------------- /doc/result_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RJJxp/CoveragePlanning/697bad42d3da8585938401766603e82db9677990/doc/result_02.png -------------------------------------------------------------------------------- /doc/result_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RJJxp/CoveragePlanning/697bad42d3da8585938401766603e82db9677990/doc/result_03.png -------------------------------------------------------------------------------- /doc/result_04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RJJxp/CoveragePlanning/697bad42d3da8585938401766603e82db9677990/doc/result_04.png -------------------------------------------------------------------------------- /doc/凹多边形凸分解算法在快速原型中的应用_朱传敏.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RJJxp/CoveragePlanning/697bad42d3da8585938401766603e82db9677990/doc/凹多边形凸分解算法在快速原型中的应用_朱传敏.pdf -------------------------------------------------------------------------------- /include/bow_shaped_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef _BOW_SHAPED_PLANNER_H_ 2 | #define _BOW_SHAPED_PLANNER_H_ 3 | 4 | #include 5 | // #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include "decomposition.h" 14 | 15 | struct RjpTrajectory { 16 | std::vector pts; 17 | }; 18 | 19 | // generated a bow-shaped path with the given convex polygon 20 | // input `odometry` is not used, only to uniform the interface in tergeo 21 | // `sweeping_area` is the given convex polygon 22 | // `traj` is the path planed 23 | // Example: 24 | // BowShapedPlanner* bsp = new BowShapePlanner(); 25 | // bsp->coveragePlan(); 26 | class BowShapedPlanner { 27 | public: 28 | BowShapedPlanner(); 29 | ~BowShapedPlanner(); 30 | 31 | // bow shaped coverage plan by rjp 32 | bool coveragePlan(const ros_msgs::Odometry& odometry, 33 | const std::vector& sweeping_area, 34 | std::vector& multi_traj); 35 | private: 36 | // calculate the private variable `_rotate_distance` 37 | bool getRotateAngle(const std::vector& in_sweeping_area, double& rotate_angle); 38 | // rotate the sweeping_area 39 | // rotate the longest edge to pi/2 40 | bool rotateSweepArea(const std::vector& in_sweeping_area, 41 | double in_angle, 42 | std::vector& out_sweeping_area); 43 | // get the minx, miny, maxx, maxy of the given polygon 44 | bool getBoundOfSweepArea(const std::vector in_sweeping_area, 45 | double& min_x, double& min_y, double& max_x, double& max_y); 46 | // get the origin path which contains turning points only 47 | // turning points include all the start points and end points of the long bow-shape line 48 | bool getTurnPointOfBowShape(const std::vector& in_sweeping_area, 49 | std::vector >& out_ori_path); 50 | // add extra points between the start point and the end point for long and short bow-shape lines 51 | bool getStraightPointOfBowShape(const std::vector >& in_ori_path, 52 | double in_long_dist, double in_short_dist, 53 | std::vector& out_result_path); 54 | // get the result point with given x coordinate 55 | // if the result point's x range is between in_pt1 and in_pt2, return true, otherwise return false 56 | bool getPointFromX(const RjpPoint& in_pt1, const RjpPoint& in_pt2, 57 | double in_x, RjpPoint& out_pt); 58 | // Following are two functions mainly used in getStriaghtPointOfBowShape() 59 | // get the striaght points given the long bow-shape line in the form of std::pair 60 | bool getLongLinePoint(const std::pair& in_pt_pair, 61 | double long_dist, 62 | std::vector& out_part_traject); 63 | // given the short bow-shape line in the form first pair.second and the second pair.first 64 | bool getShortLinePoint(const std::pair& in_pt_pair_01, 65 | const std::pair& in_pt_pair_02, 66 | double short_dist, 67 | std::vector& out_part_traject); 68 | // calculate the angle between the X axis positive direction and vector(pt1, pt2) 69 | // TODO: the getAngle function is not good with input variable 70 | double getAngle(const RjpPoint& pt1, const RjpPoint& pt2); 71 | // plan for a convex polygon 72 | bool plan4ConvexPolygon(const std::vector& in_sweeping_area, RjpTrajectory& out_traj); 73 | 74 | private: 75 | double _rotate_angle; // pi/2 - angle of the shortest line of the sweeping area 76 | 77 | // param to control the bow shape 78 | // TODO: get value of them from config file including the sweeping area 79 | // TODO: temporarily ignore the transform between 2 coordinate systems of map and world 80 | // TODO: ambiguous distance unit: pixel or m? right now they are pixel 81 | double _offset_distance; // the distance offset inside the sweeping area to avoid collision, larger than 0 82 | double _space_distance; // the distance between the 2 bow-shape long lines 83 | double _ref_point_long_dist; // in the long bow-shape line with only start point and end point 84 | // add an extra ref_point every _ref_point_long_dist for the following planning 85 | // details in function `planning_plugins/raccoon_planning/reference_line::autoMarkReferenceLine` 86 | double _ref_point_short_dist; // same as _ref_point_long_dist, the distance between short line 87 | 88 | }; 89 | 90 | 91 | #endif -------------------------------------------------------------------------------- /include/decomposition.h: -------------------------------------------------------------------------------- 1 | #ifndef _DECOMPOSITION_H_ 2 | #define _DECOMPOSITION_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | struct RjpPoint 16 | { 17 | double x; 18 | double y; 19 | }; 20 | 21 | 22 | // decompose the polygon if concave, output the convex polygons 23 | // Example: 24 | // std::vector sweeping_area; 25 | // std::vector > split_sweeping_area; 26 | // PolygonDecomposition* pdc = new PolygonDecomposition(); 27 | // pdc->decomposePolygon(sweeping_area, split_sweeping_area); 28 | // delete pdc; 29 | class PolygonDecomposition { 30 | public: 31 | PolygonDecomposition(); // constructor 32 | ~PolygonDecomposition(); // deconstructor 33 | // interface 34 | bool decomposePolygon(const std::vector& in_polygon, 35 | std::vector >& out_result_polygons); 36 | private: 37 | // convert `RjpPoint` to `cv::Point2f`, reorder the polygon counterclockwise 38 | bool convertRos2CvPolygon(const std::vector& in_polygon); 39 | // compute the concave point of given polygon 40 | bool getConcavePointsIdx(const std::vector& in_concave_polygon_idx, std::vector& out_concave_pts_idx); 41 | // functional: decompose the polygon 42 | bool decomposeIt(); 43 | // take out the index of `_output_polygon_idx` to fill the `ros_msgs::Vecor2` polygon 44 | bool convertCv2RosPolygon(std::vector >& out_polygon_ros); 45 | // calculate the cross product 46 | double calCrossProduct(cv::Point2f pt_01, cv::Point2f pt_02, cv::Point2f pt_03); 47 | double calCrossProduct(RjpPoint pt_01, RjpPoint pt_02, RjpPoint pt_03); 48 | // take out the polygon_cv from the given index in `_polygon_cv` 49 | void getCvPolygonFromIdx(const std::vector& in_polygon_idx, std::vector& out_polygon_cv); 50 | 51 | 52 | // ******************************** algorithm part ******************************** 53 | // the decomposition algorithm is proposed by ZHU chuanmin, TANG jun and XU tiangui 54 | // from College of Mechanical Engineering, Tongji University, Shanghai, China 55 | // the paper link : https://wenku.baidu.com/view/a3ccf9abf705cc1755270974.html 56 | // the reason I take this algorithm cause I am a TongjiER 57 | 58 | // compute the visible points with given a concave point idx 59 | bool findVisiblePointsInSearchRange(const std::vector& in_concave_poly, 60 | int in_concave_pt_idx, std::vector& out_visible_points_idx); 61 | // compute the area where the potential split point address 62 | bool findSearchRange(const std::vector& in_concave_poly, int in_concave_pt_idx, std::vector& out_searched_points_idx); 63 | // return true if the given search_pt is visible to concave_pt 64 | bool judgeVisibility(const std::vector& in_concave_poly, int in_concave_pt_idx, int in_search_point_idx); 65 | // compute the power of visible points in search area intended to find the split point 66 | bool calPowerOfVisiblePoints(const std::vector& in_concave_poly, 67 | int in_concave_pt_idx, 68 | const std::vector& in_visible_points_idx, 69 | std::vector& out_power_vector); 70 | 71 | // ture if all polygons in `_result_polygon_idx` are convex 72 | // compute the concave polygons' index in `_result_polygon_idx` 73 | bool isAllConvex(std::vector& out_concave_polygon_idx); 74 | // return false if the 2 lines are parallel 75 | bool getIntersectPoints(cv::Point2f line01_start, cv::Point2f line01_end, 76 | cv::Point2f line02_start, cv::Point2f line02_end, 77 | cv::Point2f& intersect_pt); 78 | // split the concave polygon into 2 polygons with given concave point and split point 79 | bool getSplitPolygons(const std::vector& in_concave_poly, int in_concave_pt_idx, int in_split_pt_idx, 80 | std::vector >& out_polys); 81 | 82 | private: 83 | std::vector _polygon_cv; // input one polygon in cv 84 | std::vector > _result_polygon_idx; // generate polygons index considering the topography 85 | }; 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/.decomposition.cpp.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RJJxp/CoveragePlanning/697bad42d3da8585938401766603e82db9677990/src/.decomposition.cpp.swp -------------------------------------------------------------------------------- /src/bow_shaped_planner.cpp: -------------------------------------------------------------------------------- 1 | #include "bow_shaped_planner.h" 2 | #include "decomposition.h" 3 | 4 | BowShapedPlanner::BowShapedPlanner(){ 5 | _offset_distance = 5; 6 | _space_distance = 10; 7 | _ref_point_long_dist = 20; 8 | _ref_point_short_dist = 20; 9 | } 10 | 11 | BowShapedPlanner::~BowShapedPlanner(){ 12 | } 13 | 14 | 15 | bool BowShapedPlanner::coveragePlan(const ros_msgs::Odometry& odometry, 16 | const std::vector& sweeping_area, 17 | std::vector& multi_traj) { 18 | multi_traj.clear(); 19 | 20 | PolygonDecomposition pdc; 21 | std::vector > split_sweeping_area; 22 | pdc.decomposePolygon(sweeping_area, split_sweeping_area); 23 | // debug 24 | cv::Mat my_panel(1000, 1000, CV_8UC3, cv::Scalar(40, 0, 0)); 25 | for (int i = 0; i < split_sweeping_area.size(); ++i) { 26 | double x, y; 27 | for (int j = 0; j < split_sweeping_area[i].size() - 1; ++j) { 28 | x = split_sweeping_area[i][j].x; 29 | y = split_sweeping_area[i][j].y; 30 | cv::Point2f p1(x, y); 31 | x = split_sweeping_area[i][j + 1].x; 32 | y = split_sweeping_area[i][j + 1].y; 33 | cv::Point2f p2(x, y); 34 | cv::line(my_panel, p1, p2, cv::Scalar(0, 200, 0), 2); 35 | } 36 | x = split_sweeping_area[i][split_sweeping_area[i].size() - 1].x; 37 | y = split_sweeping_area[i][split_sweeping_area[i].size() - 1].y; 38 | cv::Point2f p1(x, y); 39 | x = split_sweeping_area[i][0].x; 40 | y = split_sweeping_area[i][0].y; 41 | cv::Point2f p2(x, y); 42 | cv::line(my_panel, p1, p2, cv::Scalar(0, 200, 0), 2); 43 | } 44 | cv::imshow("split_polygons", my_panel); 45 | cv::waitKey(0); 46 | 47 | std::cout << "**************** decompose finished ****************" << std::endl; 48 | for (int i = 0; i < split_sweeping_area.size(); ++i) { 49 | RjpTrajectory sub_traj; 50 | plan4ConvexPolygon(split_sweeping_area[i], sub_traj); 51 | multi_traj.push_back(sub_traj); 52 | } 53 | return true; 54 | 55 | } 56 | 57 | bool BowShapedPlanner::plan4ConvexPolygon(const std::vector& in_sweeping_area, 58 | RjpTrajectory& out_traj) { 59 | std::vector my_sweeping_area = {}; 60 | for (int i = 0; i < in_sweeping_area.size(); ++i) { 61 | my_sweeping_area.push_back(in_sweeping_area[i]); 62 | } 63 | 64 | my_sweeping_area.push_back(in_sweeping_area[0]); 65 | // use varible `my_sweeping_area` to replace `sweeping_area` 66 | // ATTENTION: `my_sweeping_area` has the same first and last element 67 | std::cout << "finish copying sweeping area"<< std::endl; 68 | 69 | double rotate_angle = 0.0; 70 | if (!getRotateAngle(my_sweeping_area, rotate_angle)) { 71 | std::cout << "get rotate angle failed" << std::endl; 72 | return false; 73 | } 74 | // for debug 75 | _rotate_angle = rotate_angle; 76 | 77 | std::vector my_sweeping_area_rotate = {}; 78 | if(!rotateSweepArea(my_sweeping_area, rotate_angle, my_sweeping_area_rotate)) { 79 | std::cout << "rotate sweep area failed" << std::endl; 80 | return false; 81 | } 82 | 83 | std::vector > ori_path; 84 | if (!getTurnPointOfBowShape(my_sweeping_area_rotate, ori_path)) { 85 | std::cout << "get turn point of bow-shape failed" << std::endl; 86 | return false; 87 | } 88 | 89 | std::vector result_path; 90 | if (!getStraightPointOfBowShape(ori_path, _ref_point_long_dist, _ref_point_short_dist, result_path)) { 91 | std::cout << "get straight line points failed" << std::endl; 92 | return false; 93 | } 94 | 95 | std::vector result_path_rotate; 96 | if (!rotateSweepArea(result_path, -rotate_angle, result_path_rotate)) { 97 | std::cout << "get result path rotate failed" << std::endl; 98 | return false; 99 | } 100 | 101 | RjpPoint tmp_pose; 102 | for (int i = 0; i < result_path_rotate.size(); ++i) { 103 | // std::cout << "x " << result_path_rotate[i].x << std::endl << "y " << result_path_rotate[i].y << std::endl; 104 | 105 | tmp_pose.x = result_path_rotate[i].x; 106 | tmp_pose.y = result_path_rotate[i].y; 107 | out_traj.pts.push_back(tmp_pose); 108 | } 109 | std::cout << "coverage plan succeed" << std::endl; 110 | return true; 111 | } 112 | 113 | bool BowShapedPlanner::getRotateAngle(const std::vector& in_sweeping_area, 114 | double& rotate_angle) { 115 | // make sure sweeping area is a polygon 116 | if (in_sweeping_area.size() < 3) { 117 | std::cout << "the sweeping area is not a polygon in `getRotationAngle`" << std::endl; 118 | return false; 119 | } 120 | 121 | double dist, del_x, del_y; 122 | for (int i = 0; i < in_sweeping_area.size() - 1; ++i) { 123 | del_x = in_sweeping_area[i + 1].x - in_sweeping_area[i].x; 124 | del_y = in_sweeping_area[i + 1].y - in_sweeping_area[i].y; 125 | if (i == 0) { // the first point 126 | dist = del_x * del_x + del_y * del_y; 127 | rotate_angle = M_PI / 2 - atan2f(del_y, del_x); 128 | } else { // the other points 129 | if (dist < del_x * del_x + del_y * del_y) { 130 | dist = del_x * del_x + del_y * del_y; 131 | rotate_angle = M_PI / 2 - atan2f(del_y, del_x); 132 | } 133 | } 134 | // std::cout << i << ": " << dist << std::endl; 135 | } 136 | std::cout << "get rotate angle succeed, the angle is " << rotate_angle << " rad" << std::endl; 137 | return true; 138 | } 139 | 140 | bool BowShapedPlanner::rotateSweepArea(const std::vector& in_sweeping_area, 141 | double in_angle, 142 | std::vector& out_sweeping_area) { 143 | if (in_sweeping_area.size() < 4) { 144 | std::cout << "the sweeping area is not a polygon in `rotateSweepArea`" << std::endl; 145 | return false; 146 | } 147 | 148 | out_sweeping_area.clear(); 149 | RjpPoint tmp_vec; 150 | // rotate counterclockwise 151 | for (int i = 0;i < in_sweeping_area.size(); ++i) { 152 | tmp_vec.x = cos(in_angle) * in_sweeping_area[i].x - sin(in_angle) * in_sweeping_area[i].y; 153 | tmp_vec.y = sin(in_angle) * in_sweeping_area[i].x + cos(in_angle) * in_sweeping_area[i].y; 154 | out_sweeping_area.push_back(tmp_vec); 155 | } 156 | std::cout << "finished rotate sweep area" << std::endl; 157 | return true; 158 | } 159 | 160 | bool BowShapedPlanner::getBoundOfSweepArea(const std::vector in_sweeping_area, 161 | double& min_x, double& min_y, double& max_x, double& max_y) { 162 | if (in_sweeping_area.size() < 4) { 163 | std::cout << "the sweeping area is not a polygon in `getBoundOfSweepArea`" << std::endl; 164 | return false; 165 | } 166 | 167 | // use iter and std function to calculate the min and max 168 | // auto iter = std::min_element(in_sweeping_area.begin(), in_sweeping_area.end(), 169 | // [](const RjpPoint v1, const RjpPoint v2){ 170 | // return v1.x < v2.x; 171 | // }); 172 | // min_x = *iter; 173 | // iter = std::min_element(in_sweeping_area.begin(), in_sweeping_area.end(), 174 | // [](const RjpPoint v1, const RjpPoint v2){ 175 | // return v1.y < v2.y; 176 | // }); 177 | // min_y = *iter; 178 | // iter = std::max_element(in_sweeping_area.begin(), in_sweeping_area.end(), 179 | // [](const RjpPoint v1, const RjpPoint v2){ 180 | // return v1.x < v2.x; 181 | // }); 182 | // max_x = *iter; 183 | // iter = std::max_element(in_sweeping_area.begin(), in_sweeping_area.end(), 184 | // [](const RjpPoint v1, const RjpPoint v2){ 185 | // return v1.y < v2.y; 186 | // }); 187 | // max_y = *iter; 188 | 189 | for (int i = 0; i < in_sweeping_area.size(); ++i) { 190 | if (i == 0) { 191 | min_x = in_sweeping_area[i].x; 192 | max_x = in_sweeping_area[i].x; 193 | min_y = in_sweeping_area[i].y; 194 | max_y = in_sweeping_area[i].y; 195 | } else { 196 | min_x = in_sweeping_area[i].x < min_x ? in_sweeping_area[i].x : min_x; 197 | min_y = in_sweeping_area[i].y < min_y ? in_sweeping_area[i].y : min_y; 198 | max_x = max_x < in_sweeping_area[i].x ? in_sweeping_area[i].x : max_x; 199 | max_y = max_y < in_sweeping_area[i].y ? in_sweeping_area[i].y : max_y; 200 | } 201 | 202 | } 203 | std::cout << "get bound func is finished" << std::endl; 204 | std::cout << "minx:\t" << min_x << "\t max_x \t" << max_x << std::endl; 205 | std::cout << "miny:\t" << min_y << "\t max_y \t" << max_y << std::endl; 206 | return true; 207 | } 208 | 209 | // use a couple of cut-lines which are parallel with y axis to intersect the polygon 210 | // the part of cut-line inside the polygon become the long bow-shape line 211 | // the 2 intersections become the start point and end point, which I call turn point in general 212 | // then arrange them in order 213 | bool BowShapedPlanner::getTurnPointOfBowShape(const std::vector& in_sweeping_area, 214 | std::vector >& out_ori_path) { 215 | if (in_sweeping_area.size() < 4) { 216 | std::cout << "sweeping area is not a polygon in getTurnPoint func" << std::endl; 217 | return false; 218 | } 219 | 220 | double min_x, min_y, max_x, max_y; 221 | if (!getBoundOfSweepArea(in_sweeping_area, min_x, min_y, max_x, max_y)) { 222 | std::cout << "get bound of sweep area failed" << std::endl; 223 | return false; 224 | } 225 | 226 | std::pair tmp_pair; 227 | std::vector tmp_vec; // every time enter a loop, clear this vector 228 | RjpPoint tmp_pt; 229 | bool order_flag = true; 230 | double cutline = min_x + _offset_distance; 231 | 232 | while (cutline < max_x - _offset_distance) { 233 | for (int i = 0; i < in_sweeping_area.size(); ++i) { 234 | if (getPointFromX(in_sweeping_area[i], in_sweeping_area[i + 1], cutline, tmp_pt)) { 235 | tmp_vec.push_back(tmp_pt); 236 | } else { 237 | // the cutline is parallel to edge of polygon 238 | } 239 | } 240 | // arrange in order 241 | // guarantee the out_ori_path[i].second's next point is out_ori_path[i + 1].first 242 | if (tmp_vec.size() < 2) { 243 | std::cout << "not enough points from cutline " << cutline << std::endl; 244 | return false; 245 | } 246 | if (order_flag) { 247 | tmp_pair.first = tmp_vec[0]; 248 | tmp_pair.second = tmp_vec[1]; 249 | order_flag = false; 250 | } else { 251 | tmp_pair.first = tmp_vec[1]; 252 | tmp_pair.second = tmp_vec[0]; 253 | order_flag = true; 254 | } 255 | tmp_vec.clear(); 256 | out_ori_path.push_back(tmp_pair); 257 | cutline += _space_distance; 258 | } 259 | std::cout << "the turn path length is " << out_ori_path.size() << std::endl; 260 | std::cout << "get turn point func succeed" << std::endl; 261 | 262 | // for (int i = 0; i < out_ori_path.size(); ++i) { 263 | // std::cout << "fir.x " << out_ori_path[i].first.x << " fir.y " << out_ori_path[i].first.y << std::endl; 264 | // std::cout << "sec.x " << out_ori_path[i].second.x << " sec.y " << out_ori_path[i].second.y << std::endl; 265 | // } 266 | 267 | // for visualization debug 268 | // cv::Mat my_panel(1000, 1000, CV_8UC3, cv::Scalar(0, 0, 0)); 269 | // for (int i = 0; i < out_ori_path.size(); ++i) { 270 | // cv::Point2f tmp_cv_pt; 271 | // tmp_cv_pt.x = cos(_rotate_angle) * out_ori_path[i].first.x + sin(_rotate_angle) * out_ori_path[i].first.y; 272 | // tmp_cv_pt.y = sin(-_rotate_angle) * out_ori_path[i].first.x + cos(_rotate_angle) * out_ori_path[i].first.y; 273 | // cv::circle(my_panel, tmp_cv_pt, 3, cv::Scalar(255, 255, 0), 1, CV_FILLED); 274 | // tmp_cv_pt.x = cos(_rotate_angle) * out_ori_path[i].second.x + sin(_rotate_angle) * out_ori_path[i].second.y; 275 | // tmp_cv_pt.y = sin(-_rotate_angle) * out_ori_path[i].second.x + cos(_rotate_angle) * out_ori_path[i].second.y; 276 | // cv::circle(my_panel, tmp_cv_pt, 3, cv::Scalar(255, 255, 0), 1, CV_FILLED); 277 | // } 278 | // cv::imshow("turn points", my_panel); 279 | // cv::waitKey(0); 280 | 281 | return true; 282 | } 283 | 284 | bool BowShapedPlanner::getPointFromX(const RjpPoint& in_pt1, const RjpPoint& in_pt2, 285 | double in_x, RjpPoint& out_pt) { 286 | if ((in_pt1.x - in_x) * (in_pt2.x - in_x) > 0) return false; 287 | 288 | // the line of in_pt1 and in_pt2 is parallel to cut line 289 | if (in_pt2.x == in_pt1.x) return false; 290 | 291 | double scale = (in_x - in_pt1.x) / (in_pt2.x - in_pt1.x); 292 | out_pt.x = in_x; 293 | out_pt.y = in_pt1.y + scale * (in_pt2.y - in_pt1.y); 294 | return true; 295 | } 296 | 297 | // bow-shape long line is parallel with y axis 298 | // bow-shape short line is parallel with x axis 299 | bool BowShapedPlanner::getStraightPointOfBowShape(const std::vector >& in_ori_path, 300 | double in_long_dist, double in_short_dist, 301 | std::vector& out_result_path) { 302 | std::vector tmp_path; 303 | for (int i = 0; i < in_ori_path.size() - 1; ++i) { 304 | if (!getLongLinePoint(in_ori_path[i], in_long_dist, tmp_path)) { 305 | std::cout << ": get long line point failed" << std::endl; 306 | return false; 307 | } 308 | out_result_path.insert(out_result_path.end(), tmp_path.begin(), tmp_path.end()); 309 | if (!getShortLinePoint(in_ori_path[i], in_ori_path[i + 1], in_short_dist, tmp_path)) { 310 | std::cout << ": get short line point failed" << std::endl; 311 | return false; 312 | } 313 | out_result_path.insert(out_result_path.end(), tmp_path.begin(), tmp_path.end()); 314 | } 315 | 316 | if (!getLongLinePoint(in_ori_path[in_ori_path.size() - 1], in_long_dist, tmp_path)) { 317 | std::cout << "the last get long line point failed" << std::endl; 318 | return false; 319 | } 320 | 321 | out_result_path.insert(out_result_path.end(), tmp_path.begin(), tmp_path.end()); 322 | out_result_path.push_back(in_ori_path[in_ori_path.size() - 1].second); 323 | return true; 324 | } 325 | 326 | // the straight points in long bow-shape line range from [start_pt, end_pt) 327 | bool BowShapedPlanner::getLongLinePoint(const std::pair& in_pt_pair, 328 | double long_dist, 329 | std::vector& out_part_traject) { 330 | out_part_traject.clear(); 331 | 332 | if (in_pt_pair.first.x != in_pt_pair.second.x) { 333 | std::cout << "the given pt pair's X is not equal" << std::endl; 334 | return false; 335 | } 336 | 337 | double length = in_pt_pair.second.y - in_pt_pair.first.y; 338 | double length_flag = length; 339 | if (length < 0) { 340 | long_dist = -long_dist; 341 | } 342 | 343 | RjpPoint tmp_pt = in_pt_pair.first; 344 | out_part_traject.push_back(tmp_pt); 345 | length_flag = length_flag - long_dist; 346 | while (length * length_flag > 0) { 347 | length_flag = length_flag - long_dist; 348 | tmp_pt.y += long_dist; 349 | out_part_traject.push_back(tmp_pt); 350 | } 351 | return true; 352 | } 353 | 354 | // the straight points in short bow-shape line range from [pair_01.second, pair_02.first) 355 | // move short dist every time along the vector(pair_01.second, pair_02.first) 356 | bool BowShapedPlanner::getShortLinePoint(const std::pair& in_pt_pair_01, 357 | const std::pair& in_pt_pair_02, 358 | double short_dist, 359 | std::vector& out_part_traject) { 360 | out_part_traject.clear(); 361 | double length = in_pt_pair_02.first.x - in_pt_pair_01.second.x; 362 | double slope = (in_pt_pair_02.first.y - in_pt_pair_01.second.y) / length; 363 | double angle = getAngle(in_pt_pair_01.second, in_pt_pair_02.first); 364 | double length_flag = length; 365 | double short_dist_copy = short_dist; 366 | if (length < 0) { 367 | short_dist = -short_dist; 368 | } 369 | 370 | RjpPoint tmp_pt = in_pt_pair_01.second; 371 | out_part_traject.push_back(tmp_pt); 372 | length_flag = length_flag - short_dist * fabs(cos(angle)); 373 | while (length * length_flag > 0) { 374 | length_flag = length_flag - short_dist; 375 | tmp_pt.x += short_dist_copy * cos(angle); 376 | tmp_pt.y += short_dist_copy * sin(angle); 377 | out_part_traject.push_back(tmp_pt); 378 | } 379 | return true; 380 | } 381 | 382 | double BowShapedPlanner::getAngle(const RjpPoint& pt1, const RjpPoint& pt2) { 383 | // parallel with axis y 384 | if (pt1.x == pt2.x) { 385 | if (pt1.y < pt2.y) { 386 | return M_PI_2; 387 | } else { 388 | return -M_PI_2; 389 | } 390 | } 391 | // function `atan2` ranges from (-pi/2, pi/2) 392 | // which indicates the pt2 is in the right of pt1 by default 393 | double angle_atan = atan2(pt2.y - pt1.y, pt2.x - pt1.x); 394 | // if the pt2 is in the left of pt1 395 | if (pt2.x < pt1.x) { 396 | angle_atan += M_PI; 397 | } 398 | return angle_atan; 399 | } -------------------------------------------------------------------------------- /src/decomposition.cpp: -------------------------------------------------------------------------------- 1 | #include "decomposition.h" 2 | /* 3 | For topographic consideration, use index of each point instead of points directly 4 | It enable us to use Int to represent a point along with the point's topography 5 | The key to understand these codes is to distinguish what the index points to 6 | Which is the core of GIS thinking 7 | 8 | Specifically, we have origin_polygon, index_polygon 9 | origin_polygon is the data set, index_polygon is the index of the origin_polygon 10 | You have to make sure the index is for origin_polygon or index_polygon 11 | 12 | */ 13 | 14 | PolygonDecomposition::PolygonDecomposition () { 15 | 16 | } 17 | 18 | PolygonDecomposition::~PolygonDecomposition () { 19 | 20 | } 21 | 22 | // interface 23 | // input, output data type is `RjpPoint` 24 | // use cv::Point in computing for the convenience of OpenCV lib 25 | bool PolygonDecomposition::decomposePolygon(const std::vector& in_polygon, 26 | std::vector >& out_result_polygons) { 27 | 28 | if (!convertRos2CvPolygon(in_polygon)) { 29 | std::cout << "convertRos2CvPolygon() failed" << std::endl; 30 | return false; 31 | } 32 | 33 | if (!decomposeIt()) { 34 | std::cout << "decomposeIt() failed" << std::endl; 35 | return false; 36 | } 37 | 38 | if (!convertCv2RosPolygon(out_result_polygons)) { 39 | std::cout << "convertCv2RosPolygon() failed" << std::endl; 40 | return false; 41 | } 42 | 43 | std::cout << "decomposePolygon() succeeded" << std::endl; 44 | return true; 45 | } 46 | 47 | // find the xmin or xmax or ymin or ymax point in polygon 48 | // that's feature point P_{i} which determines the polygon's direction (clockwise or counterclockwise) 49 | // use vec(P_{i-1}, P{i}) cross vec(P_{i}, P_{i+1}) to judge polygon's direction 50 | // save `in_polygon` in the private varible `_polygon_cv` counterclockwise 51 | // initial private varible `_result_polygon_idx` which carries the index of all polygons 52 | bool PolygonDecomposition::convertRos2CvPolygon(const std::vector& in_polygon) { 53 | if (in_polygon.size() < 3) { 54 | std::cout << "the input is not a polygon with the size of " << in_polygon.size() << std::endl; 55 | return false; 56 | } 57 | 58 | // find the ymin 59 | auto ymin_iter = std::min_element(in_polygon.begin(), in_polygon.end(), 60 | [](RjpPoint p1, RjpPoint p2){return p1.y < p2.y;}); 61 | int y_min_idx = std::distance(in_polygon.begin(), ymin_iter); 62 | 63 | // calculate the cross product 64 | double cross_product; 65 | if (y_min_idx == 0) { 66 | cross_product = calCrossProduct(in_polygon[in_polygon.size() - 1], 67 | in_polygon[y_min_idx], 68 | in_polygon[y_min_idx + 1]); 69 | } else if (y_min_idx = in_polygon.size() - 1) { 70 | cross_product = calCrossProduct(in_polygon[y_min_idx - 1], 71 | in_polygon[y_min_idx], 72 | in_polygon[0]); 73 | } else { 74 | cross_product = calCrossProduct(in_polygon[y_min_idx - 1], 75 | in_polygon[y_min_idx], 76 | in_polygon[y_min_idx + 1]); 77 | } 78 | 79 | // arrange the polygon counterclockwise 80 | if (cross_product > 0) { // counterclockwise 81 | std::cout << " the polygon's direction is counterclockwise" << std::endl; 82 | for (int i = 0; i < in_polygon.size(); ++i) { 83 | _polygon_cv.push_back(cv::Point2f(in_polygon[i].x, in_polygon[i].y)); 84 | } 85 | } else { // clockwise 86 | std::cout << " the polygon's direction is clockwise" << std::endl; 87 | _polygon_cv.push_back(cv::Point2f(in_polygon[0].x, in_polygon[0].y)); 88 | for (int i = in_polygon.size() - 1; i >0; --i) { 89 | _polygon_cv.push_back(cv::Point2f(in_polygon[i].x, in_polygon[i].y)); 90 | } 91 | } 92 | 93 | // initial _result_polygon_idx 94 | std::vector tmp_poly; 95 | for (int i = 0; i < _polygon_cv.size(); ++i) { 96 | tmp_poly.push_back(i); 97 | } 98 | _result_polygon_idx.push_back(tmp_poly); 99 | 100 | std::cout << " convertRos2CvPolygon() succeeded" << std::endl; 101 | return true; 102 | } 103 | 104 | // the `in_polygon_idx` index to origin_polygon 105 | void PolygonDecomposition::getCvPolygonFromIdx(const std::vector& in_polygon_idx, 106 | std::vector& out_polygon_cv) { 107 | out_polygon_cv.clear(); 108 | // std::cout << "inpoly_idx size " << in_polygon_idx.size() << std::endl; 109 | for (int i = 0; i < in_polygon_idx.size(); i++) { 110 | // std::cout << "x " << _polygon_cv[in_polygon_idx[i]].x; 111 | // std::cout << " y " << _polygon_cv[in_polygon_idx[i]].y << std::endl; 112 | out_polygon_cv.push_back(_polygon_cv[in_polygon_idx[i]]); 113 | } 114 | } 115 | 116 | // use OpenCV to judge whether the polygon is a convex as a double check 117 | // current polygon is arranged counterclockwise 118 | // so if the cross product of vec(p-1, p) and vec(p, p+1) is less than 0 119 | // the point `p` is a concave point 120 | bool PolygonDecomposition::getConcavePointsIdx(const std::vector& in_idx, 121 | std::vector& out_pts_idx) { 122 | // double check the `in_idx` polygon 123 | out_pts_idx.clear(); 124 | 125 | std::vector in_concave_polygon_cv; 126 | getCvPolygonFromIdx(in_idx, in_concave_polygon_cv); 127 | 128 | if (cv::isContourConvex(in_concave_polygon_cv)) { 129 | std::cout << "the polygon is convex" << std::endl; 130 | // std::cout << "getConcavePoints() failed " << std::endl; 131 | return false; 132 | } 133 | 134 | for (int i = 0; i < in_idx.size(); ++i) { 135 | double cross_product; 136 | if (i == 0) { // first point 137 | cross_product = calCrossProduct(_polygon_cv[in_idx[in_idx.size() - 1]], 138 | _polygon_cv[in_idx[i]], 139 | _polygon_cv[in_idx[i + 1]]); 140 | } else if (i == in_idx.size() - 1) { // last point 141 | cross_product = calCrossProduct(_polygon_cv[in_idx[i - 1]], 142 | _polygon_cv[in_idx[i]], 143 | _polygon_cv[in_idx[0]]); 144 | } else { // other points 145 | cross_product = calCrossProduct(_polygon_cv[in_idx[i - 1]], 146 | _polygon_cv[in_idx[i]], 147 | _polygon_cv[in_idx[i + 1]]); 148 | } 149 | 150 | if (cross_product < 0) { // convex 151 | out_pts_idx.push_back(i); 152 | } 153 | } 154 | 155 | // double check 156 | if (out_pts_idx.size() == 0) { 157 | std::cout << "the concave point size is 0 " << std::endl; 158 | return false; 159 | } 160 | 161 | std::cout << "getConcavePoints() succeeded" << std::endl; 162 | return true; 163 | } 164 | 165 | // construct the `_result_polygon_idx` which index the arranged `_polygon_cv` 166 | // simplify the conventiol algorithm to statisfy my demand 167 | // update `_result_polygon_idx` when there is still a concave polygon in it 168 | // compute `_output_polygon_idx` to save the decomposed multi-convex-polygons 169 | bool PolygonDecomposition::decomposeIt() { 170 | int count = 0; 171 | std::vector > copy_result_polygon_idx = _result_polygon_idx; 172 | std::vector concave_polygon_idx; 173 | while (!isAllConvex(concave_polygon_idx)) { 174 | std::cout << "there is " << concave_polygon_idx.size() << " concave now" << std::endl; 175 | for (int i = 0; i < concave_polygon_idx.size(); ++i) { 176 | std::vector concave_poly = _result_polygon_idx[concave_polygon_idx[i]]; 177 | std::vector concave_pts_idx; 178 | if (!getConcavePointsIdx(concave_poly ,concave_pts_idx)) { 179 | std::cout << "getConcavePointsIdx() failed" << std::endl; 180 | return false; 181 | } 182 | 183 | int first_pt_idx = concave_pts_idx[0]; // first concave point idx counterclockwise 184 | std::cout << "the first concavee pt is " << _polygon_cv[concave_poly[first_pt_idx]] << std::endl; 185 | 186 | std::vector visible_pts_idx; 187 | if (!findVisiblePointsInSearchRange(concave_poly, first_pt_idx, visible_pts_idx)) { 188 | std::cout << "findVisiblePointsInSearchRange() failed" << count << std::endl; 189 | } 190 | if (visible_pts_idx.size() == 0) { // actually, there is at least one point 191 | std::cout << "visible_pts_idx size is 0" << std::endl; 192 | return false; 193 | } 194 | 195 | std::cout << "the visible pts are" < power_vector; 202 | calPowerOfVisiblePoints(concave_poly, first_pt_idx, visible_pts_idx, power_vector); 203 | 204 | std::cout << "the power vector is " << std::endl; 205 | for (int i = 0; i < power_vector.size(); ++i) { 206 | std::cout << i << " " << power_vector[i] << std::endl; 207 | } 208 | 209 | auto max_value = std::max_element(power_vector.begin(), power_vector.end()); 210 | auto max_idx = std::distance(power_vector.begin(), max_value); 211 | int split_pt_idx = visible_pts_idx[max_idx]; 212 | 213 | std::vector > split_polygons; 214 | getSplitPolygons(concave_poly, first_pt_idx, split_pt_idx, split_polygons); 215 | if (split_polygons.size() != 2) { 216 | std::cout << "the split polygons size is not 2 " << ", is " << split_polygons.size() << std::endl; 217 | return false; 218 | } 219 | 220 | copy_result_polygon_idx.push_back(split_polygons[0]); 221 | copy_result_polygon_idx.push_back(split_polygons[1]); 222 | } 223 | bool minus_one = false; 224 | for (int i = 0; i < concave_polygon_idx.size(); ++i) { 225 | if (!minus_one) { 226 | copy_result_polygon_idx.erase(copy_result_polygon_idx.begin() + concave_polygon_idx[i]); 227 | minus_one = true; 228 | } else { 229 | copy_result_polygon_idx.erase(copy_result_polygon_idx.begin() + concave_polygon_idx[i] - 1); 230 | } 231 | 232 | } 233 | _result_polygon_idx = copy_result_polygon_idx; 234 | 235 | std::cout << "the result is" << std::endl; 236 | for (int i = 0; i < _result_polygon_idx.size(); ++i) { 237 | for (int j = 0; j < _result_polygon_idx[i].size(); ++j) { 238 | int idx = _result_polygon_idx[i][j]; 239 | std::cout << _polygon_cv[idx]; 240 | } 241 | std::cout << std::endl; 242 | } 243 | std::cout << "***********************************" << std::endl; 244 | count++; 245 | } 246 | return true; 247 | } 248 | 249 | // when finished the decomposition, `_result_polygon_idx` to the output variable 250 | bool PolygonDecomposition::convertCv2RosPolygon(std::vector >& out_polygon_ros) { 251 | out_polygon_ros.clear(); 252 | for (int i = 0; i < _result_polygon_idx.size(); ++i) { 253 | std::vector sub_polygon; 254 | for (int j = 0; j < _result_polygon_idx[i].size(); ++j) { 255 | RjpPoint sub_polygon_pt; 256 | sub_polygon_pt.x = _polygon_cv[_result_polygon_idx[i][j]].x; 257 | sub_polygon_pt.y = _polygon_cv[_result_polygon_idx[i][j]].y; 258 | sub_polygon.push_back(sub_polygon_pt); 259 | } 260 | out_polygon_ros.push_back(sub_polygon); 261 | } 262 | return true; 263 | } 264 | 265 | double PolygonDecomposition::calCrossProduct(cv::Point2f pt_01, cv::Point2f pt_02, cv::Point2f pt_03) { 266 | double vec_01_x = pt_02.x - pt_01.x; 267 | double vec_01_y = pt_02.y - pt_01.y; 268 | double vec_02_x = pt_03.x - pt_02.x; 269 | double vec_02_y = pt_03.y - pt_02.y; 270 | return (vec_01_x * vec_02_y - vec_01_y * vec_02_x); 271 | } 272 | 273 | double PolygonDecomposition::calCrossProduct(RjpPoint pt_01, RjpPoint pt_02, RjpPoint pt_03) { 274 | double vec_01_x = pt_02.x - pt_01.x; 275 | double vec_01_y = pt_02.y - pt_01.y; 276 | double vec_02_x = pt_03.x - pt_02.x; 277 | double vec_02_y = pt_03.y - pt_02.y; 278 | return (vec_01_x * vec_02_y - vec_01_y * vec_02_x); 279 | } 280 | 281 | // the given `in_pt_idx` is the first concave point counterclockwise from the start point of the polygon 282 | // `in_poly` index to origin_polygon `_polygon_cv` 283 | // `in_pt_idx` index to index_polygon `_in_poly` 284 | // so by the twice index, we can get the point's data according to an Int using `_polygon[in_poly[in_pt_idx]]` 285 | // Visible Points definition in the paper mentioned in .h file 286 | // `out_pts_idx` is index to index_polygon `in_poly` 287 | bool PolygonDecomposition::findVisiblePointsInSearchRange(const std::vector& in_poly, 288 | int in_pt_idx, 289 | std::vector& out_pts_idx) { 290 | out_pts_idx.clear(); 291 | 292 | std::vector search_all_pts_idx; // index for index_polygon `in_poly` 293 | 294 | if (!findSearchRange(in_poly, in_pt_idx, search_all_pts_idx)) { 295 | std::cout << "findSearchRange() failed" << std::endl; 296 | return false; 297 | } 298 | 299 | std::cout << "the search range pts" << std::endl; 300 | for (int i = 0; i < search_all_pts_idx.size(); ++i) { 301 | std::cout << _polygon_cv[in_poly[search_all_pts_idx[i]]]; 302 | } 303 | std::cout << std::endl; 304 | 305 | if (search_all_pts_idx.size() == 0) { 306 | std::cout << "findVisiblePointsInSearchRange(): No visible points in search range" << std::endl; 307 | return false; 308 | } 309 | 310 | for (int i = 0; i < search_all_pts_idx.size(); ++i) { 311 | if (judgeVisibility(in_poly, in_pt_idx, search_all_pts_idx[i])) { 312 | out_pts_idx.push_back(search_all_pts_idx[i]); 313 | } 314 | } 315 | return true; 316 | } 317 | 318 | // Point A(X_a, Y_a), Point B(X_b, Y_b) 319 | // F(x, y) = (Y_b - Y_a)x + (X_a - X_b)y + (Y_a * X_b - X_a * Y_b) 320 | // F(x, y) > 0, the Point(x, y) is in the right region of vector(A, B) 321 | // otherwise in the right region 322 | // in this function, we search for the left region 323 | // `out_points_idx` is index for index_polygon `in_poly` 324 | bool PolygonDecomposition::findSearchRange(const std::vector& in_poly, 325 | int in_pt_idx, 326 | std::vector& out_points_idx) { 327 | out_points_idx.clear(); 328 | 329 | int A_idx = in_pt_idx; 330 | int B_idx = (A_idx == in_poly.size() - 1) ? 0 : A_idx + 1; // the A's next point counterclockwise 331 | 332 | double A_x, B_y, C_c; 333 | A_x = _polygon_cv[in_poly[B_idx]].y - _polygon_cv[in_poly[A_idx]].y; 334 | B_y = _polygon_cv[in_poly[A_idx]].x - _polygon_cv[in_poly[B_idx]].x; 335 | C_c = _polygon_cv[in_poly[A_idx]].y * _polygon_cv[in_poly[B_idx]].x - _polygon_cv[in_poly[A_idx]].x * _polygon_cv[in_poly[B_idx]].y; 336 | 337 | for (int i = 0; i < in_poly.size(); ++i) { 338 | if (i != A_idx && i != B_idx) { 339 | if ((A_x * _polygon_cv[in_poly[i]].x + B_y * _polygon_cv[in_poly[i]].y + C_c) < 0) { // less than 0 is left region 340 | out_points_idx.push_back(i); 341 | } 342 | } 343 | } 344 | return true; 345 | } 346 | 347 | // TODO: a little problem here, though it works for the complicated situation 348 | // the way I implement this is really stupid and low efficient 349 | // When I search the methods to judge the visible points of polygon 350 | // the algorithm is fucking huge!!! 351 | bool PolygonDecomposition::judgeVisibility(const std::vector& in_poly, 352 | int in_concave_pt_idx, int in_search_pt_idx) { 353 | int pt_A = in_concave_pt_idx; 354 | int pt_A_front = (pt_A == 0) ? (in_poly.size() - 1) : (pt_A - 1); 355 | int pt_B = in_search_pt_idx; 356 | int pt_B_front = (pt_B == 0) ? (in_poly.size() - 1) : (pt_B - 1); 357 | 358 | cv::Point2f line01_start = _polygon_cv[in_poly[pt_A]]; 359 | cv::Point2f line01_end = _polygon_cv[in_poly[pt_B]]; 360 | 361 | for (int i = 0; i < in_poly.size(); ++i) { 362 | if (i == pt_A || i == pt_A_front || i == pt_B || i == pt_B_front) { 363 | continue; 364 | } 365 | 366 | int i_next = (i == in_poly.size() - 1) ? 0 : i + 1 ; 367 | cv::Point2f line02_start = _polygon_cv[in_poly[i]]; 368 | cv::Point2f line02_end = _polygon_cv[in_poly[i_next]]; 369 | cv::Point2f intersect_pt; 370 | if (getIntersectPoints(line01_start, line01_end, line02_start, line02_end, intersect_pt)) { 371 | if ((line01_end.x - intersect_pt.x) * (line01_start.x - intersect_pt.x) <= 0 && 372 | (line02_end.x - intersect_pt.x) * (line02_start.x - intersect_pt.x) < 0) { 373 | // the 2 line segments intersect 374 | return false; 375 | } 376 | } else { 377 | // the lines parallel 378 | } 379 | } 380 | return true; 381 | } 382 | 383 | // clear the output `out_concave_polygon_idx` 384 | // judge polygon in `_result_polygon_idx` 385 | bool PolygonDecomposition::isAllConvex(std::vector& out_concave_polygon_idx) { 386 | out_concave_polygon_idx.clear(); 387 | 388 | bool is_all_convex = true; 389 | // std::cout << "_result_polygon_idx.size() " << _result_polygon_idx.size() << std::endl; 390 | for (int i = 0; i < _result_polygon_idx.size(); ++i) { 391 | // take out the polygon 392 | std::vector tmp_polygon_cv; 393 | getCvPolygonFromIdx(_result_polygon_idx[i], tmp_polygon_cv); 394 | 395 | // if the polygon is concave 396 | if (!cv::isContourConvex(tmp_polygon_cv)) { 397 | out_concave_polygon_idx.push_back(i); 398 | is_all_convex = false; 399 | } 400 | } 401 | // debug 402 | if (out_concave_polygon_idx.size() == 0) { 403 | std::cout << " there is 0 concave polygons " << std::endl; 404 | return true; 405 | } 406 | 407 | return is_all_convex; 408 | } 409 | 410 | // this algorithm is proposed by 411 | // https://stackoverrun.com/cn/q/1900650 412 | // an efficient way to compute the intersection of 2 lines 413 | bool PolygonDecomposition::getIntersectPoints(cv::Point2f line01_start, cv::Point2f line01_end, 414 | cv::Point2f line02_start, cv::Point2f line02_end, 415 | cv::Point2f& intersect_pt) { 416 | cv::Point2f x = line02_start - line01_start; 417 | cv::Point2f d1 = line01_end - line01_start; 418 | cv::Point2f d2 = line02_end - line02_start; 419 | 420 | float cross = d1.x * d2.y - d1.y * d2.x; 421 | if (fabs(cross) < 1e-8) return false; 422 | double t1 = (x.x * d2.y - x.y * d2.x) / cross; 423 | intersect_pt = line01_start + d1 * t1; 424 | return true; 425 | } 426 | 427 | // the full mark of power is 200 428 | // if the visible point is a concave, power adds 100, the other 100 is 100 times the absolute of 2 lines' cosine 429 | // that guarantees if there is concave point, the power of it will be the highest 430 | bool PolygonDecomposition::calPowerOfVisiblePoints(const std::vector& in_poly, 431 | int in_concave_idx, 432 | const std::vector& in_pts_idx, 433 | std::vector& out_power_vector) { 434 | out_power_vector.clear(); 435 | 436 | // get concave point bisector 437 | int center_idx = in_concave_idx; 438 | int left_idx = (center_idx == 0) ? in_poly.size() -1 : center_idx - 1 ; 439 | int right_idx = (center_idx == in_poly.size() - 1) ? 0 : center_idx + 1; 440 | double left_x = _polygon_cv[in_poly[left_idx]].x - _polygon_cv[in_poly[center_idx]].x; 441 | double left_y = _polygon_cv[in_poly[left_idx]].y - _polygon_cv[in_poly[center_idx]].y; 442 | double right_x = _polygon_cv[in_poly[right_idx]].x - _polygon_cv[in_poly[center_idx]].x; 443 | double right_y = _polygon_cv[in_poly[right_idx]].y - _polygon_cv[in_poly[center_idx]].y; 444 | Eigen::Vector2d left_vec; 445 | left_vec << left_x, left_y; 446 | Eigen::Vector2d right_vec; 447 | right_vec << right_x, right_y; 448 | left_vec.normalize(); 449 | right_vec.normalize(); 450 | Eigen::Vector2d bisector_vec = left_vec + right_vec; 451 | bisector_vec.normalize(); 452 | 453 | for (int i = 0; i < in_pts_idx.size(); ++i) { 454 | double p_value = 0; 455 | // judge `in_poly[in_pts_idx[i]]` is a concave 456 | int c_idx = in_pts_idx[i]; 457 | int l_idx = (c_idx == 0) ? in_poly.size() - 1 : c_idx - 1; 458 | int r_idx = (c_idx == in_poly.size() - 1) ? 0 : c_idx + 1; 459 | cv::Point2f l2c_vec = _polygon_cv[in_poly[c_idx]] - _polygon_cv[in_poly[l_idx]]; 460 | cv::Point2f c2r_vec = _polygon_cv[in_poly[r_idx]] - _polygon_cv[in_poly[c_idx]]; 461 | double cross = l2c_vec.x * c2r_vec.y - l2c_vec.y * c2r_vec.x; 462 | if (cross < 0) { // this point is concave, add the power with 100; 463 | p_value += 100; 464 | } 465 | double del_x = _polygon_cv[in_poly[c_idx]].x - _polygon_cv[in_poly[center_idx]].x; 466 | double del_y = _polygon_cv[in_poly[c_idx]].y - _polygon_cv[in_poly[center_idx]].y; 467 | Eigen::Vector2d line_vec; 468 | line_vec << del_x, del_y; 469 | line_vec.normalize(); 470 | p_value += fabs(bisector_vec.dot(line_vec)) * 100; 471 | out_power_vector.push_back(p_value); 472 | } 473 | return true; 474 | } 475 | 476 | // ATTENTION: `out_polys` is index to origin_polygon `_polygon_cv` stored in `_result_polygons_idx` 477 | // Since the polygon is arranged in counterclockwise 478 | // when you get the 2 split points one of which is the first concave point 479 | // it's easy to split the concave polygons into 2 polygons 480 | bool PolygonDecomposition::getSplitPolygons(const std::vector& in_poly, int in_concave_pt_idx, int in_split_pt_idx, 481 | std::vector >& out_polys) { 482 | out_polys.clear(); 483 | 484 | if (in_concave_pt_idx == in_split_pt_idx) { 485 | std::cout << "concave idx equals split idx" << std::endl; 486 | std::cout << "getSplitPolygons() failed" << std::endl; 487 | return false; 488 | } 489 | 490 | std::vector region_01, region_02; 491 | int start_idx = (in_concave_pt_idx < in_split_pt_idx) ? in_concave_pt_idx : in_split_pt_idx; 492 | int end_idx = (in_concave_pt_idx > in_split_pt_idx) ? in_concave_pt_idx : in_split_pt_idx; 493 | // the `in_poly` is already counterclockwise 494 | // region_01 495 | for (int i = start_idx; i <= end_idx; ++i) { 496 | region_01.push_back(in_poly[i]); 497 | } 498 | // region_02 499 | for (int i = end_idx; i < in_poly.size(); ++i) { 500 | region_02.push_back(in_poly[i]); 501 | } 502 | for (int i = 0; i <= start_idx; ++i) { 503 | region_02.push_back(in_poly[i]); 504 | } 505 | // check the num, use when debug 506 | if ((in_poly.size() + 2) != (region_01.size() + region_02.size())) { 507 | std::cout << "(in_poly.size() + 2) != (region_01.size() + region_02.size())" << std::endl; 508 | return false; 509 | } 510 | 511 | out_polys.push_back(region_01); 512 | out_polys.push_back(region_02); 513 | 514 | return true; 515 | } -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "bow_shaped_planner.h" 4 | #include "decomposition.h" 5 | 6 | void createSweepingArea(std::vector& sweeping_area_cv, std::vector& sweeping_area); 7 | void drawSweepAreanAndPath(const std::vector& sweeping_area_cv, const RjpTrajectory& traject_ros); 8 | void drawSweepAreanAndPath(const std::vector& sweeping_area_cv, const std::vector& traject_ros); 9 | 10 | 11 | int main(int argc, char* argv[]) { 12 | // for visualization 13 | std::vector sweeping_area_cv; 14 | // for calculation 15 | std::vector sweeping_area; 16 | std::vector traject; 17 | ros_msgs::Odometry odom; 18 | // initial the sweeping_area 19 | createSweepingArea(sweeping_area_cv, sweeping_area); 20 | 21 | BowShapedPlanner* bsp = new BowShapedPlanner(); 22 | if (!bsp->coveragePlan(odom, sweeping_area, traject)) { 23 | std::cout << "converge plan failed" << std::endl; 24 | return 0; 25 | } 26 | std::cout << "start to draw the final result" << std::endl; 27 | 28 | drawSweepAreanAndPath(sweeping_area_cv, traject); 29 | 30 | if (bsp) { 31 | delete bsp; 32 | bsp = NULL; 33 | } 34 | 35 | return 0; 36 | } 37 | 38 | 39 | void createSweepingArea(std::vector& sweeping_area_cv, std::vector& sweeping_area) { 40 | 41 | // // test 01 42 | // sweeping_area_cv.push_back(cv::Point2f(100, 300)); 43 | // sweeping_area_cv.push_back(cv::Point2f(150, 200)); 44 | // sweeping_area_cv.push_back(cv::Point2f(100, 100)); 45 | // sweeping_area_cv.push_back(cv::Point2f(400, 100)); 46 | // sweeping_area_cv.push_back(cv::Point2f(350, 200)); 47 | // sweeping_area_cv.push_back(cv::Point2f(400, 300)); 48 | 49 | 50 | 51 | // // test 02 52 | // sweeping_area_cv.push_back(cv::Point2f(100, 100)); 53 | // sweeping_area_cv.push_back(cv::Point2f(100, 300)); 54 | // sweeping_area_cv.push_back(cv::Point2f(200, 300)); 55 | // sweeping_area_cv.push_back(cv::Point2f(200, 500)); 56 | // sweeping_area_cv.push_back(cv::Point2f(500, 500)); 57 | // sweeping_area_cv.push_back(cv::Point2f(500, 100)); 58 | 59 | 60 | // test 03 61 | sweeping_area_cv.push_back(cv::Point2f(100, 400)); 62 | sweeping_area_cv.push_back(cv::Point2f(200, 300)); 63 | sweeping_area_cv.push_back(cv::Point2f(100, 100)); 64 | sweeping_area_cv.push_back(cv::Point2f(400, 100)); 65 | sweeping_area_cv.push_back(cv::Point2f(400, 200)); 66 | sweeping_area_cv.push_back(cv::Point2f(500, 200)); 67 | sweeping_area_cv.push_back(cv::Point2f(300, 300)); 68 | sweeping_area_cv.push_back(cv::Point2f(500, 400)); 69 | 70 | // // test 04 71 | // sweeping_area_cv.push_back(cv::Point2f(100, 700)); 72 | // sweeping_area_cv.push_back(cv::Point2f(100, 100)); 73 | // sweeping_area_cv.push_back(cv::Point2f(700, 100)); 74 | // sweeping_area_cv.push_back(cv::Point2f(700, 200)); 75 | // sweeping_area_cv.push_back(cv::Point2f(200, 200)); 76 | // sweeping_area_cv.push_back(cv::Point2f(200, 700)); 77 | 78 | // test 05 79 | // sweeping_area_cv.push_back(cv::Point2f(150, 200)); 80 | // sweeping_area_cv.push_back(cv::Point2f(100, 100)); 81 | // sweeping_area_cv.push_back(cv::Point2f(400, 100)); 82 | // sweeping_area_cv.push_back(cv::Point2f(350, 200)); 83 | for (int i = 0; i < sweeping_area_cv.size(); ++i) { 84 | RjpPoint pt; 85 | pt.x = sweeping_area_cv[i].x; 86 | pt.y = sweeping_area_cv[i].y; 87 | sweeping_area.push_back(pt); 88 | } 89 | 90 | cv::Mat my_panel(1000, 1000, CV_8UC3, cv::Scalar(40, 0, 0)); 91 | // cv::polylines(my_panel, sweeping_area_cv, true, cv::Scalar(45, 90, 135), 4); 92 | for (int i = 0; i < sweeping_area_cv.size() - 1; ++i) { 93 | cv::circle(my_panel, sweeping_area_cv[i], 3, cv::Scalar(200, 0 ,0), CV_FILLED); 94 | cv::line(my_panel, sweeping_area_cv[i], sweeping_area_cv[i + 1], cv::Scalar(0, 200, 0), 1); 95 | } 96 | cv::circle(my_panel, sweeping_area_cv[sweeping_area_cv.size() - 1], 3, cv::Scalar(200, 0 ,0), CV_FILLED); 97 | cv::line(my_panel, sweeping_area_cv[sweeping_area_cv.size() - 1], sweeping_area_cv[0], cv::Scalar(0, 200, 0), 1); 98 | cv::imshow("sweeping area", my_panel); 99 | cv::waitKey(0); 100 | } 101 | 102 | void drawSweepAreanAndPath(const std::vector& sweeping_area_cv, 103 | const RjpTrajectory& traject_ros) { 104 | // transform the tracject_ros to traject_cv 105 | std::vector traject_cv; 106 | cv::Point2f tmp_pt; 107 | for (int i = 0; i < traject_ros.pts.size(); ++i) { 108 | tmp_pt.x = traject_ros.pts[i].x; 109 | tmp_pt.y = traject_ros.pts[i].y; 110 | traject_cv.push_back(tmp_pt); 111 | } 112 | std::cout << "traject_cv size " << traject_cv.size() << std::endl; 113 | // std::vector > ffff; 114 | // ffff.push_back(traject_cv); 115 | // std::cout << traject_cv[traject_cv.size() - 1].x << " " << traject_cv[traject_cv.size() - 1].y << std::endl; 116 | cv::Mat my_panel(1000, 1000, CV_8UC3, cv::Scalar(40, 0, 0)); 117 | 118 | // draw sweeping area 119 | for (int i = 0; i < sweeping_area_cv.size() - 1; ++i) { 120 | // cv::circle(my_panel, sweeping_area_cv[i], 3, cv::Scalar(200, 0 ,0), CV_FILLED); 121 | cv::line(my_panel, sweeping_area_cv[i], sweeping_area_cv[i + 1], cv::Scalar(0, 200, 0), 2); 122 | } 123 | // cv::circle(my_panel, sweeping_area_cv[sweeping_area_cv.size() - 1], 3, cv::Scalar(200, 0 ,0), CV_FILLED); 124 | cv::line(my_panel, sweeping_area_cv[sweeping_area_cv.size() - 1], sweeping_area_cv[0], cv::Scalar(0, 200, 0), 2); 125 | // cv::polylines(my_panel, sweeping_area_cv, false, cv::Scalar(45, 90, 135), 4, 0, 0); 126 | 127 | // draw trajectory 128 | for (int i = 0; i < traject_cv.size() - 1; ++i) { 129 | cv::circle(my_panel, traject_cv[i], 3, cv::Scalar(0, 0, 255), CV_FILLED); 130 | cv::line(my_panel, traject_cv[i], traject_cv[i + 1], cv::Scalar(255, 255, 0), 1); 131 | } 132 | cv::circle(my_panel, traject_cv[traject_cv.size() - 1], 3, cv::Scalar(0, 0, 255), CV_FILLED); 133 | // cv::line(my_panel, traject_cv[traject_cv.size() - 1], traject_cv[0], cv::Scalar(255, 255, 0), 1); 134 | // cv::polylines(my_panel, traject_cv, false, cv::Scalar(135, 180, 215), 2, 0, 0); 135 | 136 | cv::imshow("sweeping area and trajectory", my_panel); 137 | cv::waitKey(0); 138 | } 139 | 140 | void drawSweepAreanAndPath(const std::vector& sweeping_area_cv, 141 | const std::vector& traject_ros) { 142 | cv::Mat my_panel(1000, 1000, CV_8UC3, cv::Scalar(40, 0, 0)); 143 | 144 | // draw sweeping area 145 | for (int i = 0; i < sweeping_area_cv.size() - 1; ++i) { 146 | // cv::circle(my_panel, sweeping_area_cv[i], 3, cv::Scalar(200, 0 ,0), CV_FILLED); 147 | cv::line(my_panel, sweeping_area_cv[i], sweeping_area_cv[i + 1], cv::Scalar(0, 200, 0), 2); 148 | } 149 | // cv::circle(my_panel, sweeping_area_cv[sweeping_area_cv.size() - 1], 3, cv::Scalar(200, 0 ,0), CV_FILLED); 150 | cv::line(my_panel, sweeping_area_cv[sweeping_area_cv.size() - 1], sweeping_area_cv[0], cv::Scalar(0, 200, 0), 2); 151 | 152 | // draw each trajectory 153 | for (int i = 0; i < traject_ros.size(); ++i) { 154 | // if (i == 0) continue; 155 | for (int j = 0; j < traject_ros[i].pts.size() - 1; ++j) { 156 | double A_x = traject_ros[i].pts[j].x; 157 | double A_y = traject_ros[i].pts[j].y; 158 | cv::Point2f A_pt(A_x, A_y); 159 | double B_x = traject_ros[i].pts[j + 1].x; 160 | double B_y = traject_ros[i].pts[j + 1].y; 161 | cv::Point2f B_pt(B_x, B_y); 162 | 163 | cv::circle(my_panel, A_pt, 3, cv::Scalar(200, 0 ,0), CV_FILLED); 164 | cv::line(my_panel, A_pt, B_pt, cv::Scalar(255, 255, 0), 1); 165 | } 166 | double x = traject_ros[i].pts[traject_ros[i].pts.size() - 1].x; 167 | double y = traject_ros[i].pts[traject_ros[i].pts.size() - 1].y; 168 | cv::Point2f tmp_pt(x, y); 169 | cv::circle(my_panel, tmp_pt, 3, cv::Scalar(200, 0 ,0), CV_FILLED); 170 | } 171 | 172 | cv::imshow("sweeping area and trajectory", my_panel); 173 | cv::waitKey(0); 174 | } --------------------------------------------------------------------------------