├── CMakeLists.txt ├── LICENSE ├── README.md ├── package.xml └── src ├── convexhull.cpp ├── convexhull.h ├── demo.cpp ├── plain_demo.cpp └── utility.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(convexhull_demo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | sensor_msgs 13 | geometry_msgs 14 | visualization_msgs 15 | ) 16 | 17 | 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # sensor_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | LIBRARIES exploration 110 | CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs visualization_msgs 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/lidar_process.cpp 127 | # ) 128 | 129 | ## Depedent cpp files 130 | set(SOURCES ${SOURCES} 131 | src/convexhull.cpp 132 | ) 133 | 134 | ## Add cmake target dependencies of the library 135 | ## as an example, code may need to be generated before libraries 136 | ## either from message generation or dynamic reconfigure 137 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Declare a C++ executable 140 | ## With catkin_make all packages are built within a single CMake context 141 | ## The recommended prefix ensures that target names across packages don't collide 142 | # add_executable(${PROJECT_NAME}_node src/lidar_process_node.cpp) 143 | add_executable(convex_node src/demo.cpp ${SOURCES}) 144 | target_link_libraries(convex_node ${catkin_LIBRARIES}) 145 | 146 | 147 | ## Rename C++ executable without prefix 148 | ## The above recommended prefix causes long target names, the following renames the 149 | ## target back to the shorter version for ease of user use 150 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 151 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Specify libraries to link a library or executable target against 158 | # target_link_libraries(${PROJECT_NAME}_node 159 | # ${catkin_LIBRARIES} 160 | # ) 161 | 162 | ############# 163 | ## Install ## 164 | ############# 165 | 166 | # all install targets should use catkin DESTINATION variables 167 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 168 | 169 | ## Mark executable scripts (Python etc.) for installation 170 | ## in contrast to setup.py, you can choose the destination 171 | # install(PROGRAMS 172 | # scripts/my_python_script 173 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark executables for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 178 | # install(TARGETS ${PROJECT_NAME}_node 179 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark libraries for installation 183 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 184 | # install(TARGETS ${PROJECT_NAME} 185 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 186 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 187 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 188 | # ) 189 | 190 | ## Mark cpp header files for installation 191 | # install(DIRECTORY include/${PROJECT_NAME}/ 192 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 193 | # FILES_MATCHING PATTERN "*.h" 194 | # PATTERN ".svn" EXCLUDE 195 | # ) 196 | 197 | ## Mark other files for installation (e.g. launch and bag files, etc.) 198 | # install(FILES 199 | # # myfile1 200 | # # myfile2 201 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 202 | # ) 203 | 204 | ############# 205 | ## Testing ## 206 | ############# 207 | 208 | ## Add gtest based cpp test target and link libraries 209 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lidar_process.cpp) 210 | # if(TARGET ${PROJECT_NAME}-test) 211 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 212 | # endif() 213 | 214 | ## Add folders to be run by python nosetests 215 | # catkin_add_nosetests(test) 216 | 217 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Dung-Han Lee 4 | dunghanlee@gmail.com 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Incremental 3D-Convexhull algorithm 2 | This repository contains an C++ implementation of 3D-ConvexHull algorithm from the 3 | book [Computational Geometry in C by O'Rourke](http://crtl-i.com/PDF/comp_c.pdf). RVIZ is used for visualization but is 4 | not required to use this package. This code is implemented with C++11 STL conta- 5 | iners only. The files are well documented, feel free to poke around. 6 | 7 | [![Example video](https://media.giphy.com/media/hsV1GgRby1M4kDbAgm/giphy.gif)](https://youtu.be/DDgGc7_fEyU) 8 | 9 | ## Build and Run (using RVIZ) 10 | catkin_make 11 | source devel/setup.bash 12 | roscore 13 | rosrun convexhull_demo convex_node 14 | rviz 15 | 16 | ## Build and Run (text output only) 17 | cd /src 18 | g++ -std=c++11 convexhull.cpp plain_demo.cpp -o test.o && ./test.o 19 | 20 | ## Example Usage 21 | Although in this example pcl pointcloud was used, the interface is a template 22 | function that takes any vector with points (expect to have field x, y and z) 23 | 24 | func(pcl::PointCloud::Ptr PC, Point3D& p) 25 | { 26 | Convexhull ch(PC->points); 27 | if(ch.Contains(p)) // surface point is consider outside 28 | { 29 | //do something 30 | } 31 | auto vertices = ch.GetVertices(); 32 | auto faces = ch.GetFaces(); 33 | ... 34 | } 35 | 36 | ## Who calls whom 37 | ConvexHull() 38 | --ConstructHull() 39 | -- BulildFirstHull() 40 | -- Colinear() 41 | -- Coplanar() 42 | -- AddOneFace() 43 | -- IncreHull() 44 | -- VolumeSign() 45 | -- AddOneFace() 46 | -- CleanUp() 47 | -- ExtractExteriorPoints() 48 | 49 | ## License 50 | MIT License 51 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | convexhull_demo 4 | 0.0.0 5 | package to demo 3D incremental convexhull algorithm 6 | 7 | 8 | 9 | 10 | Dung-Han Lee 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | sensor_msgs 54 | geometry_msgs 55 | visualization_msgs 56 | roscpp 57 | sensor_msgs 58 | geometry_msgs 59 | visualization_msgs 60 | roscpp 61 | sensor_msgs 62 | geometry_msgs 63 | visualization_msgs 64 | libpcl-all-dev 65 | libpcl-all 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/convexhull.cpp: -------------------------------------------------------------------------------- 1 | /*The MIT License 2 | 3 | Copyright (c) 2020 Dung-Han Lee 4 | dunghanlee@gmail.com 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. */ 23 | 24 | /* This work is an implementation of incremental convex hull algorithm from 25 | the book Computational Geometry in C by O'Rourke */ 26 | 27 | #include "convexhull.h" 28 | 29 | size_t ConvexHull::Key2Edge(const Point3D& a, const Point3D& b) const 30 | { 31 | point_hash ph; 32 | return ph(a) ^ ph(b); 33 | } 34 | 35 | int ConvexHull::VolumeSign(const Face& f, const Point3D& p) const 36 | { 37 | double vol; 38 | double ax, ay, az, bx, by, bz, cx, cy, cz; 39 | ax = f.vertices[0].x - p.x; 40 | ay = f.vertices[0].y - p.y; 41 | az = f.vertices[0].z - p.z; 42 | bx = f.vertices[1].x - p.x; 43 | by = f.vertices[1].y - p.y; 44 | bz = f.vertices[1].z - p.z; 45 | cx = f.vertices[2].x - p.x; 46 | cy = f.vertices[2].y - p.y; 47 | cz = f.vertices[2].z - p.z; 48 | vol = ax * (by * cz - bz * cy) +\ 49 | ay * (bz * cx - bx * cz) +\ 50 | az * (bx * cy - by * cx); 51 | if(vol == 0) return 0; 52 | return vol < 0 ? -1 : 1; 53 | } 54 | 55 | void ConvexHull::AddOneFace(const Point3D& a, const Point3D& b, 56 | const Point3D& c, const Point3D& inner_pt) 57 | { 58 | // Make sure face is CCW with face normal pointing outward 59 | this->faces.emplace_back(a, b, c); 60 | auto& new_face = this->faces.back(); 61 | if(this->VolumeSign(this->faces.back(), inner_pt) < 0) new_face.Reverse(); 62 | 63 | // Create edges and link them to face pointer 64 | auto create_edge = [&](const Point3D& p1, const Point3D& p2) 65 | { 66 | size_t key = this->Key2Edge(p1, p2); 67 | if(!this->map_edges.count(key)) 68 | { 69 | this->edges.emplace_back(p1, p2); 70 | this->map_edges.insert({key, &this->edges.back()}); 71 | } 72 | this->map_edges[key]->LinkAdjFace(&new_face); 73 | }; 74 | create_edge(a, b); 75 | create_edge(a, c); 76 | create_edge(b, c); 77 | } 78 | 79 | bool ConvexHull::Colinear(const Point3D& a, const Point3D& b, const Point3D& c) const 80 | { 81 | return ((c.z - a.z) * (b.y - a.y) - 82 | (b.z - a.z) * (c.y - a.y)) == 0 &&\ 83 | ((b.z - a.z) * (c.x - a.x) - 84 | (b.x - a.x) * (c.z - a.z)) == 0 &&\ 85 | ((b.x - a.x) * (c.y - a.y) - 86 | (b.y - a.y) * (c.x - a.x)) == 0; 87 | } 88 | 89 | bool ConvexHull::BuildFirstHull(std::vector& pointcloud) 90 | { 91 | const int n = pointcloud.size(); 92 | if(n <= 3) 93 | { 94 | std::cout<<"Tetrahedron: points.size() < 4\n"; 95 | return false; 96 | } 97 | 98 | int i = 2; 99 | while(this->Colinear(pointcloud[i], pointcloud[i-1], pointcloud[i-2])) 100 | { 101 | if(i++ == n - 1) 102 | { 103 | std::cout<<"Tetrahedron: All points are colinear!\n"; 104 | return false; 105 | } 106 | } 107 | 108 | Face face(pointcloud[i], pointcloud[i-1], pointcloud[i-2]); 109 | 110 | int j = i; 111 | while(!this->VolumeSign(face, pointcloud[j])) 112 | { 113 | if(j++ == n-1) 114 | { 115 | std::cout<<"Tetrahedron: All pointcloud are coplanar!\n"; 116 | return false; 117 | } 118 | } 119 | 120 | auto& p1 = pointcloud[i]; auto& p2 = pointcloud[i-1]; 121 | auto& p3 = pointcloud[i-2]; auto& p4 = pointcloud[j]; 122 | p1.processed = p2.processed = p3.processed = p4.processed = true; 123 | this->AddOneFace(p1, p2, p3, p4); 124 | this->AddOneFace(p1, p2, p4, p3); 125 | this->AddOneFace(p1, p3, p4, p2); 126 | this->AddOneFace(p2, p3, p4, p1); 127 | return true; 128 | 129 | } 130 | 131 | Point3D ConvexHull::FindInnerPoint(const Face* f, const Edge& e) 132 | { 133 | for(int i = 0; i < 3; i++) 134 | { 135 | if(f->vertices[i] == e.endpoints[0]) continue; 136 | if(f->vertices[i] == e.endpoints[1]) continue; 137 | return f->vertices[i]; 138 | } 139 | } 140 | 141 | void ConvexHull::IncreHull(const Point3D& pt) 142 | { 143 | // Find the illuminated faces (which will be removed later) 144 | bool vis = false; 145 | for(auto& face : this->faces) 146 | { 147 | if(VolumeSign(face, pt) < 0) 148 | { 149 | //std::cout<<"face illuminated by pt "<edges.begin(); it != this->edges.end(); it++) 157 | { 158 | auto& edge = *it; 159 | auto& face1 = edge.adjface1; 160 | auto& face2 = edge.adjface2; 161 | 162 | // Newly added edge 163 | if(face1 == NULL || face2 == NULL) 164 | { 165 | continue; 166 | } 167 | 168 | // This edge is to be removed because two adjacent faces will be removed 169 | else if(face1->visible && face2->visible) 170 | { 171 | edge.remove = true; 172 | } 173 | 174 | // Edge on the boundary of visibility, which will be used to extend a tagent 175 | // cone surface. 176 | else if(face1->visible|| face2->visible) 177 | { 178 | if(face1->visible) std::swap(face1, face2); 179 | auto inner_pt = this->FindInnerPoint(face2, edge); 180 | edge.Erase(face2); 181 | this->AddOneFace(edge.endpoints[0], edge.endpoints[1], pt, inner_pt); 182 | } 183 | } 184 | } 185 | 186 | void ConvexHull::ConstructHull(std::vector& pointcloud) 187 | { 188 | if(!this->BuildFirstHull(pointcloud)) return; 189 | for(const auto& pt : pointcloud) 190 | { 191 | if(pt.processed) continue; 192 | this->IncreHull(pt); 193 | this->CleanUp(); 194 | } 195 | this->ExtractExteriorPoints(); 196 | } 197 | 198 | void ConvexHull::CleanUp() 199 | { 200 | auto it_edge = this->edges.begin(); 201 | while(it_edge != this->edges.end()) 202 | { 203 | if(it_edge->remove) 204 | { 205 | auto pt1 = it_edge->endpoints[0]; 206 | auto pt2 = it_edge->endpoints[1]; 207 | auto key_to_evict = this->Key2Edge(pt1, pt2); 208 | this->map_edges.erase(key_to_evict); 209 | this->edges.erase(it_edge++); 210 | } 211 | else it_edge++; 212 | }; 213 | auto it_face = this->faces.begin(); 214 | while(it_face != this->faces.end()) 215 | { 216 | if(it_face->visible) this->faces.erase(it_face++); 217 | else it_face++; 218 | } 219 | } 220 | 221 | void ConvexHull::ExtractExteriorPoints() 222 | { 223 | std::unordered_set exterior_set; 224 | for(const auto& f : this->faces) 225 | { 226 | for(int i =0; i < 3; i++) 227 | exterior_set.insert(f.vertices[i]); 228 | } 229 | this->exterior_points = \ 230 | std::vector(exterior_set.begin(), exterior_set.end()); 231 | } 232 | 233 | void ConvexHull::Print(const std::string mode = "none") 234 | { 235 | if(mode == "vertice") 236 | { 237 | for(const auto& pt : this->exterior_points) std::cout<edges) std::cout<<(e)<<"\n"; 242 | } 243 | else if( mode == "face") 244 | { 245 | for(const auto& f : this->faces) std::cout< 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "utility.h" 37 | 38 | 39 | // Defined in CCW 40 | struct Face 41 | { 42 | Face(const Point3D& p1, const Point3D& p2, const Point3D& p3): visible(false) 43 | { vertices[0] = p1; vertices[1] = p2; vertices[2] = p3;}; 44 | 45 | void Reverse(){std::swap(vertices[0], vertices[2]); }; 46 | 47 | friend std::ostream& operator<<(std::ostream& os, const Face& f) 48 | { 49 | os << "[face pt1 = " << f.vertices[0].ToString() 50 | << " | face pt2 = " << f.vertices[1].ToString() 51 | << " | face pt3 = " << f.vertices[2].ToString()<<"] "; 52 | return os; 53 | } 54 | 55 | bool visible; 56 | Point3D vertices[3]; 57 | }; 58 | 59 | struct Edge 60 | { 61 | Edge(const Point3D& p1, const Point3D& p2): 62 | adjface1(nullptr), adjface2(nullptr), remove(false) 63 | { endpoints[0] = p1; endpoints[1] = p2; }; 64 | 65 | void LinkAdjFace(Face* face) 66 | { 67 | if( adjface1 != NULL && adjface2 != NULL ) 68 | { 69 | std::cout<<"warning: property violated!\n"; 70 | } 71 | (adjface1 == NULL ? adjface1 : adjface2)= face; 72 | }; 73 | 74 | void Erase(Face* face) 75 | { 76 | if(adjface1 != face && adjface2 != face) return; 77 | (adjface1 == face ? adjface1 : adjface2) = nullptr; 78 | }; 79 | 80 | friend std::ostream& operator<<(std::ostream& os, const Edge& e) 81 | { 82 | os << "[edge pt1 = " << e.endpoints[0].ToString() 83 | << " | edge pt2 = " << e.endpoints[1].ToString() << " ]"; 84 | return os; 85 | } 86 | 87 | bool remove; 88 | Face *adjface1, *adjface2; 89 | Point3D endpoints[2]; 90 | }; 91 | 92 | class ConvexHull 93 | { 94 | public: 95 | template ConvexHull(const std::vector& points); 96 | // All major works are conducted upon calling of constructor 97 | 98 | ~ConvexHull() = default; 99 | 100 | template bool Contains(T p) const; 101 | // In out test for a query point (surface point is considered outside) 102 | 103 | const std::list& GetFaces() const {return this->faces;}; 104 | 105 | const std::vector GetVertices() const \ 106 | {return this->exterior_points;}; 107 | // Return exterior vertices than defines the convell hull 108 | 109 | void Print(const std::string mode); 110 | // mode {face, edge, vertice} 111 | 112 | int Size() const {return this->exterior_points.size();}; 113 | 114 | private: 115 | 116 | bool Colinear(const Point3D& a, const Point3D& b, const Point3D& c) const; 117 | 118 | bool CoPlanar(Face& f, Point3D& p); 119 | 120 | int VolumeSign(const Face& f, const Point3D& p) const; 121 | // A point is considered outside of a CCW face if the volume of tetrahedron 122 | // formed by the face and point is negative. Note that origin is set at p. 123 | 124 | size_t Key2Edge(const Point3D& a, const Point3D& b) const; 125 | // Hash key for edge. hash(a, b) = hash(b, a) 126 | 127 | void AddOneFace(const Point3D& a, const Point3D& b, 128 | const Point3D& c, const Point3D& inner_pt); 129 | // Inner point is used to make the orientation of face consistent in counter- 130 | // clockwise direction 131 | 132 | bool BuildFirstHull(std::vector& pointcloud); 133 | // Build a tetrahedron as first convex hull 134 | 135 | void IncreHull(const Point3D& p); 136 | 137 | void ConstructHull(std::vector& pointcloud); 138 | 139 | void CleanUp(); 140 | 141 | void ExtractExteriorPoints(); 142 | 143 | Point3D FindInnerPoint(const Face* f, const Edge& e); 144 | // for face(a,b,c) and edge(a,c), return b 145 | 146 | std::vector pointcloud = {}; 147 | std::vector exterior_points = {}; 148 | std::list faces = {}; 149 | std::list edges = {}; 150 | std::unordered_map map_edges; 151 | }; 152 | 153 | template ConvexHull::ConvexHull(const std::vector& points) 154 | { 155 | const int n = points.size(); 156 | this->pointcloud.resize(n); 157 | for(int i = 0; i < n; i++) 158 | { 159 | this->pointcloud[i].x = points[i].x; 160 | this->pointcloud[i].y = points[i].y; 161 | this->pointcloud[i].z = points[i].z; 162 | } 163 | this->ConstructHull(this->pointcloud); 164 | } 165 | 166 | template bool ConvexHull::Contains(T p) const 167 | { 168 | Point3D pt(p.x, p.y, p.z); 169 | for(auto& face : this->faces) 170 | { 171 | if(VolumeSign(face, pt) <= 0) return false; 172 | } 173 | return true; 174 | } 175 | #endif -------------------------------------------------------------------------------- /src/demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "convexhull.h" 5 | 6 | class Master{ 7 | public: 8 | Master(ros::NodeHandle& nh); 9 | 10 | ~Master() = default; 11 | 12 | void VizPolyhedron(const std::list& triangles) const; 13 | 14 | void VizPoints(const std::vector& points, 15 | const ConvexHull& hull) const; 16 | 17 | int Loop(); 18 | 19 | private: 20 | 21 | void SetMarker(visualization_msgs::Marker& scan_marker, 22 | float scale = 1.f) const; 23 | 24 | std::vector> tests; 25 | 26 | ros::Publisher convex_pub, point_pub; 27 | 28 | int cnt; 29 | 30 | bool random = 1; 31 | }; 32 | 33 | Master::Master(ros::NodeHandle& nh){ 34 | this->convex_pub = \ 35 | nh.advertise("convex_hull", 10); 36 | this->point_pub = \ 37 | nh.advertise("points", 10); 38 | this->cnt = 4; 39 | } 40 | 41 | int Master::Loop(){ 42 | while(ros::ok()) 43 | { 44 | if(this->random) 45 | { 46 | double x = (std::rand()%10 - 5) * 1.f; 47 | double y = (std::rand()%10 - 5) * 1.f; 48 | double z = (std::rand()%10) * 1.f; 49 | std::vector tmp = {x, y, z}; 50 | this->tests.push_back(tmp); 51 | } 52 | else 53 | { 54 | this->tests = { 55 | {1,2,1}, {2,1,1}, {1,1,1}, {2,2,1}, {1,2,2}, {2,1,2}, {1,1,2}, {2,2,2}, 56 | {0,3,0}, {3,0,0}, {0,0,0}, {3,3,0}, {0,3,3}, {3,0,3}, {0,0,3}, {3,3,3}}; 57 | } 58 | std::vector vecs; 59 | int n = this->cnt >= tests.size() ? tests.size() : this->cnt; 60 | for(int i =0 ; i < n; i++) 61 | vecs.emplace_back(tests[i][0], tests[i][1], tests[i][2]); 62 | ConvexHull c(vecs); 63 | this->VizPolyhedron(c.GetFaces()); 64 | if(c.Size() >= 4) this->VizPoints(vecs, c); 65 | this->cnt++; 66 | ros::Rate loop_rate(10); 67 | ros::spinOnce(); 68 | loop_rate.sleep(); 69 | } 70 | } 71 | 72 | void Master::SetMarker(\ 73 | visualization_msgs::Marker& scan_marker, float scale) const 74 | { 75 | scan_marker.header.frame_id = "map"; 76 | scan_marker.header.stamp = ros::Time::now(); 77 | scan_marker.ns = "scan"; 78 | scan_marker.action = visualization_msgs::Marker::ADD; 79 | scan_marker.scale.x = scan_marker.scale.y = scan_marker.scale.z = scale; 80 | scan_marker.pose.orientation.x = 0.0; 81 | scan_marker.pose.orientation.y = 0.0; 82 | scan_marker.pose.orientation.z = 0.0; 83 | scan_marker.pose.orientation.w = 1.0; 84 | scan_marker.pose.position.x = 0.0; 85 | scan_marker.pose.position.y = 0.0; 86 | scan_marker.pose.position.z = 0.0; 87 | std_msgs::ColorRGBA scan_color; 88 | scan_color.a = 0.5f; 89 | scan_color.r = 0.f; 90 | scan_color.g = 0.9f; 91 | scan_color.b = 0.9f; 92 | scan_marker.color = scan_color; 93 | } 94 | 95 | void Master::VizPolyhedron(const std::list& triangles) const 96 | { 97 | visualization_msgs::Marker scan_marker; 98 | this->SetMarker(scan_marker); 99 | scan_marker.id = 0; 100 | scan_marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 101 | 102 | geometry_msgs::Point temp; 103 | for (const auto& triangle : triangles) 104 | { 105 | for(int i = 0; i < 3; i++) 106 | { 107 | temp.x = triangle.vertices[i].x; 108 | temp.y = triangle.vertices[i].y; 109 | temp.z = triangle.vertices[i].z; 110 | scan_marker.points.push_back(temp); 111 | } 112 | } 113 | this->convex_pub.publish(scan_marker); 114 | } 115 | 116 | void Master::VizPoints(const std::vector& points, 117 | const ConvexHull& hull) const 118 | { 119 | visualization_msgs::Marker scan_marker; 120 | this->SetMarker(scan_marker, 0.1); 121 | scan_marker.id = 1; 122 | scan_marker.type = visualization_msgs::Marker::POINTS; 123 | 124 | geometry_msgs::Point temp; 125 | for (const auto& pt : points) 126 | { 127 | temp.x = pt.x; temp.y = pt.y; temp.z = pt.z; 128 | scan_marker.points.push_back(temp); 129 | 130 | bool inside = hull.Contains(pt); 131 | std_msgs::ColorRGBA c; 132 | c.a = 1.0f; 133 | c.r = inside ? 0.0f : 0.9f; 134 | c.b = 0.f; 135 | c.g = inside ? 0.9f : 0.f; 136 | scan_marker.colors.push_back(c); 137 | } 138 | this->point_pub.publish(scan_marker); 139 | 140 | } 141 | 142 | int main(int argc, char** argv){ 143 | ros::init(argc, argv, "convex_node"); 144 | ros::NodeHandle nh; 145 | Master master(nh); 146 | master.Loop(); 147 | } -------------------------------------------------------------------------------- /src/plain_demo.cpp: -------------------------------------------------------------------------------- 1 | #include "convexhull.h" 2 | #include 3 | 4 | int main() 5 | { 6 | std::vector tests = \ 7 | {{3,0,0}, {0,3,0}, {0,0,3}, {3,3,3}}; 8 | ConvexHull C(tests); 9 | std::cout<<"vertices (x, y, z, intensity)\n"; C.Print("vertice"); 10 | std::cout<<"faces\n"; C.Print("face"); 11 | 12 | return 0; 13 | } -------------------------------------------------------------------------------- /src/utility.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | struct Point3D { 4 | float x; 5 | float y; 6 | float z; 7 | float intensity; 8 | bool processed; 9 | Point3D() = default; 10 | Point3D(float _x, float _y, float _z):\ 11 | x(_x), y(_y), z(_z), intensity(0), processed(false) {} 12 | Point3D(float _x, float _y, float _z, float _i):\ 13 | x(_x), y(_y), z(_z), intensity(_i), processed(false) {} 14 | 15 | bool operator ==(const Point3D& pt) const 16 | { 17 | return x == pt.x && y == pt.y && z == pt.z; 18 | } 19 | 20 | float operator *(const Point3D& pt) const 21 | { 22 | return x * pt.x + y * pt.y + z * pt.z; 23 | } 24 | 25 | Point3D operator *(const float factor) const 26 | { 27 | return Point3D(x*factor, y*factor, z*factor); 28 | } 29 | 30 | Point3D operator /(const float factor) const 31 | { 32 | return Point3D(x/factor, y/factor, z/factor); 33 | } 34 | 35 | Point3D operator +(const Point3D& pt) const 36 | { 37 | return Point3D(x+pt.x, y+pt.y, z+pt.z); 38 | } 39 | Point3D operator -(const Point3D& pt) const 40 | { 41 | return Point3D(x-pt.x, y-pt.y, z-pt.z); 42 | } 43 | 44 | std::string ToString() const 45 | { 46 | return std::to_string(x).substr(0,4) + ", " + 47 | std::to_string(y).substr(0,4) + ", " + 48 | std::to_string(z).substr(0,4) + ", " + 49 | std::to_string(intensity).substr(0,4); 50 | }; 51 | 52 | friend std::ostream& operator<<(std::ostream& os, const Point3D& p) 53 | { 54 | os << "["<< p.ToString() << "] "; 55 | return os; 56 | } 57 | 58 | }; 59 | 60 | typedef std::vector PointStack; 61 | 62 | struct point_hash 63 | { 64 | std::size_t operator() (const Point3D& p) const 65 | { 66 | std::string sx, sy, sz; 67 | sx = std::to_string(p.x); 68 | sy = std::to_string(p.y); 69 | sz = std::to_string(p.z); 70 | return std::hash{}(sx+sy+sz); 71 | } 72 | }; 73 | --------------------------------------------------------------------------------