├── .gitignore ├── CMake └── cotire.cmake ├── CMakeLists.txt ├── README.md ├── build_surface ├── CMakeLists.txt ├── include │ └── build_surface │ │ └── BuildSurface.h ├── package.xml └── src │ └── build_surface │ └── BuildSurface.cpp ├── collect_points ├── CMakeLists.txt ├── include │ └── collect_points │ │ └── CollectPoints.h ├── package.xml └── src │ └── collect_points │ └── CollectPoints.cpp ├── detect_surfaces ├── CMakeLists.txt ├── include │ └── detect_surfaces │ │ └── DetectSurfaces.h ├── package.xml └── src │ └── detect_surfaces │ └── DetectSurfaces.cpp ├── expand_surfaces ├── CMakeLists.txt ├── include │ └── expand_surfaces │ │ └── ExpandSurfaces.h ├── package.xml └── src │ └── expand_surfaces │ └── ExpandSurfaces.cpp ├── surface_detection ├── CMakeLists.txt ├── custom_rosconsole.conf ├── include │ └── surface_detection │ │ └── SurfaceDetection.h ├── launch │ └── surface_detection.launch ├── package.xml └── src │ ├── SurfaceDetection.cpp │ └── surface_detection_node.cpp ├── surface_msgs2 ├── CMakeLists.txt ├── msg │ ├── Surface.msg │ ├── SurfaceStamped.msg │ └── Surfaces.msg ├── package.xml └── srv │ └── SurfaceDetection.srv ├── surface_types ├── CMakeLists.txt ├── include │ └── surface_types │ │ ├── Surface.hpp │ │ ├── SurfaceStamped.hpp │ │ ├── Surfaces.hpp │ │ └── pcl_shim │ │ ├── ModelCoefficients_Serialization.hpp │ │ ├── PointIndices_Serialization.hpp │ │ ├── PolygonMesh_Serialization.hpp │ │ └── Vertices_Serialization.hpp └── package.xml └── surface_utils ├── CMakeLists.txt ├── include └── surface_utils │ ├── SurfaceVisualizationController.hpp │ ├── color_generator.hpp │ ├── conversions.hpp │ ├── eigen_helpers_conversions.hpp │ ├── geom_utils.hpp │ ├── pcl_utils.hpp │ └── smart_ptr.hpp └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | .idea 3 | *.pyc 4 | core 5 | CMakeLists.txt.user 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | 4 | add_subdirectory(surface_detection) 5 | add_subdirectory(surface_utils) 6 | #add_subdirectory(surface_msgs2) 7 | add_subdirectory(surface_types) 8 | add_subdirectory(expand_surfaces) 9 | add_subdirectory(detect_surfaces) 10 | add_subdirectory(collect_points) 11 | add_subdirectory(build_surface) 12 | 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Surface Extraction 2 | 3 | This project is an on-demand surface extraction framework designed for humanoid locomotion planning. Its key feature is the ability to incrementally process the scene in order to waste unnecessary effort processing areas of the environment for which surface information is not required. 4 | 5 | ## Setup 6 | 7 | Surface Extraction runs on Ubuntu 16.04 and ROS Kinetic (http://wiki.ros.org/kinetic/Installation/Ubuntu) 8 | 9 | 1. Install PCL version 1.8. As of this writing PCL 1.8 is not available by PPA, so it must be installed from source as described here: http://pointclouds.org/documentation/tutorials/compiling_pcl_posix.php 10 | 2. Install CGAL with `apt-get install libcgal-dev` 11 | 3. Download the `arc_utilities` package and put it in your Catkin workspace 12 | 4. Download `surface_extraction` and put it in your Catkin workspace 13 | 5. Compile with `catkin_make` 14 | 15 | ## Usage 16 | 17 | Surface Extraction is configured entirely through ROS parameters, recieves data on ROS topics, and is queried using ROS services. 18 | 19 | ### Configuration 20 | 21 | According to ROS convention, all distances are expressed in meters and all angles are expressed in radians. 22 | 23 | Parameters: 24 | - `target_frame` (string, default `"/world"`: The frame that surfaces will be computed relative to. Assumed to be static. Requesting surfaces in a different frame than `target_frame` is currently not supported. 25 | - `camera_frame` (boolean, default `"/left_camera_frame"`): The frame, or one of the frames, associated with the camera. Used to identify the direction of each plane's normal. 26 | - `discretization` (float, default `0.02`): The discretization resolution to use when discretizing input point clouds. Smaller values give greater accuracy at the cost of greater computation time. This value must be smaller than all distance thresholds. 27 | - `parallel_distance` (float, default `0.06`): The threshold distance along each plane that separates points which are considered to be part of this plane from those which are assumed to be outside the plane. 28 | - `perpendicular_distance` (float, default `0.04`): The threshold distance above and below each plane which separates points which are considered to be a part of the plane from points which are assumed to be outside the plane. 29 | - `point_inside_threshold` (float, default `0.1`): Newly-recieved points which are this close to an existing surface will not be recorded, and are assumed to be either part of the existing surface or belong to an object on the surface. 30 | - `mls_radius` (float, default `0.12`): The radius to use when computing neighbors for normals estimation. 31 | - `min_points_per_surface` (integer, default `50`): Surfaces with fewer than this number of points are assumed to be too small to be useful and are discarded. 32 | - `min_plane_width` (float, default `0.15`): If the spread of points in any direction along the surface plane is smaller than this value, the surface is assumed to be too small to be useful and is discarded. 33 | - `extrusion_distance` (float, default `0.02`): The assumed thickness of every surface. This is used when computing the surface mesh. 34 | - `optimistic` (boolean, default `true`): Controls whether the entire area of every newly detected surface is expanded (`true`) or just the area within the query volume (`false`). NOTE The implementation of pessimistic mode is currently broken. 35 | - `enable_visualization` (boolean, default `true`): If this is true the query box and the polygons, meshes, and normals of every detected surface are published on `surface_markers` and the unprocessed points are published on the topic specified by the `unprocessed_points_topic` parameter. 36 | - `initial_surface` (float, optional): Add an implicit starting surface at the robot's feet, which cannot be seen because the robot is in the way but whose existence can be inferred from the fact that the robot is standing on it. The surface is a square in the XY plane with the given side length and centered at (0, 0, 0). If this is not specified, no initial surface is added. 37 | - `wait_for_points` (integer, optional): The number of points which must be collected before the robot can expect to find surfaces. The service will not be advertised until the specified number of points is collected. Clients should use `waitForService()` to delay until the service is available. If this is not specified, the service is advertised immediately. 38 | 39 | Input: 40 | - `input_points_topic` (string, default `"/scan_cloud"`): The topic on which to recieve new points. A transform from the point clouds' frame to `target_frame` must exist. 41 | 42 | Output: 43 | - `unprocessed_points_topic` (string, default `"/pending_points"`): The topic on which to publish the collected but unprocessed points. 44 | 45 | ### Calling 46 | -------------------------------------------------------------------------------- /build_surface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(build_surface) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | set(CATKIN_PACKAGES 8 | roscpp 9 | surface_types 10 | surface_utils 11 | arc_utilities) 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | ${CATKIN_PACKAGES} 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 19 | find_package(PCL 1.8 REQUIRED) 20 | find_package(CGAL REQUIRED COMPONENTS Core) 21 | include(${CGAL_USE_FILE}) 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 run_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 run_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 | # std_msgs # Or other packages containing 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 run_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 you 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 | INCLUDE_DIRS include 110 | LIBRARIES build_surface 111 | CATKIN_DEPENDS ${CATKIN_PACKAGES} 112 | DEPENDS PCL CGAL 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | set(CMAKE_BUILD_TYPE Debug) 119 | 120 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O2 -std=c++11 -Wall -Wextra -Wno-unused-parameter -Wno-unused-local-typedefs") 121 | 122 | ## Specify additional locations of header files 123 | ## Your package locations should be listed before other locations 124 | # include_directories(include) 125 | include_directories( 126 | include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | include_directories(SYSTEM 130 | ${Boost_INCLUDE_DIRS} 131 | ${PCL_INCLUDE_DIRS} 132 | ${Eigen3_INCLUDE_DIRS} 133 | ${CGAL_INCLUDE_DIRS} 134 | ) 135 | 136 | ## Declare a C++ library 137 | add_library(build_surface 138 | src/${PROJECT_NAME}/BuildSurface.cpp 139 | ) 140 | 141 | ## Add cmake target dependencies of the library 142 | ## as an example, code may need to be generated before libraries 143 | ## either from message generation or dynamic reconfigure 144 | add_dependencies(build_surface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | target_link_libraries(build_surface 147 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES} ${CGAL_LIBS} 148 | ) 149 | ## Declare a C++ executable 150 | # add_executable(build_surface_node src/build_surface_node.cpp) 151 | 152 | ## Add cmake target dependencies of the executable 153 | ## same as for the library above 154 | # add_dependencies(build_surface_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Specify libraries to link a library or executable target against 157 | # target_link_libraries(build_surface_node 158 | # ${catkin_LIBRARIES} 159 | # ) 160 | 161 | ############# 162 | ## Install ## 163 | ############# 164 | 165 | # all install targets should use catkin DESTINATION variables 166 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 167 | 168 | ## Mark executable scripts (Python etc.) for installation 169 | ## in contrast to setup.py, you can choose the destination 170 | # install(PROGRAMS 171 | # scripts/my_python_script 172 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark executables and/or libraries for installation 176 | install(TARGETS build_surface 177 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 180 | ) 181 | 182 | ## Mark cpp header files for installation 183 | install(DIRECTORY include/${PROJECT_NAME}/ 184 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 186 | PATTERN ".svn" EXCLUDE 187 | ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_build_surface.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /build_surface/include/build_surface/BuildSurface.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/22/16. 3 | // 4 | 5 | #ifndef PROJECT_BuildSurfaces_H 6 | #define PROJECT_BuildSurfaces_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace pcl { 27 | struct PointXYZ; 28 | class PointIndices; 29 | class Vertices; 30 | } 31 | 32 | namespace surface_types { 33 | class Surface; 34 | class SurfaceMesh; 35 | } 36 | 37 | namespace random_colors { 38 | class color_generator; 39 | } 40 | 41 | //////////////////////////////////////// HERE THERE BE CGAL //////////////////////////////////////// 42 | 43 | // Along with each point, store a PCL point index (uint32_t) and a visited boolean 44 | struct CustomPointInfo { 45 | uint32_t pcl_index; 46 | uint32_t pcl_index2; 47 | bool visited; 48 | 49 | // For CGAL 50 | CustomPointInfo() : pcl_index(std::numeric_limits::max()), pcl_index2(pcl_index), visited(false) {} 51 | // For me 52 | CustomPointInfo(uint32_t i) : pcl_index(i), pcl_index2(std::numeric_limits::max()), visited(false) {} 53 | // For completeness 54 | CustomPointInfo(uint32_t i, uint32_t i2, bool v) : pcl_index(i), pcl_index2(i), visited(v) {} 55 | }; 56 | 57 | struct CustomFaceInfo { 58 | CustomFaceInfo() : nesting_level(-1) {} 59 | int nesting_level; 60 | bool in_domain() { return nesting_level % 2 == 1; } 61 | }; 62 | 63 | // Alias the fairly long polyline simplification namespace 64 | namespace PS = CGAL::Polyline_simplification_2; 65 | 66 | // Get a kernel (which AFAIK is in charge of geometric operations) and the relevant contained types 67 | typedef CGAL::Exact_predicates_inexact_constructions_kernel K; 68 | typedef K::FT FT; 69 | typedef K::Point_2 CGALPoint; 70 | typedef K::Segment_2 CGALSegment; 71 | // Convenience typedef for the type CGAL will create for each point 72 | typedef std::pair CustomPoint; 73 | // Get a vertex base type that stores a 2D point and my custom info 74 | typedef CGAL::Triangulation_vertex_base_with_info_2 CustomVb; 75 | // Get a vertex base type that works with Alpha shapes, and make it inherit from the type with my custom info 76 | typedef CGAL::Alpha_shape_vertex_base_2 AlphaVb; 77 | 78 | // Get the rest of the types required for alpha shapes 79 | typedef CGAL::Alpha_shape_face_base_2 AlphaFb; 80 | typedef CGAL::Triangulation_data_structure_2 AlphaTds; 81 | typedef CGAL::Delaunay_triangulation_2 Triangulation_2; 82 | typedef CGAL::Alpha_shape_2 Alpha_shape_2; 83 | 84 | // Get the rest of the types required for simplification 85 | typedef PS::Vertex_base_2 SimplificationVbb; 86 | typedef CGAL::Triangulation_vertex_base_with_info_2 SimplificationVb; 87 | typedef CGAL::Constrained_triangulation_face_base_2 SimplificationFbb; 88 | typedef CGAL::Triangulation_face_base_with_info_2 SimplificationFb; 89 | typedef CGAL::Triangulation_data_structure_2 SimplificationTds; 90 | typedef CGAL::Constrained_Delaunay_triangulation_2 CDT; 91 | typedef CGAL::Constrained_triangulation_plus_2 CT; 92 | typedef PS::Stop_above_cost_threshold SimplificationStop; 93 | typedef PS::Squared_distance_cost SimplificationCost; 94 | //////////////////////////////////////// END CGAL //////////////////////////////////////// 95 | 96 | class BuildSurface { 97 | typedef pcl::PointXYZ Point; 98 | typedef pcl::PointCloud PointCloud; 99 | 100 | typedef surface_types::Surface Surface; 101 | 102 | public: 103 | BuildSurface(double perpendicular_distance, double parallel_distance, double alpha, float extrude_distance); 104 | 105 | void build_updated_surface(const Surface &old_surface, const SurfaceVisualizationController &p, 106 | const std::function callback); 107 | 108 | void build_new_surface(PointCloud inliers, pcl::ModelCoefficients model, Eigen::Affine3f pose, 109 | const SurfaceVisualizationController &p, 110 | std::function callback); 111 | 112 | BuildSurface::PointCloud find_surface_boundary(const BuildSurface::PointCloud &inliers, 113 | const Eigen::Affine3f &transform); 114 | 115 | Surface new_partial_surface(const PointCloud &inliers, const pcl::ModelCoefficients &model, 116 | const Eigen::Affine3f &transform); 117 | 118 | PointCloud tile_surface(const Surface &surface); 119 | 120 | protected: 121 | double perpendicular_distance_; 122 | double parallel_distance_; 123 | double alpha_; 124 | float extrude_distance_; 125 | 126 | uint32_t next_id_; 127 | random_colors::color_generator color_gen_; 128 | 129 | private: 130 | pcl::ModelCoefficients optimize_model_for_inliers(const PointCloud &inliers, 131 | const pcl::ModelCoefficients &old_coeff); 132 | pcl::ModelCoefficients find_model_for_inliers(const PointCloud &cloud, const pcl::ModelCoefficients &prev_model); 133 | 134 | Eigen::Affine3d adjust_pose_to_model(Eigen::Affine3d pose, pcl::ModelCoefficients model, 135 | const SurfaceVisualizationController &p); 136 | 137 | std::tuple> 138 | find_boundary_and_polygons(const PointCloud &cloud, const std::vector &cgal_points, 139 | const Eigen::Affine3f &transform, const SurfaceVisualizationController &p); 140 | 141 | std::vector simplify_polygons(const CT &ct); 142 | 143 | std::vector get_cgal_2d_points(const PointCloud &cloud, const Eigen::Affine3f &transform); 144 | 145 | CT get_simplified_triangulation(const std::vector &cgal_points, 146 | const std::vector &polygons) const; 147 | 148 | shape_msgs::Mesh create_trimesh(CT ct, const Eigen::Affine3f &transform); 149 | 150 | void add_mesh_face(shape_msgs::Mesh &mesh, const Eigen::Affine3f &transform, const CT &ct, bool bottom) const; 151 | 152 | PointCloud get_boundary_from_alpha(const Alpha_shape_2 &shape, const Eigen::Affine3f &transform, 153 | const SurfaceVisualizationController &p) const; 154 | 155 | PointCloud get_boundary_from_alpha(const Alpha_shape_2 &shape, const Eigen::Affine3f &transform) const; 156 | }; 157 | 158 | #endif // PROJECT_BuildSurfaces_H 159 | -------------------------------------------------------------------------------- /build_surface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | build_surface 4 | 0.0.0 5 | The build_surface package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | roscpp 44 | surface_types 45 | surface_utils 46 | arc_utilities 47 | roscpp 48 | surface_types 49 | surface_utils 50 | arc_utilities 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /collect_points/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(collect_points) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | arc_utilities 10 | surface_types 11 | surface_utils 12 | eigen_conversions 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 17 | find_package(PCL 1.8 REQUIRED) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if you package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | INCLUDE_DIRS include 107 | LIBRARIES collect_points 108 | # CATKIN_DEPENDS roscpp 109 | DEPENDS PCL 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O2 -std=c++11 -Wall -Wextra -Wno-unused-parameter") 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | # include_directories(include) 121 | include_directories( 122 | include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | include_directories(SYSTEM 126 | ${Boost_INCLUDE_DIRS} 127 | ${PCL_INCLUDE_DIRS} 128 | ${Eigen3_INCLUDE_DIRS} 129 | ) 130 | 131 | ## Declare a C++ library 132 | add_library(collect_points 133 | src/${PROJECT_NAME}/CollectPoints.cpp 134 | ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | add_dependencies(collect_points ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | target_link_libraries(collect_points 142 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES} 143 | ) 144 | ## Declare a C++ executable 145 | # add_executable(collect_points_node src/collect_points_node.cpp) 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(collect_points_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(collect_points_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | install(TARGETS collect_points 172 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | ) 176 | 177 | ## Mark cpp header files for installation 178 | install(DIRECTORY include/${PROJECT_NAME}/ 179 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 181 | PATTERN ".svn" EXCLUDE 182 | ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_collect_points.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /collect_points/include/collect_points/CollectPoints.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #ifndef PROJECT_COLLECTPOINTS_H 6 | #define PROJECT_COLLECTPOINTS_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace pcl { 14 | class PointIndices; 15 | } 16 | 17 | class CollectPoints { 18 | typedef pcl::PointXYZ Point; 19 | typedef pcl::PointCloud PointCloud; 20 | 21 | typedef pcl::PointXYZL LabeledPoint; 22 | typedef pcl::PointCloud LabeledCloud; 23 | 24 | typedef pcl::octree::OctreePointCloudSearch SurfacePointsOctree; 25 | typedef pcl::octree::OctreePointCloudVoxelCentroid PendingPointsOctree; 26 | 27 | typedef std::pair CloudIndexPair; 28 | 29 | public: 30 | CollectPoints(double discretization, double perpendicular_dist, double point_inside_threshold, std::string target_frame, std::string camera_frame); 31 | 32 | void add_points(const PointCloud::ConstPtr &points); 33 | 34 | PointCloud get_pending_points(); 35 | 36 | bool inside_any_surface(const Point &point) const; 37 | 38 | CloudIndexPair pending_points_within(const Eigen::Affine3f ¢er, const Eigen::Vector3f &extents); 39 | 40 | void surfaces_within(const Eigen::Affine3f ¢er, const Eigen::Vector3f &extents, const std::function callback); 41 | 42 | void add_surface(const PointCloud &points, uint32_t label); 43 | 44 | void remove_surface(uint32_t label); 45 | 46 | protected: 47 | double perpendicular_dist_; 48 | double point_inside_threshold_; 49 | std::string target_frame_; 50 | 51 | std::string camera_frame_; 52 | 53 | tf::TransformListener tf_listener_; 54 | SurfacePointsOctree surface_points_; 55 | PendingPointsOctree pending_points_; 56 | SurfacePointsOctree::PointCloud::Ptr surface_points_cloud_; 57 | PendingPointsOctree::PointCloud::Ptr pending_points_cloud_; 58 | }; 59 | 60 | #endif // PROJECT_COLLECTPOINTS_H 61 | -------------------------------------------------------------------------------- /collect_points/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | collect_points 4 | 0.0.0 5 | The collect_points package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | roscpp 44 | arc_utilities 45 | surface_types 46 | surface_utils 47 | eigen_conversions 48 | roscpp 49 | arc_utilities 50 | surface_types 51 | surface_utils 52 | eigen_conversions 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /collect_points/src/collect_points/CollectPoints.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #include "collect_points/CollectPoints.h" 6 | 7 | #include 8 | 9 | // PCL Includes 10 | #include 11 | #include 12 | #include 13 | 14 | // Utils 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using surface_utils::toLabeledPoint; 22 | 23 | CollectPoints::CollectPoints(double discretization, double perpendicular_dist, double point_inside_threshold, std::string target_frame, 24 | std::string camera_frame) 25 | : perpendicular_dist_(perpendicular_dist), point_inside_threshold_(point_inside_threshold), target_frame_(target_frame), camera_frame_(camera_frame), 26 | tf_listener_(), surface_points_(discretization), pending_points_(discretization) { 27 | pending_points_cloud_ = boost::make_shared(); 28 | pending_points_.setInputCloud(pending_points_cloud_); 29 | 30 | surface_points_cloud_ = boost::make_shared(); 31 | surface_points_.setInputCloud(surface_points_cloud_); 32 | } 33 | 34 | void CollectPoints::add_points(const PointCloud::ConstPtr &points) { 35 | for (const auto &point : *points) { 36 | if (!inside_any_surface(point)) { 37 | pending_points_.addPointToCloud(point, pending_points_cloud_); 38 | } 39 | } 40 | } 41 | 42 | CollectPoints::PointCloud CollectPoints::get_pending_points() { 43 | PointCloud cloud; 44 | // Assume the current camera transform is the viewpoint of all the pending points 45 | tf::StampedTransform transform; 46 | try { 47 | tf_listener_.lookupTransform(target_frame_, camera_frame_, ros::Time(0), transform); 48 | } catch (tf2::TransformException ex) { 49 | // I don't really want this throttled, but the only way I can get it to not warn within a few seconds of 50 | // starting up is to use DELAYED_THROTTLE, and it's probably fine if it is throttled 51 | ROS_WARN_STREAM_DELAYED_THROTTLE(5, "Failed to get camera viewpoint: " << ex.what()); 52 | return cloud; // Return an empty cloud 53 | } 54 | pending_points_.getVoxelCentroids(cloud.points); 55 | // Populate the sensor origin 56 | cloud.sensor_origin_[0] = static_cast(transform.getOrigin().x()); 57 | cloud.sensor_origin_[1] = static_cast(transform.getOrigin().y()); 58 | cloud.sensor_origin_[2] = static_cast(transform.getOrigin().z()); 59 | cloud.sensor_origin_[3] = 1; 60 | // Populate the sensor orientation 61 | geometry_msgs::Quaternion quat; 62 | tf::quaternionTFToMsg(transform.getRotation(), quat); 63 | Eigen::Quaterniond eigen_quat; 64 | tf::quaternionMsgToEigen(quat, eigen_quat); 65 | cloud.sensor_orientation_ = eigen_quat.cast(); 66 | return cloud; 67 | } 68 | 69 | bool CollectPoints::inside_any_surface(const Point &point) const { 70 | std::vector neighbors; 71 | std::vector distances; 72 | 73 | return surface_points_.radiusSearch(toLabeledPoint(point), point_inside_threshold_, neighbors, distances, 1) > 0; 74 | } 75 | 76 | CollectPoints::CloudIndexPair CollectPoints::pending_points_within(const Eigen::Affine3f ¢er, 77 | const Eigen::Vector3f &extents) { 78 | //pcl::ScopeTime st("CollectPoints::pending_points_within"); 79 | auto result = std::make_pair(get_pending_points(), pcl::PointIndices()); 80 | Eigen::Vector3f xyz, rpy; 81 | pcl::getTranslationAndEulerAngles(center, xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]); 82 | 83 | pcl::CropBox crop; 84 | 85 | crop.setInputCloud(boost::shared_ptr(&result.first, null_deleter())); 86 | crop.setMax({extents[0], extents[1], extents[2], 1}); 87 | crop.setMin({-extents[0], -extents[1], -extents[2], 1}); 88 | crop.setTranslation(xyz); 89 | crop.setRotation(rpy); 90 | crop.filter(result.second.indices); 91 | 92 | return result; 93 | } 94 | 95 | void CollectPoints::surfaces_within(const Eigen::Affine3f ¢er, const Eigen::Vector3f &extents, 96 | const std::function callback) { 97 | // First limit to within the radius 98 | LabeledPoint search_pt; 99 | search_pt.x = center.translation()[0]; 100 | search_pt.y = center.translation()[1]; 101 | search_pt.z = center.translation()[2]; 102 | std::vector indices; 103 | std::vector sqr_distances_unused; 104 | // The largest diagonal of the bounding box happens to be equal to the l2 norm 105 | surface_points_.radiusSearch(search_pt, extents.norm(), indices, sqr_distances_unused); 106 | 107 | // Make sure each point is unique and within the transform 108 | std::set seen_labels; 109 | const auto pts = surface_points_.getInputCloud()->points; 110 | auto tf = center.inverse(); 111 | for (auto &i : indices) { 112 | if (seen_labels.find(pts[i].label) != seen_labels.end()) continue; 113 | 114 | LabeledPoint pt = pcl::transformPoint(pts[i], tf); 115 | if (std::abs(pt.x) <= extents[0] && std::abs(pt.y) <= extents[1] && std::abs(pt.z) <= extents[2]) { 116 | seen_labels.insert(pt.label); 117 | callback(pt.label); 118 | } 119 | } 120 | } 121 | 122 | void CollectPoints::add_surface(const CollectPoints::PointCloud &points, uint32_t label) { 123 | for (auto &pt : points.points) { 124 | LabeledPoint lpt; 125 | lpt.x = pt.x; 126 | lpt.y = pt.y; 127 | lpt.z = pt.z; 128 | lpt.label = label; 129 | surface_points_.addPointToCloud(lpt, surface_points_cloud_); 130 | } 131 | 132 | // Find and remove all the points inside the newly added surface 133 | // (actually checks all surfaces, but only newly added should return true) 134 | PointCloud pending_pts; // I only need the array but I don't want to risk doing an eigen aligned array wrong 135 | pending_points_.getVoxelCentroids(pending_pts.points); 136 | for (const auto &point : pending_pts) { 137 | if (inside_any_surface(point)) { 138 | pending_points_.deleteVoxelAtPoint(point); 139 | } 140 | } 141 | } 142 | 143 | void CollectPoints::remove_surface(uint32_t label) { 144 | // Find and remove all the points with label `label` 145 | // Note: don't just remove voxels, because that may delete more than it should 146 | auto n_removed = 0; 147 | for (auto lit = surface_points_.leaf_begin(); lit != surface_points_.leaf_end(); ++lit) { 148 | auto &leaf_indices = lit.getLeafContainer().getPointIndicesVector(); 149 | auto remove_posn = std::remove_if(leaf_indices.begin(), leaf_indices.end(), [=](const int &idx) { 150 | return surface_points_cloud_->points[idx].label == label; 151 | }); 152 | n_removed += std::distance(remove_posn, leaf_indices.end()); 153 | leaf_indices.erase(remove_posn, leaf_indices.end()); 154 | } 155 | ROS_DEBUG_STREAM("Removed " << n_removed << " points with label " << label << " from the surfaces octree"); 156 | } 157 | -------------------------------------------------------------------------------- /detect_surfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(detect_surfaces) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | surface_types 10 | surface_utils 11 | arc_utilities 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 16 | find_package(PCL 1.8 REQUIRED) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a run_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if you package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | INCLUDE_DIRS include 106 | LIBRARIES detect_surfaces 107 | # CATKIN_DEPENDS roscpp 108 | DEPENDS PCL 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O2 -std=c++11 -Wall -Wextra -Wno-unused-parameter") 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | # include_directories(include) 120 | include_directories( 121 | include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | include_directories(SYSTEM 125 | ${Boost_INCLUDE_DIRS} 126 | ${PCL_INCLUDE_DIRS} 127 | ${Eigen3_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | add_library(detect_surfaces 132 | src/${PROJECT_NAME}/DetectSurfaces.cpp 133 | ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | add_dependencies(detect_surfaces ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | target_link_libraries(detect_surfaces 141 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES} 142 | ) 143 | ## Declare a C++ executable 144 | # add_executable(detect_surfaces_node src/detect_surfaces_node.cpp) 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(detect_surfaces_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(detect_surfaces_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # install(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables and/or libraries for installation 170 | install(TARGETS detect_surfaces 171 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | ) 175 | 176 | ## Mark cpp header files for installation 177 | install(DIRECTORY include/${PROJECT_NAME}/ 178 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 180 | PATTERN ".svn" EXCLUDE 181 | ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_detect_surfaces.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /detect_surfaces/include/detect_surfaces/DetectSurfaces.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #ifndef PROJECT_DETECTSURFACES_HPP 6 | #define PROJECT_DETECTSURFACES_HPP 7 | 8 | #include 9 | 10 | // Forward declarations 11 | namespace pcl { 12 | struct PointXYZ; 13 | struct PointNormal; 14 | struct PointXYZRGB; 15 | class PointIndices; 16 | class ModelCoefficients; 17 | namespace search { 18 | template 19 | class Search; 20 | } 21 | } 22 | 23 | class SurfaceVisualizationController; 24 | 25 | namespace surface_types { 26 | class Surface; 27 | } 28 | 29 | class DetectSurfaces { 30 | typedef pcl::PointXYZ Point; 31 | typedef pcl::PointCloud PointCloud; 32 | 33 | typedef pcl::PointXYZRGB ColoredPoint; 34 | typedef pcl::PointCloud ColoredPointCloud; 35 | 36 | typedef pcl::PointNormal Normal; 37 | typedef pcl::PointCloud NormalCloud; 38 | typedef pcl::search::Search NormalSearch; 39 | 40 | typedef std::pair CloudIndexPair; 41 | 42 | public: 43 | DetectSurfaces(double perpendicular_dist, double parallel_dist, double mls_radius, 44 | unsigned int min_points_per_surface, double min_plane_width); 45 | 46 | void detect_surfaces(const CloudIndexPair &input, const SurfaceVisualizationController &p, 47 | std::function callback); 48 | 49 | private: 50 | CloudIndexPair radius_filter(const CloudIndexPair &input); 51 | 52 | NormalCloud::Ptr get_normals(const CloudIndexPair &input); 53 | 54 | unsigned long region_segmentation(NormalCloud::Ptr &normals, NormalSearch::Ptr &search, 55 | std::function callback); 56 | 57 | pcl::PointIndices sac_segmentation_and_fit(NormalCloud::Ptr &normals, NormalSearch::Ptr &search, 58 | pcl::PointIndices ®ion, 59 | std::function callback); 60 | 61 | void euclidean_segmentation(NormalCloud::Ptr &normals, NormalSearch::Ptr &search, pcl::PointIndices &inliers, 62 | std::function callback); 63 | 64 | void find_transform_and_filter(NormalCloud::Ptr &normals, pcl::PointIndices &inliers, 65 | std::function callback); 66 | 67 | ColoredPointCloud::Ptr make_segment_colored_cloud(NormalCloud::Ptr &normals, 68 | std::vector &segments); 69 | 70 | protected: 71 | double perpendicular_distance_; 72 | double parallel_distance_; 73 | double mls_radius_; 74 | unsigned int min_points_per_surface_; 75 | double min_plane_width_; 76 | }; 77 | 78 | #endif // PROJECT_DETECTSURFACES_HPP 79 | -------------------------------------------------------------------------------- /detect_surfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | detect_surfaces 4 | 0.0.0 5 | The detect_surfaces package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | roscpp 44 | arc_utilities 45 | surface_types 46 | surface_utils 47 | roscpp 48 | arc_utilities 49 | surface_types 50 | surface_utils 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /detect_surfaces/src/detect_surfaces/DetectSurfaces.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #include "detect_surfaces/DetectSurfaces.h" 6 | 7 | // PCL 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // Utils 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // Some much-needed shorter names for the sake of code formatting 24 | using Indices = pcl::PointIndices; 25 | using Model = pcl::ModelCoefficients; 26 | 27 | DetectSurfaces::DetectSurfaces(double perpendicular_dist, double parallel_dist, double mls_radius, 28 | unsigned int min_points_per_surface, double min_plane_width) 29 | : perpendicular_distance_(perpendicular_dist), parallel_distance_(parallel_dist), mls_radius_(mls_radius), 30 | min_points_per_surface_(min_points_per_surface), min_plane_width_(min_plane_width) { 31 | assert(perpendicular_distance_ > 0); 32 | assert(parallel_distance_ > 0); 33 | assert(mls_radius > 0); 34 | assert(min_points_per_surface_ >= 3); 35 | } 36 | 37 | void DetectSurfaces::detect_surfaces(const CloudIndexPair &input_pre_filtering, const SurfaceVisualizationController &p, 38 | std::function callback) { 39 | //pcl::ScopeTime st("DetectSurfaces::detect_surfaces"); 40 | auto input = radius_filter(input_pre_filtering); 41 | auto normals = get_normals(input); 42 | if (input.second.indices.size() != normals->size()) { 43 | ROS_ERROR_STREAM("Expected normals to have " << input.second.indices.size() << " points, but it had " 44 | << normals->size()); 45 | assert(false && "Normals cloud has the wrong number of points"); 46 | } 47 | 48 | assert(input_pre_filtering.first.sensor_origin_.isApprox(normals->sensor_origin_) && 49 | "Get normals did not preserve sensor origin"); 50 | assert(input_pre_filtering.first.sensor_orientation_.isApprox(normals->sensor_orientation_) && 51 | "Get normals did not preserve sensor orientation"); 52 | 53 | p.points("normals", normals); 54 | p.normal_vectors("normal_vectors", normals); 55 | 56 | // Build a search object that can be shared among the algorithms 57 | NormalSearch::Ptr normal_search = boost::make_shared>(); 58 | normal_search->setInputCloud(normals); 59 | 60 | std::vector planes; 61 | std::vector tfs; 62 | 63 | int n_regions = 0, n_planes = 0, n_euclidean = 0, n_filtered = 0; 64 | 65 | // Welcome to callback hell! 66 | region_segmentation(normals, normal_search, [&, this](Indices region) { 67 | n_regions++; 68 | sac_segmentation_and_fit(normals, normal_search, region, [&, this](Indices inliers, Model model) { 69 | n_planes++; 70 | euclidean_segmentation(normals, normal_search, inliers, [&, this](Indices segment) { 71 | n_euclidean++; 72 | find_transform_and_filter(normals, segment, [&, this](Eigen::Affine3f transform) { 73 | n_filtered++; 74 | 75 | planes.push_back(segment); 76 | tfs.push_back(transform); 77 | // segment refers to normals, which is a subset of input.first, so it needs to be reindex to 78 | // refer to index.first before being returned 79 | if (input.second.indices.size() != normals->size()) { 80 | ROS_ERROR_STREAM("Expected normals to have " << input.second.indices.size() 81 | << " points, but it had " << normals->size()); 82 | assert(false && "Normals cloud has the wrong number of points"); 83 | } 84 | callback(surfaces_pcl_utils::reindex(input.second, segment), model, transform); 85 | }); 86 | }); 87 | }); 88 | }); 89 | 90 | ROS_DEBUG_STREAM("DetectSurfaces found " << n_regions << " regions, " << n_planes << " planes, " << n_euclidean 91 | << " euclidean segements, " << n_filtered << " surfaces after filtering"); 92 | 93 | p.points("new_planes", make_segment_colored_cloud(normals, planes)); 94 | p.poses("plane_transforms", tfs); 95 | } 96 | 97 | DetectSurfaces::CloudIndexPair DetectSurfaces::radius_filter(const CloudIndexPair &input) { 98 | pcl::RadiusOutlierRemoval filter; 99 | filter.setRadiusSearch(mls_radius_); 100 | filter.setMinNeighborsInRadius(3); 101 | 102 | filter.setInputCloud(boost::shared_ptr(&input.first, null_deleter())); 103 | filter.setIndices(boost::shared_ptr>(&input.second.indices, null_deleter())); 104 | 105 | auto output = std::make_pair(input.first, pcl::PointIndices()); 106 | 107 | filter.filter(output.second.indices); 108 | return output; 109 | } 110 | 111 | DetectSurfaces::NormalCloud::Ptr DetectSurfaces::get_normals(const CloudIndexPair &input) { 112 | pcl::MovingLeastSquares mls; 113 | mls.setSearchRadius(mls_radius_); 114 | mls.setComputeNormals(true); 115 | mls.setPolynomialFit(false); 116 | mls.setPolynomialOrder(0); 117 | 118 | mls.setInputCloud(boost::shared_ptr(&input.first, null_deleter())); 119 | mls.setIndices(boost::shared_ptr>(&input.second.indices, null_deleter())); 120 | 121 | // Do the reconstruction 122 | auto normals = boost::make_shared(); 123 | mls.process(*normals); 124 | 125 | assert(normals->size() <= input.second.indices.size() && "Normals size is greater than indices size"); 126 | assert(normals->size() >= input.second.indices.size() && "Normals size is less than indices size"); 127 | 128 | normals->sensor_origin_ = input.first.sensor_origin_; 129 | normals->sensor_orientation_ = input.first.sensor_orientation_; 130 | 131 | return normals; 132 | } 133 | 134 | std::size_t DetectSurfaces::region_segmentation(NormalCloud::Ptr &normals, NormalSearch::Ptr &search, 135 | std::function callback) { 136 | pcl::RegionGrowing rgs; 137 | rgs.setMinClusterSize(min_points_per_surface_); 138 | rgs.setSmoothModeFlag(false); 139 | 140 | rgs.setInputCloud(normals); 141 | rgs.setInputNormals(normals); 142 | rgs.setSearchMethod(search); 143 | 144 | // Do the reconstruction 145 | std::vector clusters; 146 | rgs.extract(clusters); 147 | 148 | // ROS_INFO_STREAM("Region segmentation found " << clusters.size() << " regions"); 149 | 150 | std::reverse(clusters.begin(), clusters.end()); // For debuggging 151 | 152 | std::for_each(clusters.begin(), clusters.end(), callback); 153 | 154 | return clusters.size(); 155 | } 156 | 157 | pcl::PointIndices 158 | DetectSurfaces::sac_segmentation_and_fit(NormalCloud::Ptr &normals, NormalSearch::Ptr &search, 159 | pcl::PointIndices ®ion, 160 | std::function callback) { 161 | pcl::SACSegmentationFromNormals sac; 162 | sac.setModelType(pcl::SACMODEL_NORMAL_PLANE); 163 | sac.setMethodType(pcl::SAC_RANSAC); 164 | sac.setDistanceThreshold(perpendicular_distance_); 165 | sac.setNormalDistanceWeight(0.1); 166 | sac.setOptimizeCoefficients(true); 167 | 168 | sac.setInputCloud(normals); 169 | sac.setInputNormals(normals); 170 | // sac.setSamplesMaxDist(parallel_distance_, search); 171 | 172 | auto remaining_indices = boost::make_shared(region); 173 | pcl::PointIndices remaining_indices_tmp; 174 | int n_allowed_fails = 3; 175 | int n_found_clusters = 0; 176 | 177 | while (n_allowed_fails > 0) { 178 | pcl::PointIndices inliers; 179 | pcl::ModelCoefficients model; 180 | // Limit SAC to only the points in this cluster 181 | sac.setIndices(remaining_indices); 182 | 183 | // Do the SAC segmentation 184 | sac.segment(inliers, model); 185 | 186 | if (inliers.indices.size() >= min_points_per_surface_) { 187 | // Make sure the normal is oriented towards the cloud's acquisition viewpoint 188 | normals->sensor_origin_[3] = 1; // Required to ensure the correctness of the `if` statement 189 | // The correctness of the `if` statement depends on the first 3 values of the model being normalized 190 | assert(std::abs(Eigen::Map(model.values.data(), 3).norm() - 1) < 1e-4 && 191 | "Coefficients aren't normalized"); 192 | if (Eigen::Map(model.values.data(), 4).dot(normals->sensor_origin_) < 0) { 193 | ROS_DEBUG_STREAM("Orienting plane normal towards point <" 194 | << normals->sensor_origin_[0] << ", " << normals->sensor_origin_[1] << ", " 195 | << normals->sensor_origin_[2] << ", " << normals->sensor_origin_[3] << ">"); 196 | model.values[0] *= -1; 197 | model.values[1] *= -1; 198 | model.values[2] *= -1; 199 | model.values[3] *= -1; 200 | } 201 | // Make sure I didn't mess it up 202 | assert(std::abs(Eigen::Map(model.values.data(), 3).norm() - 1) < 1e-4 && 203 | "Coefficients aren't normalized after re-orienting"); 204 | assert(Eigen::Map(model.values.data(), 4).dot(normals->sensor_origin_) >= 0 && 205 | "Model isn't oriented towards viewpoint after re-orienting"); 206 | 207 | n_found_clusters++; 208 | callback(inliers, model); 209 | } else { 210 | n_allowed_fails -= 1; 211 | continue; 212 | } 213 | 214 | if (remaining_indices->indices.size() - inliers.indices.size() < min_points_per_surface_) { 215 | break; 216 | } 217 | 218 | // Only sort if necessary 219 | if (remaining_indices_tmp.indices.empty()) { 220 | std::sort(remaining_indices->indices.begin(), remaining_indices->indices.end()); 221 | } else { 222 | remaining_indices_tmp.indices.clear(); 223 | } 224 | 225 | // The inliers have no guarantee of sorted-ness 226 | std::sort(inliers.indices.begin(), inliers.indices.end()); 227 | 228 | // Copies everything in remaining_src EXCEPT the contents of inliners into remaining_temp 229 | std::set_difference(remaining_indices->indices.begin(), remaining_indices->indices.end(), 230 | inliers.indices.begin(), inliers.indices.end(), 231 | std::back_inserter(remaining_indices_tmp.indices)); 232 | remaining_indices->indices.swap(remaining_indices_tmp.indices); 233 | } 234 | 235 | // ROS_INFO_STREAM("SAC segmentation found " << n_found_clusters << " planes"); 236 | 237 | return *remaining_indices; 238 | } 239 | 240 | void DetectSurfaces::euclidean_segmentation(NormalCloud::Ptr &normals, NormalSearch::Ptr &search, 241 | pcl::PointIndices &inliers, 242 | std::function callback) { 243 | pcl::EuclideanClusterExtraction seg; 244 | seg.setMinClusterSize(min_points_per_surface_); 245 | seg.setClusterTolerance(parallel_distance_); 246 | 247 | seg.setInputCloud(normals); 248 | seg.setIndices(boost::shared_ptr>(&inliers.indices, null_deleter())); 249 | seg.setSearchMethod(search); 250 | 251 | std::vector clusters; 252 | seg.extract(clusters); 253 | // ROS_INFO_STREAM("Euclidean segmentation found " << clusters.size() << " clusters"); 254 | 255 | std::for_each(clusters.begin(), clusters.end(), callback); 256 | } 257 | 258 | void DetectSurfaces::find_transform_and_filter(NormalCloud::Ptr &normals, pcl::PointIndices &inliers, 259 | std::function callback) { 260 | // Lifted from pcl::getPrincipalTransformation 261 | // (not using that because I want to be able to pass indices) 262 | Eigen::Affine3f tf; 263 | EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 264 | Eigen::Vector4f centroid; 265 | 266 | pcl::computeMeanAndCovarianceMatrix(*normals, inliers, covariance_matrix, centroid); 267 | 268 | EIGEN_ALIGN16 Eigen::Matrix3f eigen_vects; 269 | Eigen::Vector3f eigen_vals; 270 | pcl::eigen33(covariance_matrix, eigen_vects, eigen_vals); 271 | 272 | tf.translation() = centroid.head(3); 273 | tf.linear() = eigen_vects; 274 | 275 | // This puts x along the major axis instead of z 276 | tf.rotate(Eigen::AngleAxisf(M_PI_2, Eigen::Vector3f::UnitY())); 277 | 278 | auto tf_inverse = tf.inverse(); 279 | 280 | // Note: Don't start at zero, because if there aren't points around zero that incorrectly increases the spread 281 | float max_y = std::numeric_limits::min(), min_y = std::numeric_limits::max(); 282 | float max_z = std::numeric_limits::min(), min_z = std::numeric_limits::max(); 283 | for (const int point_idx : inliers.indices) { 284 | Normal pt = pcl::transformPoint((*normals)[point_idx], tf_inverse); 285 | 286 | max_y = std::max(max_y, pt.y); 287 | min_y = std::min(min_y, pt.y); 288 | max_z = std::max(max_z, pt.z); 289 | min_z = std::min(min_z, pt.z); 290 | } 291 | 292 | assert((max_z - min_z) * perpendicular_distance_ && "Z-spread was not less than twice the perpendicular distance"); 293 | 294 | if (max_y - min_y < min_plane_width_) { 295 | return; 296 | } 297 | 298 | // Make sure the orientation of the transform puts the sensor origin in +z direction 299 | auto origin_tf = tf.inverse() * normals->sensor_origin_.head<3>(); 300 | if (origin_tf[2] < 0) { 301 | // Rotate by pi around the y axis to flip the transform 302 | tf.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY())); 303 | } 304 | 305 | callback(tf); 306 | } 307 | 308 | auto DetectSurfaces::make_segment_colored_cloud(NormalCloud::Ptr &normals, std::vector &segments) 309 | -> DetectSurfaces::ColoredPointCloud::Ptr { 310 | auto result = boost::make_shared(); 311 | 312 | random_colors::color_generator gen(0.6, 0.99); 313 | 314 | std::size_t result_i = 0; 315 | for (auto &segment : segments) { 316 | // resize + assign is more efficient than reserve + push_back because of PointCloud implementation details 317 | result->resize(result->size() + segment.indices.size()); 318 | 319 | auto color_tuple = gen.rgb(); 320 | // This point object is reused in the loop 321 | ColoredPoint pt(static_cast(std::get<0>(color_tuple) * 255), 322 | static_cast(std::get<1>(color_tuple) * 255), 323 | static_cast(std::get<2>(color_tuple) * 255)); 324 | 325 | for (auto &index : segment.indices) { 326 | pt.x = (*normals)[index].x; 327 | pt.y = (*normals)[index].y; 328 | pt.z = (*normals)[index].z; 329 | 330 | // Assigns a copy 331 | (*result)[result_i] = pt; 332 | result_i++; 333 | } 334 | } 335 | 336 | return result; 337 | } 338 | -------------------------------------------------------------------------------- /expand_surfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(expand_surfaces) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | surface_types 10 | surface_utils 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 15 | find_package(PCL 1.8 REQUIRED) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if you package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | INCLUDE_DIRS include 105 | LIBRARIES expand_surfaces 106 | # CATKIN_DEPENDS roscpp 107 | DEPENDS PCL 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O2 -std=c++11 -Wall -Wextra -Wno-unused-parameter") 115 | 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | # include_directories(include) 120 | include_directories( 121 | include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | include_directories(SYSTEM 125 | ${Boost_INCLUDE_DIRS} 126 | ${PCL_INCLUDE_DIRS} 127 | ${Eigen3_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | add_library(expand_surfaces 132 | src/${PROJECT_NAME}/ExpandSurfaces.cpp 133 | ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | add_dependencies(expand_surfaces ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | target_link_libraries(expand_surfaces 141 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES} 142 | ) 143 | ## Declare a C++ executable 144 | # add_executable(expand_surfaces_node src/expand_surfaces_node.cpp) 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(expand_surfaces_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(expand_surfaces_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # install(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | # Mark executables and/or libraries for installation 170 | install(TARGETS expand_surfaces 171 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | ) 175 | 176 | ## Mark cpp header files for installation 177 | install(DIRECTORY include/${PROJECT_NAME}/ 178 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 180 | PATTERN ".svn" EXCLUDE 181 | ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_expand_surfaces.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /expand_surfaces/include/expand_surfaces/ExpandSurfaces.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/20/16. 3 | // 4 | 5 | #ifndef PROJECT_EXPANDSURFACES_HPP 6 | #define PROJECT_EXPANDSURFACES_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // Forward declarations 15 | namespace pcl { 16 | struct PointXYZ; 17 | class PointIndices; 18 | class ModelCoefficients; 19 | namespace search { 20 | template 21 | class Search; 22 | } 23 | } 24 | 25 | namespace surface_types { 26 | class Surface; 27 | } 28 | 29 | class ExpandSurfaces { 30 | typedef pcl::PointXYZ Point; 31 | typedef pcl::PointCloud PointCloud; 32 | typedef surface_types::Surface Surface; 33 | 34 | typedef pcl::search::Search Search; 35 | 36 | typedef std::pair CloudIndexPair; 37 | 38 | public: 39 | ExpandSurfaces(double perpendicular_dist, double parallel_dist); 40 | 41 | pcl::PointIndices expand_surfaces(const std::vector &surfaces, const CloudIndexPair &input, 42 | std::function callback); 43 | 44 | void expand_new_surface(const PointCloud &points, const pcl::search::Search &search, 45 | const pcl::PointIndices &inliers, const Eigen::Affine3f &transform, 46 | std::function callback); 47 | 48 | private: 49 | std::set filterWithinRadiusConnected(const PointCloud &cloud, const Search &search, 50 | const PointCloud &edge_points, const Eigen::Affine3f &tf) const; 51 | 52 | pcl::PointIndices filterWithinRadiusConnected(const PointCloud &cloud, const Search &search, 53 | const PointCloud &edge_points, const Eigen::Affine3f &tf, 54 | const pcl::PointIndices &remaining_indices) const; 55 | 56 | pcl::PointIndices filterWithinModelDistance(const PointCloud::ConstPtr &input, const pcl::PointIndices &indices, 57 | const pcl::ModelCoefficients &coeff); 58 | 59 | protected: 60 | double perpendicular_distance_; 61 | double parallel_distance_; 62 | }; 63 | 64 | #endif // PROJECT_EXPANDSURFACES_HPP 65 | -------------------------------------------------------------------------------- /expand_surfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | expand_surfaces 4 | 0.0.0 5 | The expand_surfaces package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | roscpp 44 | arc_utilities 45 | surface_types 46 | surface_utils 47 | roscpp 48 | arc_utilities 49 | surface_types 50 | surface_utils 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /expand_surfaces/src/expand_surfaces/ExpandSurfaces.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #include "expand_surfaces/ExpandSurfaces.h" 6 | 7 | // std and Boost 8 | #include 9 | 10 | // PCL 11 | #include 12 | #include 13 | 14 | // Surface types 15 | #include 16 | 17 | // Utils 18 | #include 19 | #include 20 | #include 21 | 22 | ExpandSurfaces::ExpandSurfaces(double perpendicular_dist, double parallel_dist) 23 | : perpendicular_distance_(perpendicular_dist), parallel_distance_(parallel_dist) { 24 | assert(perpendicular_distance_ > 0); 25 | assert(parallel_distance_ > 0); 26 | } 27 | 28 | std::set ExpandSurfaces::filterWithinRadiusConnected(const PointCloud &cloud, const Search &search, 29 | const PointCloud &edge_points, 30 | const Eigen::Affine3f &tf) const { 31 | std::set within_radius_indices; 32 | 33 | std::queue to_search(std::deque(edge_points.begin(), edge_points.end())); 34 | 35 | auto tfi = tf.inverse(); 36 | 37 | while (!to_search.empty()) { 38 | const auto &point = to_search.front(); 39 | 40 | std::vector tmp_indices; 41 | std::vector tmp_sqrdistances; // Only needed to fill a parameter 42 | 43 | search.radiusSearch(point, parallel_distance_, tmp_indices, tmp_sqrdistances); 44 | 45 | for (const auto &nearby_index : tmp_indices) { 46 | // Filter out points that are too far away 47 | if (std::abs(pcl::transformPoint(cloud[nearby_index], tfi).z) > perpendicular_distance_) { 48 | continue; 49 | } 50 | 51 | const auto insert_result = within_radius_indices.insert(nearby_index); 52 | 53 | if (insert_result.second) { // If the index didn't already exist in the list 54 | to_search.push(search.getInputCloud()->at(static_cast(nearby_index))); 55 | } 56 | } 57 | 58 | to_search.pop(); 59 | } 60 | 61 | return within_radius_indices; 62 | } 63 | 64 | pcl::PointIndices ExpandSurfaces::filterWithinRadiusConnected(const PointCloud &cloud, const Search &search, 65 | const PointCloud &edge_points, const Eigen::Affine3f &tf, 66 | const pcl::PointIndices &remaining_indices) const { 67 | std::set within_radius_indices = filterWithinRadiusConnected(cloud, search, edge_points, tf); 68 | 69 | pcl::PointIndices within_radius_nodupes; 70 | // This may reserve more space than necessary, but it shouldn't be much more. 71 | within_radius_nodupes.indices.reserve(within_radius_indices.size()); 72 | 73 | // Sortedness Invariants: 74 | // within_radius_indices is sorted because it's a set 75 | // remaining_indices' sortedness is a loop invariant 76 | std::set_intersection(within_radius_indices.begin(), within_radius_indices.end(), remaining_indices.indices.begin(), 77 | remaining_indices.indices.end(), std::back_inserter(within_radius_nodupes.indices)); 78 | 79 | return within_radius_nodupes; 80 | } 81 | 82 | pcl::PointIndices ExpandSurfaces::filterWithinModelDistance(const PointCloud::ConstPtr &input, 83 | const pcl::PointIndices &indices, 84 | const pcl::ModelCoefficients &coeff) { 85 | auto model = pcl::SampleConsensusModelPlane(input, indices.indices); 86 | 87 | pcl::PointIndices output_indices; 88 | model.selectWithinDistance(Eigen::Vector4f(coeff.values[0], coeff.values[1], coeff.values[2], coeff.values[3]), 89 | perpendicular_distance_, output_indices.indices); 90 | 91 | return output_indices; 92 | } 93 | 94 | pcl::PointIndices ExpandSurfaces::expand_surfaces(const std::vector &surfaces, const CloudIndexPair &input, 95 | std::function callback) { 96 | //pcl::ScopeTime st("SurfaceDetection::detect_surfaces_within"); 97 | auto cloud = boost::shared_ptr(&input.first, null_deleter()); 98 | auto indices = boost::shared_ptr(&input.second, null_deleter()); 99 | 100 | if (cloud->size() == 0 || indices->indices.size() == 0 || surfaces.size() == 0) { 101 | return input.second; 102 | } 103 | 104 | pcl::search::KdTree search(false); 105 | search.setInputCloud(cloud, boost::shared_ptr>(&input.second.indices, null_deleter())); 106 | 107 | pcl::PointIndices remaining_indices; 108 | pcl::PointIndices remaining_indices_tmp; 109 | remaining_indices.indices = indices->indices; 110 | 111 | for (const auto &old_surface : surfaces) { 112 | 113 | // 114 | // FILTER TO POINTS NEAR SURFACE BOUNDARY AND ON THE SURFACE 115 | // 116 | auto distance_filtered = filterWithinRadiusConnected(*cloud, search, old_surface.boundary, 117 | old_surface.pose.cast(), remaining_indices); 118 | if (distance_filtered.indices.size() == 0) continue; 119 | 120 | // 121 | // ADD NEW POINTS TO SURFACE 122 | // 123 | surface_types::Surface new_surface(old_surface); 124 | new_surface.inliers += pcl::PointCloud(*cloud, distance_filtered.indices); 125 | new_surface.clear_computed_values(); 126 | 127 | callback(new_surface); 128 | 129 | // 130 | // REMOVE THOSE POINTS FROM THE INPUT 131 | // 132 | remaining_indices_tmp.indices.clear(); 133 | remaining_indices_tmp.indices.reserve(remaining_indices.indices.size() - distance_filtered.indices.size()); 134 | std::set_difference(remaining_indices.indices.begin(), remaining_indices.indices.end(), 135 | distance_filtered.indices.begin(), distance_filtered.indices.end(), 136 | std::back_inserter(remaining_indices_tmp.indices)); 137 | remaining_indices.indices.swap(remaining_indices_tmp.indices); 138 | } 139 | 140 | // Always publish remaining indices 141 | return remaining_indices; 142 | } 143 | 144 | void ExpandSurfaces::expand_new_surface(const PointCloud &points, const pcl::search::Search &search, 145 | const pcl::PointIndices &inliers, const Eigen::Affine3f &transform, 146 | std::function callback) { 147 | // 148 | // FILTER TO POINTS NEAR SURFACE BOUNDARY 149 | // 150 | auto radius_filtered = filterWithinRadiusConnected(points, search, PointCloud(points, inliers.indices), transform); 151 | 152 | // 153 | // ADD NEW POINTS TO SURFACE 154 | // 155 | pcl::PointIndices indices; 156 | indices.indices = inliers.indices; 157 | indices.indices.reserve(indices.indices.size() + radius_filtered.size()); 158 | std::copy(radius_filtered.begin(), radius_filtered.end(), std::back_inserter(indices.indices)); 159 | 160 | callback(indices); 161 | } 162 | -------------------------------------------------------------------------------- /surface_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(surface_detection) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | pcl_conversions 9 | pcl_ros 10 | roscpp 11 | surface_utils 12 | arc_utilities 13 | surface_msgs2 14 | surface_types 15 | expand_surfaces 16 | detect_surfaces 17 | collect_points 18 | build_surface 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 23 | find_package(PCL 1.8 REQUIRED) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a run_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # std_msgs # Or other packages containing msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if you package contains header files 108 | ## LIBRARIES: libraries you create in this project thasdependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects t so need 110 | ## DEPENDS: system dependencies of this project that dalendent projects also need 111 | catkin_package( 112 | INCLUDE_DIRS include 113 | # LIBRARIES surface_detection 114 | # CATKIN_DEPENDS other_catkin_pkg 115 | DEPENDS PCL 116 | ) 117 | 118 | ########### 119 | ## Build ## 120 | ########### 121 | 122 | set(CMAKE_BUILD_TYPE Debug) 123 | 124 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O2 -std=c++11 -Wall -Wextra -Wno-unused-parameter") 125 | 126 | ## Specify additional locations of header files 127 | ## Your package locations should be listed before other locations 128 | include_directories( 129 | include 130 | ${catkin_INCLUDE_DIRS} 131 | ) 132 | include_directories(SYSTEM 133 | ${Boost_INCLUDE_DIRS} 134 | ${PCL_INCLUDE_DIRS} 135 | ${Eigen3_INCLUDE_DIRS} 136 | ) 137 | 138 | ## Declare a C++ library 139 | # add_library(surface_detection 140 | # src/${PROJECT_NAME}/surface_detection.cpp 141 | # ) 142 | 143 | ## Add cmake target dependencies of the library 144 | ## as an example, code may need to be generated before libraries 145 | ## either from message generation or dynamic reconfigure 146 | # add_dependencies(surface_detection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Declare a C++ executable 149 | add_executable(surface_detection_node 150 | src/surface_detection_node.cpp 151 | src/SurfaceDetection.cpp) 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | add_dependencies(surface_detection_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Specify libraries to link a library or executable target against 158 | target_link_libraries(surface_detection_node 159 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES} 160 | ) 161 | 162 | #cotire(surface_detection_node) 163 | 164 | ############# 165 | ## Install ## 166 | ############# 167 | 168 | # all install targets should use catkin DESTINATION variables 169 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 170 | 171 | ## Mark executable scripts (Python etc.) for installation 172 | ## in contrast to setup.py, you can choose the destination 173 | # install(PROGRAMS 174 | # scripts/my_python_script 175 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark executables and/or libraries for installation 179 | # install(TARGETS surface_detection surface_detection_node 180 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark cpp header files for installation 186 | install(DIRECTORY include/${PROJECT_NAME}/ 187 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 188 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 189 | PATTERN ".svn" EXCLUDE 190 | ) 191 | 192 | ## Mark other files for installation (e.g. launch and bag files, etc.) 193 | # install(FILES 194 | # # myfile1 195 | # # myfile2 196 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 197 | # ) 198 | 199 | ############# 200 | ## Testing ## 201 | ############# 202 | 203 | ## Add gtest based cpp test target and link libraries 204 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_surface_detection.cpp) 205 | # if(TARGET ${PROJECT_NAME}-test) 206 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 207 | # endif() 208 | 209 | ## Add folders to be run by python nosetests 210 | # catkin_add_nosetests(test) 211 | -------------------------------------------------------------------------------- /surface_detection/custom_rosconsole.conf: -------------------------------------------------------------------------------- 1 | # Override surface_filters to output everything 2 | log4j.logger.ros.build_surface=DEBUG 3 | log4j.logger.ros.collect_points=DEBUG 4 | log4j.logger.ros.detect_surfaces=DEBUG 5 | log4j.logger.ros.expand_surfaces=DEBUG 6 | log4j.logger.ros.surface_detection=DEBUG 7 | 8 | # Override surface_filters to output nothing 9 | #log4j.logger.ros.build_surface=ERROR 10 | #log4j.logger.ros.collect_points=ERROR 11 | #log4j.logger.ros.detect_surfaces=ERROR 12 | #log4j.logger.ros.expand_surfaces=ERROR 13 | #log4j.logger.ros.surface_detection=ERROR 14 | -------------------------------------------------------------------------------- /surface_detection/include/surface_detection/SurfaceDetection.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/20/16. 3 | // 4 | 5 | #ifndef PROJECT_SURFACEDETECTION_H 6 | #define PROJECT_SURFACEDETECTION_H 7 | 8 | // std / Boost include 9 | #include 10 | 11 | // Surface types includes 12 | #include 13 | 14 | // Algorithm includes 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // Utils includes 21 | #include "surface_utils/SurfaceVisualizationController.hpp" 22 | #include 23 | 24 | namespace surface_detection { 25 | class SurfaceDetection { 26 | public: 27 | typedef pcl::PointXYZ Point; 28 | typedef pcl::PointCloud PointCloud; 29 | 30 | typedef surface_types::Surface Surface; 31 | typedef surface_types::Surfaces Surfaces; 32 | 33 | SurfaceDetection(double discretization, double perpendicular_dist, double parallel_dist, double point_inside_threshold, double mls_radius, 34 | unsigned int min_pts_in_surface, double min_plane_width, double alpha, float extrusion_distance, 35 | std::string target_frame, std::string camera_frame); 36 | 37 | void add_points(const PointCloud::ConstPtr &points) { collect_points_.add_points(points); } 38 | 39 | PointCloud get_pending_points() { return collect_points_.get_pending_points(); } 40 | 41 | Surfaces detect_surfaces_within(const Eigen::Affine3f ¢er, const Eigen::Vector3f &extents, 42 | const SurfaceVisualizationController &p); 43 | 44 | void add_start_surface(double discretization, double start_surface_extent_x, 45 | double start_surface_extent_y, const SurfaceVisualizationController &p); 46 | 47 | private: 48 | // Configuration 49 | std::string target_frame_; 50 | double parallel_distance_; 51 | double perpendicular_distance_; 52 | double sqr_perpendicular_distance_; 53 | unsigned int min_pts_in_surface_; 54 | 55 | // State 56 | std::map surfaces_; 57 | 58 | // Implementation 59 | CollectPoints collect_points_; 60 | ExpandSurfaces expand_surfaces_; 61 | DetectSurfaces detect_surfaces_; 62 | BuildSurface build_surface_; 63 | 64 | // Private methods 65 | void add_or_update_surface(Surface &updated_surface, const SurfaceVisualizationController &p); 66 | void remove_surface(const Surface &surface, const SurfaceVisualizationController &p); 67 | 68 | // This is the delegated-to one that does the work 69 | void find_merge(const PointCloud &inliers, const pcl::ModelCoefficients &model, 70 | const std::vector &surfaces, const Maybe::Maybe &ignore_surface, 71 | const std::function)> &callback); 72 | // This is the one used when calling with a new surface 73 | void find_merge(const PointCloud &inliers, const pcl::ModelCoefficients &model, 74 | const std::vector &surfaces, const std::function)> &callback); 75 | // This is the one used when calling with an existing surface 76 | void find_merge(const Surface &test_surf, const std::vector &surfaces, 77 | const std::function)> &callback); 78 | 79 | Surface get_surface(uint32_t surface_id) const; 80 | }; 81 | } 82 | #endif // PROJECT_SURFACEDETECTION_H 83 | -------------------------------------------------------------------------------- /surface_detection/launch/surface_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /surface_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | surface_detection 4 | 0.0.0 5 | The surface_detection package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | pcl_conversions 44 | pcl_ros 45 | roscpp 46 | surface_utils 47 | arc_utilities 48 | surface_msgs2 49 | surface_types 50 | expand_surfaces 51 | detect_surfaces 52 | collect_points 53 | build_surface 54 | 55 | pcl_conversions 56 | pcl_ros 57 | roscpp 58 | surface_utils 59 | arc_utilities 60 | surface_msgs2 61 | surface_types 62 | expand_surfaces 63 | detect_surfaces 64 | collect_points 65 | build_surface 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /surface_detection/src/SurfaceDetection.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/29/16. 3 | // 4 | 5 | #include "surface_detection/SurfaceDetection.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace surface_detection { 15 | 16 | SurfaceDetection::SurfaceDetection(double discretization, double perpendicular_dist, double parallel_dist, double point_inside_threshold, 17 | double mls_radius, unsigned int min_pts_in_surface, double min_plane_width, 18 | double alpha, float extrusion_distance, std::string target_frame, 19 | std::string camera_frame) 20 | : target_frame_(target_frame), parallel_distance_(parallel_dist), perpendicular_distance_(perpendicular_dist), 21 | sqr_perpendicular_distance_(perpendicular_dist * perpendicular_dist), min_pts_in_surface_(min_pts_in_surface), 22 | // State 23 | surfaces_(), 24 | // Implementation 25 | collect_points_(discretization, perpendicular_dist, point_inside_threshold, target_frame, camera_frame), 26 | expand_surfaces_(perpendicular_dist, parallel_dist), 27 | detect_surfaces_(perpendicular_dist, parallel_dist, mls_radius, min_pts_in_surface, min_plane_width), 28 | build_surface_(perpendicular_dist, parallel_dist, alpha, extrusion_distance) {} 29 | 30 | void SurfaceDetection::add_start_surface(double discretization, double start_surface_extent_x, 31 | double start_surface_extent_y, const SurfaceVisualizationController &p) { 32 | pcl::ModelCoefficients coeff; 33 | coeff.values = {0, 0, 1, 0}; 34 | Eigen::Affine3f pose = Eigen::Affine3f::Identity(); 35 | PointCloud pts; 36 | for (float x = -start_surface_extent_x; x < start_surface_extent_x; x += discretization) { 37 | for (float y = -start_surface_extent_y; y < start_surface_extent_y; y += discretization) { 38 | Point pt; 39 | pt.x = x; 40 | pt.y = y; 41 | pts.push_back(pt); 42 | } 43 | } 44 | build_surface_.build_new_surface( 45 | std::move(pts), std::move(coeff), std::move(pose), p, 46 | [&, this](Surface updated_surface) { this->add_or_update_surface(updated_surface, p); }); 47 | } 48 | 49 | SurfaceDetection::Surfaces SurfaceDetection::detect_surfaces_within(const Eigen::Affine3f ¢er, 50 | const Eigen::Vector3f &extents, 51 | const SurfaceVisualizationController &p) { 52 | // pcl::ScopeTime st("SurfaceDetection::detect_surfaces_within"); 53 | 54 | float roll, pitch, yaw; 55 | pcl::getEulerAngles(center, roll, pitch, yaw); 56 | ROS_DEBUG_STREAM("Detecting surfaces within " << extents.transpose() << " of " << center.translation().transpose() 57 | << ", orientation " << roll << ", " << pitch << ", " << yaw); 58 | 59 | // Make an object to populate 60 | Surfaces new_surfaces; 61 | new_surfaces.header.frame_id = target_frame_; 62 | 63 | // Get points to process 64 | auto input = collect_points_.pending_points_within(center, extents); 65 | ROS_DEBUG_STREAM("Processing " << input.second.indices.size() << " points"); 66 | p.pair("input_to_detection", input); 67 | 68 | // Get existing surfaces nearby and try to expand them 69 | collect_points_.surfaces_within(center, extents, [&, this](uint32_t surface_id) { 70 | const auto existing_surface = get_surface(surface_id); 71 | existing_surface.validate(); 72 | this->find_merge(existing_surface, new_surfaces.surfaces, [&, this](Maybe::Maybe merged) { 73 | if (merged.Valid()) { 74 | merged.Get().validate(); 75 | ROS_DEBUG_STREAM(merged.Get().print_color() 76 | << "Merged existing surface " << merged.Get().id << " (" << merged.Get().inliers.size() 77 | << " points) with existing surface " << existing_surface.print_color() << "surface " 78 | << existing_surface.id << " (" << existing_surface.inliers.size() << " points)"); 79 | this->remove_surface(existing_surface, p); 80 | ROS_WARN_STREAM(merged.Get().print_color() 81 | << "Found a merged surface before expanding or detecting -- is that expected?"); 82 | new_surfaces.update_surface(merged.Get()); 83 | } else { 84 | ROS_DEBUG_STREAM(existing_surface.print_color() << "Added existing surface " << existing_surface.id); 85 | 86 | // Then this surface should be added with no modification 87 | new_surfaces.add_surface(existing_surface); 88 | } 89 | }); 90 | }); 91 | ROS_DEBUG_STREAM("Started with " << new_surfaces.surfaces.size() << " existing surfaces"); 92 | 93 | if (new_surfaces.surfaces.size() == 0) { 94 | ROS_DEBUG_STREAM("Not doing expansion because there are no surfaces"); 95 | } else if (input.second.indices.size() < 3) { 96 | ROS_DEBUG_STREAM("Not enough points to do expansion (" << input.second.indices.size() << ")"); 97 | } else { 98 | auto indices_before = input.second.indices.size(); 99 | input.second = expand_surfaces_.expand_surfaces(new_surfaces.surfaces, input, [&, this](Surface s_expanded) { 100 | s_expanded.validate(); 101 | ROS_DEBUG_STREAM(s_expanded.print_color() << "Expanded existing surface " << s_expanded.id); 102 | this->find_merge(s_expanded, new_surfaces.surfaces, [&, this](Maybe::Maybe merged) { 103 | if (merged.Valid()) { 104 | merged.Get().validate(); 105 | ROS_DEBUG_STREAM(merged.Get().print_color() 106 | << "Merged expanded existing surface " << merged.Get().id << " (" 107 | << merged.Get().inliers.size() << " points) into " << s_expanded.print_color() 108 | << "existing surface " << s_expanded.id << " (" << s_expanded.inliers.size() 109 | << " points)"); 110 | this->remove_surface(s_expanded, p); 111 | new_surfaces.update_surface(merged.Get()); 112 | } else { 113 | // The surface was still changed even if it wasn't merged 114 | new_surfaces.update_surface(s_expanded); 115 | } 116 | }); 117 | }); 118 | ROS_DEBUG_STREAM("Expand surfaces reduced number of indices from " << indices_before << " to " 119 | << input.second.indices.size()); 120 | } 121 | 122 | if (input.second.indices.size() < min_pts_in_surface_) { 123 | ROS_DEBUG_STREAM("Not enough remaining indices do run detection (" << input.second.indices.size() << ")"); 124 | } else { 125 | // Build a search object for the input points 126 | pcl::search::KdTree search(false); 127 | search.setInputCloud(boost::shared_ptr(&input.first, null_deleter()), 128 | boost::shared_ptr>(&input.second.indices, null_deleter())); 129 | 130 | // Make some shorter names for the sake of formatting 131 | using Indices = pcl::PointIndices; 132 | 133 | // Detect new surfaces 134 | detect_surfaces_.detect_surfaces( 135 | input, p, [&](Indices indices, pcl::ModelCoefficients model, Eigen::Affine3f tf) { 136 | // Expect expand_new_surface to always find more points because of how detection works 137 | expand_surfaces_.expand_new_surface(input.first, search, indices, tf, [&, this](pcl::PointIndices in) { 138 | PointCloud new_inliers_cloud(input.first, in.indices); 139 | assert(new_inliers_cloud.size() && "Got an empty cloud for some reason"); 140 | 141 | this->find_merge( 142 | new_inliers_cloud, model, new_surfaces.surfaces, [&, this](Maybe::Maybe merged) { 143 | if (merged.Valid()) { 144 | merged.Get().validate(); 145 | ROS_DEBUG_STREAM(merged.Get().print_color() << "Merged surface " << merged.Get().id 146 | << " (" << merged.Get().inliers.size() 147 | << " points) with un-numbered new surface (" 148 | << new_inliers_cloud.size() << " points)"); 149 | assert(merged.Get().model.values.size() == 4 && 150 | "Find merge returned a surface without a valid model"); 151 | new_surfaces.update_surface(merged.Get()); 152 | } else { 153 | Surface new_surface = build_surface_.new_partial_surface(new_inliers_cloud, model, tf); 154 | ROS_DEBUG_STREAM(new_surface.print_color() << "Got a brand new surface with id " 155 | << new_surface.id); 156 | new_surfaces.add_surface(new_surface); 157 | } 158 | }); 159 | }); 160 | }); 161 | ROS_DEBUG_STREAM("Detected a total of " << new_surfaces.surfaces.size() << " surfaces"); 162 | } 163 | 164 | // Final pass through surfaces to turn all partial surfaces into complete surfaces 165 | for (const auto &surface : new_surfaces.surfaces) { 166 | if (!surface.is_complete()) { 167 | build_surface_.build_updated_surface(surface, p, [&, this](Surface updated_surface) { 168 | ROS_DEBUG_STREAM(updated_surface.print_color() << "Built new or updated surface " 169 | << updated_surface.id); 170 | this->add_or_update_surface(updated_surface, p); 171 | new_surfaces.update_surface(updated_surface); 172 | }); 173 | } 174 | } 175 | 176 | return new_surfaces; 177 | } 178 | 179 | surface_types::Surface SurfaceDetection::get_surface(uint32_t surface_id) const { 180 | auto sit = surfaces_.find(surface_id); 181 | assert(sit != surfaces_.end() && "Tried to get a surface we don't have"); 182 | return sit->second; 183 | } 184 | 185 | void SurfaceDetection::find_merge(const PointCloud &inliers, const pcl::ModelCoefficients &model, 186 | const std::vector &surfaces, const Maybe::Maybe &ignore_surface, 187 | const std::function)> &callback) { 188 | assert(model.values.size() == 4 && "Find merge recieved invalid model"); 189 | auto test_normal = Eigen::Map(model.values.data()); 190 | pcl::search::KdTree search(false); 191 | for (auto &surface : surfaces) { 192 | surface.validate(); 193 | if (ignore_surface.Valid() && surface.id == ignore_surface.GetImmutable()) { 194 | continue; 195 | } 196 | 197 | auto candidate_normal = Eigen::Map(surface.model.values.data()); 198 | if (std::abs(std::acos(test_normal.dot(candidate_normal))) > 0.1) { 199 | // If the angle between the planes is greater than 1 radian, don't merge 200 | continue; 201 | } 202 | 203 | if (std::abs(model.values[3] - surface.model.values[3]) > 2 * perpendicular_distance_) { 204 | // If the surfaces are too far away, don't merge 205 | continue; 206 | } 207 | 208 | if (!search.getInputCloud()) { 209 | search.setInputCloud(boost::shared_ptr(&inliers, null_deleter())); 210 | } 211 | 212 | std::vector indices; 213 | std::vector distances; 214 | 215 | auto n_close = 0; 216 | for (auto &pt : surface.is_complete() ? surface.boundary : surface.inliers) { 217 | if (search.radiusSearch(pt, parallel_distance_, indices, distances, 1) > 0) { 218 | n_close += 1; 219 | 220 | if (n_close > 3) { 221 | // Then merge! 222 | Surface new_surface; 223 | new_surface.id = surface.id; 224 | new_surface.color = surface.color; 225 | new_surface.model = surface.model; // Use the model of the original TODO: or the largest? 226 | new_surface.pose = surface.pose; // Use the pose of the original 227 | new_surface.inliers = surface.inliers; 228 | new_surface.inliers += inliers; 229 | 230 | // ROS_INFO_STREAM(surface.print_color() << "Merging surface " << surface.id << " 231 | // with " 232 | // << inliers.size() << " new points"); 233 | callback(new_surface); 234 | return; // Don't try to merge more than once 235 | } 236 | } 237 | } 238 | } 239 | 240 | // If we got here, then no merge needed 241 | callback({}); 242 | } 243 | 244 | void SurfaceDetection::find_merge(const PointCloud &inliers, const pcl::ModelCoefficients &model, 245 | const std::vector &surfaces, 246 | const std::function)> &callback) { 247 | find_merge(inliers, model, surfaces, {}, callback); 248 | } 249 | 250 | void SurfaceDetection::find_merge(const Surface &test_surf, const std::vector &surfaces, 251 | const std::function)> &callback) { 252 | test_surf.validate(); 253 | find_merge(test_surf.inliers, test_surf.model, surfaces, test_surf.id, callback); 254 | } 255 | 256 | void SurfaceDetection::remove_surface(const Surface &surface, const SurfaceVisualizationController &p) { 257 | auto n_removed = surfaces_.erase(surface.id); 258 | assert(n_removed > 0 && "Tried to remove a surface that didn't exist"); 259 | assert(n_removed == 1 && "Tried to remove one surface and accidentally removed multiple -- how??"); 260 | collect_points_.remove_surface(surface.id); 261 | 262 | p.remove_marker("polygons", surface.id); 263 | p.remove_marker("mesh", surface.id); 264 | p.remove_marker("pose", surface.id); 265 | p.remove_marker("pose_x", surface.id); 266 | p.remove_marker("pose_y", surface.id); 267 | } 268 | 269 | void SurfaceDetection::add_or_update_surface(Surface &updated_surface, const SurfaceVisualizationController &p) { 270 | updated_surface.validate_complete(); 271 | 272 | bool is_new = surfaces_.find(updated_surface.id) == surfaces_.end(); 273 | surfaces_[updated_surface.id] = updated_surface; 274 | auto tiling = build_surface_.tile_surface(updated_surface); 275 | 276 | if (!is_new) { 277 | // Remove previous points 278 | collect_points_.remove_surface(updated_surface.id); 279 | } 280 | collect_points_.add_surface(tiling, updated_surface.id); 281 | 282 | p.points("tiling", tiling.makeShared()); 283 | p.mesh("mesh", updated_surface); 284 | p.polygons("polygons", updated_surface); 285 | p.pose("pose", updated_surface); 286 | } 287 | } -------------------------------------------------------------------------------- /surface_detection/src/surface_detection_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/18/16. 3 | // 4 | 5 | #include 6 | // pcl_ros/point_cloud.h enables subscribing to topics using PCL types 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include "surface_utils/eigen_helpers_conversions.hpp" 14 | #include "surface_detection/SurfaceDetection.h" 15 | #include "surface_utils/SurfaceVisualizationController.hpp" 16 | #include 17 | 18 | using surface_msgs2::SurfaceDetectionRequest; 19 | using surface_msgs2::SurfaceDetectionResponse; 20 | 21 | using EigenHelpersConversions::GeometryPoseToEigenAffine3f; 22 | using EigenHelpersConversions::GeometryVector3ToEigenVector3f; 23 | 24 | using surface_detection::SurfaceDetection; 25 | 26 | // Parameters 27 | // TODO: Get these from parameter server 28 | std::string target_frame = "/plan_start"; 29 | std::string camera_frame = "/left_camera_frame"; 30 | const double discretization = 0.03; 31 | const double perpendicular_distance = 0.01; 32 | const double parallel_distance = 0.05; 33 | const double point_inside_threshold = 0.03; 34 | const double mls_radius = 0.10; 35 | const unsigned int min_points_per_surface = 50; 36 | const double min_plane_width = 0.15; 37 | const double alpha = 0.01; 38 | const float extrusion_distance = 0.02; 39 | const double start_surface_extent_x = 0.5; 40 | const double start_surface_extent_y = 0.5; 41 | 42 | int main(int argc, char **argv) { 43 | // Setup 44 | ros::init(argc, argv, "surface_detection"); 45 | ros::NodeHandle n; 46 | SurfaceDetection surface_detection(discretization, perpendicular_distance, parallel_distance, point_inside_threshold, mls_radius, 47 | min_points_per_surface, min_plane_width, alpha, extrusion_distance, target_frame, 48 | camera_frame); 49 | 50 | 51 | ROS_INFO_STREAM("Connected to ROS"); 52 | 53 | // Make publishers 54 | auto pp_pub = n.advertise("pending_points", 1); 55 | 56 | // Make subscribers 57 | auto sub = n.subscribe("scan_cloud", 1000, &SurfaceDetection::add_points, &surface_detection); 58 | 59 | // Make timers 60 | auto pp_timer = n.createTimer(ros::Duration(1), [&surface_detection, &pp_pub](const ros::TimerEvent &) { 61 | auto cloud = surface_detection.get_pending_points(); 62 | cloud.header.frame_id = target_frame; 63 | 64 | ROS_DEBUG_STREAM_DELAYED_THROTTLE(10, "Currently have " << cloud.size() << " pending points"); 65 | 66 | pp_pub.publish(cloud); 67 | }); 68 | 69 | // I SURE DO LOVE MAKING FUNCTORS ALL OVER HTE GODDAMN PLACE 70 | class get_surfaces { 71 | public: 72 | SurfaceDetection &surface_detection; 73 | SurfaceVisualizationController progress; 74 | 75 | get_surfaces(SurfaceDetection &sd, ros::NodeHandle *nhp, std::string frame) 76 | : surface_detection(sd), progress{nhp, frame} { 77 | } 78 | 79 | bool go(SurfaceDetectionRequest &req, SurfaceDetectionResponse &resp) { 80 | auto center = GeometryPoseToEigenAffine3f(req.center); 81 | auto extents = GeometryVector3ToEigenVector3f(req.extents); 82 | 83 | progress.bounding_box(center, extents); 84 | resp = surface_detection.detect_surfaces_within(center, extents, progress); 85 | ROS_INFO_STREAM("Replying with " << resp.surfaces.size() << " surfaces"); 86 | return true; 87 | } 88 | }; 89 | get_surfaces getter(surface_detection, &n, target_frame); 90 | surface_detection.add_start_surface(discretization, start_surface_extent_x, start_surface_extent_y, getter.progress); 91 | // Make service provider 92 | auto srv = n.advertiseService("get_surfaces", &get_surfaces::go, &getter); 93 | 94 | ROS_INFO_STREAM("Setup complete!"); 95 | ros::spin(); 96 | 97 | return 0; 98 | } -------------------------------------------------------------------------------- /surface_msgs2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(surface_msgs2) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | pcl_msgs 9 | std_msgs 10 | shape_msgs 11 | geometry_msgs 12 | message_generation 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a run_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | add_message_files( 50 | FILES 51 | Surface.msg 52 | SurfaceStamped.msg 53 | Surfaces.msg 54 | ) 55 | 56 | ## Generate services in the 'srv' folder 57 | add_service_files( 58 | FILES 59 | SurfaceDetection.srv 60 | ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | generate_messages( 71 | DEPENDENCIES 72 | pcl_msgs 73 | std_msgs 74 | shape_msgs 75 | sensor_msgs 76 | geometry_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 run_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 you 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 | # INCLUDE_DIRS include 110 | # LIBRARIES surface_msgs 111 | # CATKIN_DEPENDS pcl_msgs std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | # include_directories(include) 122 | include_directories( 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(surface_msgs 128 | # src/${PROJECT_NAME}/surface_msgs.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(surface_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | # add_executable(surface_msgs_node src/surface_msgs_node.cpp) 138 | 139 | ## Add cmake target dependencies of the executable 140 | ## same as for the library above 141 | # add_dependencies(surface_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 142 | 143 | ## Specify libraries to link a library or executable target against 144 | # target_link_libraries(surface_msgs_node 145 | # ${catkin_LIBRARIES} 146 | # ) 147 | 148 | ############# 149 | ## Install ## 150 | ############# 151 | 152 | # all install targets should use catkin DESTINATION variables 153 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 154 | 155 | ## Mark executable scripts (Python etc.) for installation 156 | ## in contrast to setup.py, you can choose the destination 157 | # install(PROGRAMS 158 | # scripts/my_python_script 159 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark executables and/or libraries for installation 163 | # install(TARGETS surface_msgs surface_msgs_node 164 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 165 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark cpp header files for installation 170 | # install(DIRECTORY include/${PROJECT_NAME}/ 171 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 172 | # FILES_MATCHING PATTERN "*.h" 173 | # PATTERN ".svn" EXCLUDE 174 | # ) 175 | 176 | ## Mark other files for installation (e.g. launch and bag files, etc.) 177 | # install(FILES 178 | # # myfile1 179 | # # myfile2 180 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 181 | # ) 182 | 183 | ############# 184 | ## Testing ## 185 | ############# 186 | 187 | ## Add gtest based cpp test target and link libraries 188 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_surface_msgs.cpp) 189 | # if(TARGET ${PROJECT_NAME}-test) 190 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 191 | # endif() 192 | 193 | ## Add folders to be run by python nosetests 194 | # catkin_add_nosetests(test) 195 | -------------------------------------------------------------------------------- /surface_msgs2/msg/Surface.msg: -------------------------------------------------------------------------------- 1 | # A sequential numeric ID uniquely identifies the surface 2 | uint32 id 3 | 4 | # Each surface has a unique random color which helps to visuually identify it across different visualizations 5 | std_msgs/ColorRGBA color 6 | 7 | # Describes the plane using a plane equation in the for ax + by + cz + d = 0, where |||| = 1 8 | pcl_msgs/ModelCoefficients model 9 | 10 | # A point cloud containing all the points on the plane 11 | sensor_msgs/PointCloud2 inliers 12 | 13 | # The surface pose. The position is always in the surface plane (but not necessarily inside the polygon). The x and y 14 | # axes are always on the plane. The pose begins with the position in the center (mean) of the polygon and with the x 15 | # axis aligned to the polygon's largest dimension. When new area is added to the surface, the plane equation is re- 16 | # calculated and the pose is moved as little as possible while also remaining on the plane. Poses relative to the 17 | # surface pose will always be on the surface and have as high a probability as possible of remaining valid for their 18 | # original purpose 19 | geometry_msgs/Pose pose 20 | 21 | # A list of boundary points, where the largest gap between points is controlled by a parameter to the surface detection 22 | sensor_msgs/PointCloud2 boundary 23 | 24 | # The surface polygon, described as a list of simple polygons, each of which is an ordered list of indices into the 25 | # inliers point cloud. The first item in the list is the outer boundary, and all subsequent items describe holes. Every 26 | # hole polygon will be completely contained within the outer boundary, but it is possible for a hole to share a vertex 27 | # with the outer boundary. Both the outer polygon and all holes are indexed counter-clockwise with respect to the 28 | # plane normal. Unlike the boundary, there is no maximum distance between points (leading to simpler polygons) 29 | pcl_msgs/Vertices[] polygons 30 | 31 | # A triangle mesh describing the surface. The mesh is a thin (width controlled by a parameter to the surface detection) 32 | # extrusion of the surface polygon in the direction opposite the normal vector. 33 | shape_msgs/Mesh mesh 34 | -------------------------------------------------------------------------------- /surface_msgs2/msg/SurfaceStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | surface_msgs2/Surface surface -------------------------------------------------------------------------------- /surface_msgs2/msg/Surfaces.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | surface_msgs2/Surface[] surfaces -------------------------------------------------------------------------------- /surface_msgs2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | surface_msgs2 4 | 0.0.0 5 | Messages for use in surface extraction 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | pcl_msgs 44 | std_msgs 45 | message_generation 46 | pcl_msgs 47 | std_msgs 48 | message_runtime 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /surface_msgs2/srv/SurfaceDetection.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose center 2 | geometry_msgs/Vector3 extents 3 | --- 4 | surface_msgs2/Surface[] surfaces -------------------------------------------------------------------------------- /surface_types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(surface_types) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | set(CATKIN_PACKAGES 8 | pcl_conversions 9 | pcl_msgs 10 | pcl_ros 11 | std_msgs 12 | shape_msgs 13 | roscpp 14 | surface_msgs2 15 | eigen_conversions) 16 | find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGES}) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 21 | find_package(PCL 1.8 REQUIRED) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # surface_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if you package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | INCLUDE_DIRS include 111 | # LIBRARIES surface_types 112 | CATKIN_DEPENDS ${CATKIN_PACKAGES} 113 | # DEPENDS PCL boost 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | #set(CMAKE_BUILD_TYPE Debug) 120 | 121 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O3 -std=c++11 -Wall -Wextra -Wno-unused-parameter") 122 | # Flags to make Clang ignore directories that Catkin should have marked as system imports 123 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Xclang -isystem-prefix -Xclang pcl/") 124 | 125 | ## Specify additional locations of header files 126 | ## Your package locations should be listed before other locations 127 | # include_directories(include) 128 | include_directories( 129 | include 130 | ${catkin_INCLUDE_DIRS} 131 | ) 132 | #include_directories(SYSTEM 133 | # ${Boost_INCLUDE_DIRS} 134 | # ${PCL_INCLUDE_DIRS} 135 | # ${Eigen3_INCLUDE_DIRS} 136 | #) 137 | 138 | ## Declare a C++ library 139 | #add_library(surface_types 140 | # include/surface_types/pcl_shim/ModelCoefficients_Serialization.hpp 141 | # include/surface_types/pcl_shim/PointIndices_Serialization.hpp 142 | # include/surface_types/pcl_shim/PolygonMesh_Serialization.hpp 143 | # include/surface_types/pcl_shim/Vertices_Serialization.hpp 144 | # 145 | # include/surface_types/PointClusters.hpp 146 | # include/surface_types/Polygons.hpp 147 | # include/surface_types/Segment.hpp 148 | # include/surface_types/Surface.hpp 149 | # include/surface_types/SurfaceMesh.hpp 150 | # include/surface_types/SurfaceMeshStamped.hpp 151 | # include/surface_types/Surfaces.hpp 152 | # include/surface_types/SurfaceStamped.hpp 153 | # 154 | # include/surface_types/utils.hpp) 155 | #SET_TARGET_PROPERTIES(surface_types PROPERTIES LINKER_LANGUAGE CXX) 156 | #add_dependencies(surface_types ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 157 | #target_link_libraries(surface_types ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${PCL_LIBRARIES}) 158 | 159 | ############# 160 | ## Install ## 161 | ############# 162 | 163 | # all install targets should use catkin DESTINATION variables 164 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 165 | 166 | ## Mark executable scripts (Python etc.) for installation 167 | ## in contrast to setup.py, you can choose the destination 168 | # install(PROGRAMS 169 | # scripts/my_python_script 170 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark executables and/or libraries for installation 174 | # install(TARGETS surface_types surfaces_node 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | install(DIRECTORY include/${PROJECT_NAME}/ 182 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 184 | PATTERN ".svn" EXCLUDE 185 | ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_surfaces.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/Surface.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 2/15/16. 3 | // 4 | 5 | #ifndef SURFACE_MANAGER_SURFACE_HPP 6 | #define SURFACE_MANAGER_SURFACE_HPP 7 | 8 | // Contained types 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // Manual conversion to/from ROS 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | // ROS message serialization 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace surface_types { 28 | struct Surface { 29 | uint32_t id; 30 | std_msgs::ColorRGBA color; 31 | pcl::PointCloud inliers; 32 | pcl::ModelCoefficients model; 33 | Eigen::Affine3d pose; 34 | pcl::PointCloud boundary; 35 | std::vector polygons; 36 | shape_msgs::Mesh mesh; 37 | 38 | public: 39 | Surface() : id(0), color(), inliers(), model(), pose(), boundary(), polygons(), mesh() {} 40 | 41 | operator surface_msgs2::Surface() const { 42 | // Can only convert complete surfaces to ROS messages 43 | validate_complete(); 44 | 45 | surface_msgs2::Surface s_ros; 46 | 47 | // ID, color, model, pose 48 | s_ros.id = id; 49 | s_ros.color = color; 50 | pcl_conversions::fromPCL(model, s_ros.model); 51 | tf::poseEigenToMsg(pose, s_ros.pose); 52 | 53 | // Inliers 54 | pcl::PCLPointCloud2 inliers_pointcloud2; 55 | pcl::toPCLPointCloud2(inliers, inliers_pointcloud2); 56 | pcl_conversions::fromPCL(inliers_pointcloud2, s_ros.inliers); 57 | 58 | // Boundary 59 | pcl::PCLPointCloud2 boundary_pointcloud2; 60 | pcl::toPCLPointCloud2(boundary, boundary_pointcloud2); 61 | pcl_conversions::fromPCL(boundary_pointcloud2, s_ros.boundary); 62 | 63 | // Vertices 64 | s_ros.polygons.resize(polygons.size()); 65 | for (std::size_t i = 0; i < polygons.size(); i++) { 66 | pcl_conversions::fromPCL(polygons[i], s_ros.polygons[i]); 67 | } 68 | 69 | // Mesh 70 | s_ros.mesh = mesh; 71 | 72 | return s_ros; 73 | } 74 | 75 | void validate() const { 76 | if (is_complete()) { 77 | validate_complete(); // Reduntant, but keeping it in case I remove the check from is_complete 78 | } else { 79 | validate_partial(); 80 | } 81 | } 82 | 83 | void validate_complete() const { 84 | // ID and color technically have no invalid values 85 | assert(inliers.size() > 0 && "Attempted to validate a surface with no inliers"); 86 | assert(model.values.size() == 4 && 87 | "Attempted to validate a surface with an invalid model (to few or too many values)"); 88 | assert(std::abs(Eigen::Map(model.values.data()).norm() - 1) < 1e-3 && 89 | "Attempted to validate a surface with an invalid model (normal is not unit length)"); 90 | // pose can't be invalid (I think) 91 | 92 | assert(boundary.size() > 0 && "Attempted to validate a complete surface with no boundary"); 93 | assert(polygons.size() > 0 && "Attempted to validate a complete surface with no polygons"); 94 | assert(std::all_of(polygons.begin(), polygons.end(), [](pcl::Vertices v) { return v.vertices.size() > 0; }) && 95 | "Attempted to validate a complete surface with an empty polygon"); 96 | assert(mesh.vertices.size() > 0 && "Attempted to validate a complete surface with an empty mesh (no vertices)"); 97 | assert(mesh.triangles.size() > 0 && 98 | "Attempted to validate a complete surface with an empty mesh (no triangles)"); 99 | } 100 | 101 | void validate_partial() const { 102 | // ID and color technically have no invalid values 103 | assert(inliers.size() > 0 && "Attempted to validate a surface with no inliers"); 104 | assert(model.values.size() == 4 && 105 | "Attempted to validate a surface with an invalid model (to few or too many values)"); 106 | assert(std::abs(Eigen::Map(model.values.data()).norm() - 1) < 1e-3 && 107 | "Attempted to validate a surface with an invalid model (normal is not unit length)"); 108 | // pose can't be invalid (I think) 109 | 110 | assert(boundary.size() == 0 && "Attempted to validate a partial surface with non-empty boundary"); 111 | assert(polygons.size() == 0 && "Attempted to validate a partial surface with non-empty polygons"); 112 | assert(mesh.vertices.size() == 0 && "Attempted to validate a partial surface with non-empty mesh (vertices)"); 113 | assert(mesh.triangles.size() == 0 && "Attempted to validate a partial surface with non-empty mesh (triangles)"); 114 | } 115 | 116 | bool is_complete() const { 117 | // Use boundary.size as the indicator of completeness 118 | if (boundary.size() > 0) { 119 | validate_complete(); 120 | 121 | return true; 122 | } else { 123 | validate_partial(); 124 | 125 | return false; 126 | } 127 | } 128 | 129 | void clear_computed_values() { 130 | boundary.clear(); 131 | polygons.clear(); 132 | mesh = shape_msgs::Mesh(); 133 | } 134 | 135 | boost::format print_color() const { 136 | return boost::format("\x1b[38;2;%d;%d;%dm") % static_cast(color.r * 256) % 137 | static_cast(color.g * 256) % static_cast(color.b * 256); 138 | } 139 | 140 | typedef boost::shared_ptr<::surface_types::Surface> Ptr; 141 | typedef boost::shared_ptr<::surface_types::Surface const> ConstPtr; 142 | }; 143 | 144 | // struct Surface 145 | 146 | inline std::ostream &operator<<(std::ostream &s, const Surface &v) { 147 | s << "id: " << std::endl; 148 | s << " " << v.id; 149 | s << "color: " << std::endl; 150 | s << " " << v.color.r << ", " << v.color.g << ", " << v.color.b << ", " << v.color.a << std::endl; 151 | s << "model:" << std::endl; 152 | s << " " << v.model.values[0] << ", " << v.model.values[1]; 153 | s << ", " << v.model.values[2] << ", " << v.model.values[3] << std::endl; 154 | s << "inliers[" << v.inliers.size() << "]:" << std::endl; 155 | for (auto &inlier : v.inliers) 156 | s << " " << inlier << std::endl; 157 | s << "pose:" << std::endl; 158 | if (v.is_complete()) { 159 | // Print eigen transform as a matrix using .format() to set the indentation 160 | s << v.pose.matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", "\n", " ")) << std::endl; 161 | s << "boundary[" << v.boundary.size() << "]:" << std::endl; 162 | for (auto &inlier : v.boundary) 163 | s << " " << inlier << std::endl; 164 | s << "polygons[" << v.polygons.size() << "]:" << std::endl; 165 | for (auto &vertices : v.polygons) { 166 | s << " vertices[" << vertices.vertices.size() << "]:" << std::endl; 167 | for (auto &vertex : vertices.vertices) 168 | s << " " << vertex << std::endl; 169 | } 170 | s << "mesh:" << std::endl; 171 | s << " " << v.mesh << std::endl; // You're on your own for this one 172 | } 173 | return (s); 174 | } 175 | } 176 | 177 | namespace ros { 178 | namespace message_traits { 179 | template <> 180 | struct IsFixedSize : public FalseType {}; 181 | template <> 182 | struct IsSimple : public FalseType {}; 183 | template <> 184 | struct HasHeader : public FalseType {}; 185 | 186 | template <> 187 | struct MD5Sum { 188 | static const char *value() { return MD5Sum::value(); } 189 | 190 | static const char *value(const surface_types::Surface &m) { return MD5Sum::value(); } 191 | }; 192 | 193 | template <> 194 | struct DataType { 195 | static const char *value() { return DataType::value(); } 196 | 197 | static const char *value(const surface_types::Surface &m) { return DataType::value(); } 198 | }; 199 | 200 | template <> 201 | struct Definition { 202 | static const char *value() { return Definition::value(); } 203 | 204 | static const char *value(const surface_types::Surface &m) { return Definition::value(); } 205 | }; 206 | } // namespace message_traits 207 | 208 | namespace serialization { 209 | template <> 210 | struct Serializer { 211 | template 212 | inline static void write(Stream &stream, const surface_types::Surface &s) { 213 | // Only try to serialize complete surfaces 214 | s.validate_complete(); 215 | 216 | geometry_msgs::Pose geom_pose; 217 | tf::poseEigenToMsg(s.pose, geom_pose); 218 | 219 | stream.next(s.id); 220 | stream.next(s.color); 221 | stream.next(s.model); 222 | stream.next(s.inliers); 223 | stream.next(geom_pose); 224 | stream.next(s.boundary); 225 | stream.next(s.polygons); 226 | stream.next(s.mesh); 227 | } 228 | 229 | template 230 | inline static void read(Stream &stream, surface_types::Surface &s) { 231 | geometry_msgs::Pose geom_pose; 232 | 233 | stream.next(s.id); 234 | stream.next(s.color); 235 | stream.next(s.model); 236 | stream.next(s.inliers); 237 | stream.next(geom_pose); 238 | stream.next(s.boundary); 239 | stream.next(s.polygons); 240 | stream.next(s.mesh); 241 | 242 | tf::poseMsgToEigen(geom_pose, s.pose); 243 | } 244 | 245 | inline static uint32_t serializedLength(const surface_types::Surface &s) { 246 | geometry_msgs::Pose geom_pose; 247 | tf::poseEigenToMsg(s.pose, geom_pose); 248 | 249 | uint32_t size = 0; 250 | 251 | size += serializationLength(s.id); 252 | size += serializationLength(s.color); 253 | size += serializationLength(s.model); 254 | size += serializationLength(s.inliers); 255 | size += serializationLength(geom_pose); 256 | size += serializationLength(s.boundary); 257 | size += serializationLength(s.polygons); 258 | size += serializationLength(s.mesh); 259 | 260 | return size; 261 | } 262 | }; 263 | } // namespace serialization 264 | } // namespace ros 265 | 266 | #endif // SURFACE_MANAGER_SURFACE_HPP 267 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/SurfaceStamped.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 2/15/16. 3 | // 4 | 5 | #ifndef SURFACE_MANAGER_SURFACE_STAMPED_HPP 6 | #define SURFACE_MANAGER_SURFACE_STAMPED_HPP 7 | 8 | #include 9 | #include 10 | #include "Surface.hpp" 11 | #include 12 | 13 | namespace surface_types { 14 | 15 | // Defined similarly to pcl::PointIndices 16 | struct SurfaceStamped 17 | { 18 | SurfaceStamped() : header(), surface() 19 | {} 20 | 21 | ::pcl::PCLHeader header; 22 | ::surface_types::Surface surface; 23 | 24 | 25 | public: 26 | typedef boost::shared_ptr< ::surface_types::SurfaceStamped> Ptr; 27 | typedef boost::shared_ptr< ::surface_types::SurfaceStamped const> ConstPtr; 28 | }; // struct SurfaceStamped 29 | 30 | inline std::ostream& operator << (std::ostream& s, const ::surface_types::SurfaceStamped &v) 31 | { 32 | s << "header: " << std::endl; 33 | s << " " << v.header; 34 | s << "surface: " << std::endl; 35 | s << " " << v.surface; 36 | return (s); 37 | } 38 | } 39 | 40 | namespace ros 41 | { 42 | namespace message_traits 43 | { 44 | struct IsFixedSize : public FalseType {}; 45 | struct IsSimple : public FalseType {}; 46 | struct HasHeader : public FalseType {}; 47 | 48 | struct MD5Sum 49 | { 50 | static const char* value() 51 | { 52 | return MD5Sum::value(); 53 | } 54 | 55 | static const char* value(const surface_types::SurfaceStamped& m) 56 | { 57 | return MD5Sum::value(); 58 | } 59 | }; 60 | 61 | struct DataType 62 | { 63 | static const char* value() 64 | { 65 | return DataType::value(); 66 | } 67 | 68 | static const char* value(const surface_types::SurfaceStamped& m) 69 | { 70 | return DataType::value(); 71 | } 72 | }; 73 | 74 | struct Definition 75 | { 76 | static const char* value() 77 | { 78 | return Definition::value(); 79 | } 80 | 81 | static const char* value(const surface_types::SurfaceStamped& m) 82 | { 83 | return Definition::value(); 84 | } 85 | }; 86 | 87 | struct TimeStamp 88 | { 89 | static ros::Time* pointer(surface_types::SurfaceStamped &m) { 90 | header_.reset(new std_msgs::Header()); 91 | pcl_conversions::fromPCL(m.header, *(header_)); 92 | return &(header_->stamp); 93 | } 94 | static ros::Time const* pointer(const surface_types::SurfaceStamped& m) { 95 | header_const_.reset(new std_msgs::Header()); 96 | pcl_conversions::fromPCL(m.header, *(header_const_)); 97 | return &(header_const_->stamp); 98 | } 99 | static ros::Time value(const surface_types::SurfaceStamped& m) { 100 | return pcl_conversions::fromPCL(m.header).stamp; 101 | } 102 | private: 103 | static boost::shared_ptr header_; 104 | static boost::shared_ptr header_const_; 105 | }; 106 | 107 | } // namespace message_traits 108 | 109 | namespace serialization 110 | { 111 | struct Serializer 112 | { 113 | template 114 | inline static void write(Stream& stream, const surface_types::SurfaceStamped& item) 115 | { 116 | stream.next(item.header); 117 | stream.next(item.surface); 118 | } 119 | 120 | template 121 | inline static void read(Stream& stream, surface_types::SurfaceStamped& item) 122 | { 123 | stream.next(item.header); 124 | stream.next(item.surface); 125 | } 126 | 127 | inline static int32_t serializedLength(const surface_types::SurfaceStamped& item) 128 | { 129 | uint32_t size = 0; 130 | size += serializationLength(item.header); 131 | size += serializationLength(item.surface); 132 | return size; 133 | } 134 | 135 | }; 136 | } // namespace serialization 137 | 138 | } // namespace ros 139 | 140 | #endif //SURFACE_MANAGER_SURFACE_STAMPED_HPP 141 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/Surfaces.hpp: -------------------------------------------------------------------------------- 1 | #ifndef surface_types_HPP 2 | #define surface_types_HPP 3 | 4 | #include 5 | #include "Surface.hpp" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace surface_types { 12 | 13 | // Defined similarly to pcl::PointIndices 14 | struct Surfaces { 15 | Surfaces() : header(), surfaces() {} 16 | 17 | pcl::PCLHeader header; 18 | 19 | std::vector surfaces; 20 | 21 | public: 22 | void add_surface(const Surface &s) { 23 | s.validate(); 24 | 25 | surfaces.push_back(s); 26 | } 27 | 28 | void update_surface(const Surface &s) { 29 | s.validate(); 30 | 31 | auto posn = find_id(s.id); 32 | assert(posn != surfaces.end() && "Tried to update surface that isn't in this Surfaces object"); 33 | (*posn) = s; 34 | } 35 | 36 | std::vector::iterator find_id(uint32_t surface_id) { 37 | return std::find_if(surfaces.begin(), surfaces.end(), 38 | [&surface_id](const Surface &s) { return s.id == surface_id; }); 39 | } 40 | 41 | operator surface_msgs2::Surfaces() const { 42 | surface_msgs2::Surfaces ros_surfaces; 43 | 44 | pcl_conversions::fromPCL(header, ros_surfaces.header); 45 | ros_surfaces.surfaces.reserve(surfaces.size()); 46 | std::copy(surfaces.begin(), surfaces.end(), std::back_inserter(ros_surfaces.surfaces)); 47 | 48 | return ros_surfaces; 49 | } 50 | operator surface_msgs2::SurfaceDetectionResponse() const { 51 | surface_msgs2::SurfaceDetectionResponse ros_surfaces; 52 | 53 | ros_surfaces.surfaces.reserve(surfaces.size()); 54 | std::copy(surfaces.begin(), surfaces.end(), std::back_inserter(ros_surfaces.surfaces)); 55 | 56 | return ros_surfaces; 57 | } 58 | typedef boost::shared_ptr<::surface_types::Surfaces> Ptr; 59 | typedef boost::shared_ptr<::surface_types::Surfaces const> ConstPtr; 60 | }; // struct surface_types 61 | 62 | inline std::ostream &operator<<(std::ostream &s, const ::surface_types::Surfaces &v) { 63 | s << "header: " << std::endl; 64 | s << " " << v.header; 65 | s << "surfaces[]" << std::endl; 66 | for (size_t i = 0; i < v.surfaces.size(); ++i) { 67 | s << " surfaces[" << i << "]: "; 68 | s << " " << v.surfaces[i] << std::endl; 69 | } 70 | return (s); 71 | } 72 | } 73 | 74 | namespace ros { 75 | namespace message_traits { 76 | template <> 77 | struct IsFixedSize : public FalseType {}; 78 | // TODO: Determine if this actually is a simple message 79 | template <> 80 | struct IsSimple : public FalseType {}; 81 | template <> 82 | struct HasHeader : public FalseType {}; 83 | 84 | template <> 85 | struct MD5Sum { 86 | static const char *value() { return MD5Sum::value(); } 87 | 88 | static const char *value(const surface_types::Surfaces &m) { return MD5Sum::value(); } 89 | }; 90 | 91 | template <> 92 | struct DataType { 93 | static const char *value() { return DataType::value(); } 94 | 95 | static const char *value(const surface_types::Surfaces &m) { return DataType::value(); } 96 | }; 97 | 98 | template <> 99 | struct Definition { 100 | static const char *value() { return Definition::value(); } 101 | 102 | static const char *value(const surface_types::Surfaces &m) { return Definition::value(); } 103 | }; 104 | 105 | template <> 106 | struct TimeStamp { 107 | static ros::Time *pointer(surface_types::Surfaces &m) { 108 | header_.reset(new std_msgs::Header()); 109 | pcl_conversions::fromPCL(m.header, *(header_)); 110 | return &(header_->stamp); 111 | } 112 | static ros::Time const *pointer(const surface_types::Surfaces &m) { 113 | header_const_.reset(new std_msgs::Header()); 114 | pcl_conversions::fromPCL(m.header, *(header_const_)); 115 | return &(header_const_->stamp); 116 | } 117 | static ros::Time value(const surface_types::Surfaces &m) { return pcl_conversions::fromPCL(m.header).stamp; } 118 | 119 | private: 120 | static boost::shared_ptr header_; 121 | static boost::shared_ptr header_const_; 122 | }; 123 | } // namespace message_traits 124 | 125 | namespace serialization { 126 | template <> 127 | struct Serializer { 128 | template 129 | inline static void write(Stream &stream, const surface_types::Surfaces &item) { 130 | stream.next(item.header); 131 | stream.next(item.surfaces); 132 | } 133 | 134 | template 135 | inline static void read(Stream &stream, surface_types::Surfaces &item) { 136 | // Convert ROS header to PCL header 137 | std_msgs::Header ros_header; 138 | stream.next(ros_header); 139 | pcl_conversions::toPCL(ros_header, item.header); 140 | stream.next(item.surfaces); 141 | } 142 | 143 | inline static int32_t serializedLength(const surface_types::Surfaces &item) { 144 | uint32_t size = 0; 145 | size += serializationLength(item.header); 146 | size += serializationLength(item.surfaces); 147 | return size; 148 | } 149 | }; 150 | } // namespace serialization 151 | } // namespace ros 152 | 153 | #endif // surface_types_HPP 154 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/pcl_shim/ModelCoefficients_Serialization.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MODELCOEFFICIENTS_SERIALIZATION_H 2 | #define MODELCOEFFICIENTS_SERIALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros 9 | { 10 | namespace message_traits 11 | { 12 | template<> struct IsFixedSize : public FalseType {}; 13 | // TODO: Determine if this actually is a simple message 14 | template<> struct IsSimple : public FalseType {}; 15 | template<> struct HasHeader : public TrueType {}; 16 | 17 | template<> 18 | struct MD5Sum 19 | { 20 | static const char* value() 21 | { 22 | return MD5Sum::value(); 23 | } 24 | 25 | static const char* value(const pcl::ModelCoefficients& m) 26 | { 27 | return MD5Sum::value(); 28 | } 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return DataType::value(); 37 | } 38 | 39 | static const char* value(const pcl::ModelCoefficients& m) 40 | { 41 | return DataType::value(); 42 | } 43 | }; 44 | 45 | template<> 46 | struct Definition 47 | { 48 | static const char* value() 49 | { 50 | return Definition::value(); 51 | } 52 | 53 | static const char* value(const pcl::ModelCoefficients& m) 54 | { 55 | return Definition::value(); 56 | } 57 | }; 58 | 59 | 60 | template<> 61 | struct TimeStamp 62 | { 63 | static ros::Time* pointer(pcl::ModelCoefficients &m) { 64 | header_.reset(new std_msgs::Header()); 65 | pcl_conversions::fromPCL(m.header, *(header_)); 66 | return &(header_->stamp); 67 | } 68 | static ros::Time const* pointer(const pcl::ModelCoefficients& m) { 69 | header_const_.reset(new std_msgs::Header()); 70 | pcl_conversions::fromPCL(m.header, *(header_const_)); 71 | return &(header_const_->stamp); 72 | } 73 | static ros::Time value(const pcl::ModelCoefficients& m) { 74 | return pcl_conversions::fromPCL(m.header).stamp; 75 | } 76 | private: 77 | static boost::shared_ptr header_; 78 | static boost::shared_ptr header_const_; 79 | }; 80 | } // namespace message_traits 81 | 82 | namespace serialization 83 | { 84 | template<> 85 | struct Serializer 86 | { 87 | template 88 | inline static void write(Stream& stream, const pcl::ModelCoefficients& item) 89 | { 90 | stream.next(item.header); 91 | stream.next(item.values); 92 | } 93 | 94 | template 95 | inline static void read(Stream& stream, pcl::ModelCoefficients& item) 96 | { 97 | // Convert ROS header to PCL header 98 | std_msgs::Header ros_header; 99 | stream.next(ros_header); 100 | pcl_conversions::toPCL(ros_header, item.header); 101 | stream.next(item.values); 102 | } 103 | 104 | inline static int32_t serializedLength(const pcl::ModelCoefficients& item) 105 | { 106 | uint32_t size = 0; 107 | size += serializationLength(item.header); 108 | size += serializationLength(item.values); 109 | return size; 110 | } 111 | 112 | }; 113 | } // namespace serialization 114 | } // namespace ros 115 | 116 | #endif // MODELCOEFFICIENTS_SERIALIZATION_H 117 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/pcl_shim/PointIndices_Serialization.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POINT_INDICES_SERIALIZATION_H 2 | #define POINT_INDICES_SERIALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros 9 | { 10 | namespace message_traits 11 | { 12 | template<> struct IsFixedSize : public FalseType {}; 13 | // TODO: Determine if this actually is a simple message 14 | template<> struct IsSimple : public FalseType {}; 15 | template<> struct HasHeader : public TrueType {}; 16 | 17 | template<> 18 | struct MD5Sum 19 | { 20 | static const char* value() 21 | { 22 | return MD5Sum::value(); 23 | } 24 | 25 | static const char* value(const pcl::PointIndices& m) 26 | { 27 | return MD5Sum::value(); 28 | } 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return DataType::value(); 37 | } 38 | 39 | static const char* value(const pcl::PointIndices& m) 40 | { 41 | return DataType::value(); 42 | } 43 | }; 44 | 45 | template<> 46 | struct Definition 47 | { 48 | static const char* value() 49 | { 50 | return Definition::value(); 51 | } 52 | 53 | static const char* value(const pcl::PointIndices& m) 54 | { 55 | return Definition::value(); 56 | } 57 | }; 58 | 59 | 60 | template<> 61 | struct TimeStamp 62 | { 63 | static ros::Time* pointer(pcl::PointIndices &m) { 64 | header_.reset(new std_msgs::Header()); 65 | pcl_conversions::fromPCL(m.header, *(header_)); 66 | return &(header_->stamp); 67 | } 68 | static ros::Time const* pointer(const pcl::PointIndices& m) { 69 | header_const_.reset(new std_msgs::Header()); 70 | pcl_conversions::fromPCL(m.header, *(header_const_)); 71 | return &(header_const_->stamp); 72 | } 73 | static ros::Time value(const pcl::PointIndices& m) { 74 | return pcl_conversions::fromPCL(m.header).stamp; 75 | } 76 | private: 77 | static boost::shared_ptr header_; 78 | static boost::shared_ptr header_const_; 79 | }; 80 | } // namespace message_traits 81 | 82 | namespace serialization 83 | { 84 | template<> 85 | struct Serializer 86 | { 87 | template 88 | inline static void write(Stream& stream, const pcl::PointIndices& item) 89 | { 90 | stream.next(item.header); 91 | stream.next(item.indices); 92 | } 93 | 94 | template 95 | inline static void read(Stream& stream, pcl::PointIndices& item) 96 | { 97 | // Convert ROS header to PCL header 98 | std_msgs::Header ros_header; 99 | stream.next(ros_header); 100 | pcl_conversions::toPCL(ros_header, item.header); 101 | stream.next(item.indices); 102 | } 103 | 104 | inline static int32_t serializedLength(const pcl::PointIndices& item) 105 | { 106 | uint32_t size = 0; 107 | size += serializationLength(item.header); 108 | size += serializationLength(item.indices); 109 | return size; 110 | } 111 | 112 | }; 113 | } // namespace serialization 114 | } // namespace ros 115 | 116 | #endif // POINT_INDICES_SERIALIZATION_H 117 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/pcl_shim/PolygonMesh_Serialization.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POLYGONMESH_SERIALIZATION_H 2 | #define POLYGONMESH_SERIALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros 9 | { 10 | namespace message_traits 11 | { 12 | template<> struct IsFixedSize : public FalseType {}; 13 | // TODO: Determine if this actually is a simple message 14 | template<> struct IsSimple : public FalseType {}; 15 | template<> struct HasHeader : public TrueType {}; 16 | 17 | template<> 18 | struct MD5Sum 19 | { 20 | static const char* value() 21 | { 22 | return MD5Sum::value(); 23 | } 24 | 25 | static const char* value(const pcl::PolygonMesh& m) 26 | { 27 | return MD5Sum::value(); 28 | } 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return DataType::value(); 37 | } 38 | 39 | static const char* value(const pcl::PolygonMesh& m) 40 | { 41 | return DataType::value(); 42 | } 43 | }; 44 | 45 | template<> 46 | struct Definition 47 | { 48 | static const char* value() 49 | { 50 | return Definition::value(); 51 | } 52 | 53 | static const char* value(const pcl::PolygonMesh& m) 54 | { 55 | return Definition::value(); 56 | } 57 | }; 58 | 59 | 60 | template<> 61 | struct TimeStamp 62 | { 63 | static ros::Time* pointer(pcl::PolygonMesh &m) { 64 | header_.reset(new std_msgs::Header()); 65 | pcl_conversions::fromPCL(m.header, *(header_)); 66 | return &(header_->stamp); 67 | } 68 | static ros::Time const* pointer(const pcl::PolygonMesh& m) { 69 | header_const_.reset(new std_msgs::Header()); 70 | pcl_conversions::fromPCL(m.header, *(header_const_)); 71 | return &(header_const_->stamp); 72 | } 73 | static ros::Time value(const pcl::PolygonMesh& m) { 74 | return pcl_conversions::fromPCL(m.header).stamp; 75 | } 76 | private: 77 | static boost::shared_ptr header_; 78 | static boost::shared_ptr header_const_; 79 | }; 80 | } // namespace message_traits 81 | 82 | namespace serialization 83 | { 84 | template<> 85 | struct Serializer 86 | { 87 | template 88 | inline static void write(Stream& stream, const pcl::PolygonMesh& item) 89 | { 90 | stream.next(item.header); 91 | stream.next(item.cloud); 92 | stream.next(item.polygons); 93 | } 94 | 95 | template 96 | inline static void read(Stream& stream, pcl::PolygonMesh& item) 97 | { 98 | // Convert ROS header to PCL header 99 | std_msgs::Header ros_header; 100 | stream.next(ros_header); 101 | pcl_conversions::toPCL(ros_header, item.header); 102 | stream.next(item.cloud); 103 | stream.next(item.polygons); 104 | } 105 | 106 | inline static int32_t serializedLength(const pcl::PolygonMesh& item) 107 | { 108 | uint32_t size = 0; 109 | size += serializationLength(item.header); 110 | size += serializationLength(item.cloud); 111 | size += serializationLength(item.polygons); 112 | return size; 113 | } 114 | 115 | }; 116 | } // namespace serialization 117 | } // namespace ros 118 | 119 | #endif // POLYGONMESH_SERIALIZATION_H 120 | -------------------------------------------------------------------------------- /surface_types/include/surface_types/pcl_shim/Vertices_Serialization.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VERTICES_SERIALIZATION_H 2 | #define VERTICES_SERIALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ros 9 | { 10 | namespace message_traits 11 | { 12 | template<> struct IsFixedSize : public FalseType {}; 13 | // TODO: Determine if this actually is a simple message 14 | template<> struct IsSimple : public FalseType {}; 15 | template<> struct HasHeader : public FalseType {}; 16 | 17 | template<> 18 | struct MD5Sum 19 | { 20 | static const char* value() 21 | { 22 | return MD5Sum::value(); 23 | } 24 | 25 | static const char* value(const pcl::Vertices& m) 26 | { 27 | return MD5Sum::value(); 28 | } 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return DataType::value(); 37 | } 38 | 39 | static const char* value(const pcl::Vertices& m) 40 | { 41 | return DataType::value(); 42 | } 43 | }; 44 | 45 | template<> 46 | struct Definition 47 | { 48 | static const char* value() 49 | { 50 | return Definition::value(); 51 | } 52 | 53 | static const char* value(const pcl::Vertices& m) 54 | { 55 | return Definition::value(); 56 | } 57 | }; 58 | } // namespace message_traits 59 | 60 | 61 | namespace serialization 62 | { 63 | template<> 64 | struct Serializer 65 | { 66 | template 67 | inline static void write(Stream& stream, const pcl::Vertices& item) 68 | { 69 | stream.next(item.vertices); 70 | } 71 | 72 | template 73 | inline static void read(Stream& stream, pcl::Vertices& item) 74 | { 75 | stream.next(item.vertices); 76 | } 77 | 78 | inline static int32_t serializedLength(const pcl::Vertices& item) 79 | { 80 | uint32_t size = 0; 81 | size += serializationLength(item.vertices); 82 | return size; 83 | } 84 | 85 | }; 86 | } // namespace serialization 87 | } // namespace ros 88 | 89 | #endif // VERTICES_SERIALIZATION_H 90 | -------------------------------------------------------------------------------- /surface_types/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | surface_types 4 | 0.0.0 5 | The surface_types package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | pcl_conversions 44 | pcl_msgs 45 | pcl_ros 46 | std_msgs 47 | shape_msgs 48 | roscpp 49 | surface_msgs2 50 | eigen_conversions 51 | pcl_conversions 52 | pcl_msgs 53 | pcl_ros 54 | std_msgs 55 | shape_msgs 56 | roscpp 57 | surface_msgs2 58 | eigen_conversions 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /surface_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(surface_utils) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | set(CATKIN_PACKAGES 8 | roscpp 9 | pcl_ros 10 | pcl_msgs 11 | visualization_msgs 12 | geometry_msgs 13 | eigen_conversions 14 | ) 15 | find_package(catkin REQUIRED COMPONENTS 16 | ${CATKIN_PACKAGES} 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | find_package(cmake_modules) 21 | find_package(Boost 1.6 REQUIRED COMPONENTS system) 22 | find_package(PCL 1.8 REQUIRED) 23 | find_package(Eigen REQUIRED) 24 | 25 | ## System dependencies are found with CMake's conventions 26 | # find_package(Boost REQUIRED COMPONENTS system) 27 | 28 | 29 | ## Uncomment this if the package has a setup.py. This macro ensures 30 | ## modules and global scripts declared therein get installed 31 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 32 | # catkin_python_setup() 33 | 34 | ################################################ 35 | ## Declare ROS messages, services and actions ## 36 | ################################################ 37 | 38 | ## To declare and build messages, services or actions from within this 39 | ## package, follow these steps: 40 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 41 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 42 | ## * In the file package.xml: 43 | ## * add a build_depend tag for "message_generation" 44 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 45 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 46 | ## but can be declared for certainty nonetheless: 47 | ## * add a run_depend tag for "message_runtime" 48 | ## * In this file (CMakeLists.txt): 49 | ## * add "message_generation" and every package in MSG_DEP_SET to 50 | ## find_package(catkin REQUIRED COMPONENTS ...) 51 | ## * add "message_runtime" and every package in MSG_DEP_SET to 52 | ## catkin_package(CATKIN_DEPENDS ...) 53 | ## * uncomment the add_*_files sections below as needed 54 | ## and list every .msg/.srv/.action file to be processed 55 | ## * uncomment the generate_messages entry below 56 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 57 | 58 | ## Generate messages in the 'msg' folder 59 | # add_message_files( 60 | # FILES 61 | # Message1.msg 62 | # Message2.msg 63 | # ) 64 | 65 | ## Generate services in the 'srv' folder 66 | # add_service_files( 67 | # FILES 68 | # Service1.srv 69 | # Service2.srv 70 | # ) 71 | 72 | ## Generate actions in the 'action' folder 73 | # add_action_files( 74 | # FILES 75 | # Action1.action 76 | # Action2.action 77 | # ) 78 | 79 | ## Generate added messages and services with any dependencies listed here 80 | # generate_messages( 81 | # DEPENDENCIES 82 | # std_msgs # Or other packages containing msgs 83 | # ) 84 | 85 | ################################################ 86 | ## Declare ROS dynamic reconfigure parameters ## 87 | ################################################ 88 | 89 | ## To declare and build dynamic reconfigure parameters within this 90 | ## package, follow these steps: 91 | ## * In the file package.xml: 92 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 93 | ## * In this file (CMakeLists.txt): 94 | ## * add "dynamic_reconfigure" to 95 | ## find_package(catkin REQUIRED COMPONENTS ...) 96 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 97 | ## and list every .cfg file to be processed 98 | 99 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 100 | # generate_dynamic_reconfigure_options( 101 | # cfg/DynReconf1.cfg 102 | # cfg/DynReconf2.cfg 103 | # ) 104 | 105 | ################################### 106 | ## catkin specific configuration ## 107 | ################################### 108 | ## The catkin_package macro generates cmake config files for your package 109 | ## Declare things to be passed to dependent projects 110 | ## INCLUDE_DIRS: uncomment this if you package contains header files 111 | ## LIBRARIES: libraries you create in this project that dependent projects also need 112 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 113 | ## DEPENDS: system dependencies of this project that dependent projects also need 114 | catkin_package( 115 | INCLUDE_DIRS include 116 | # LIBRARIES surface_utils 117 | CATKIN_DEPENDS ${CATKIN_PACKAGES} 118 | DEPENDS PCL Eigen 119 | ) 120 | 121 | ########### 122 | ## Build ## 123 | ########### 124 | 125 | ## Specify additional locations of header files 126 | ## Your package locations should be listed before other locations 127 | include_directories( 128 | include 129 | ${catkin_INCLUDE_DIRS} 130 | ) 131 | 132 | ## Declare a C++ library 133 | # add_library(surface_utils 134 | # src/${PROJECT_NAME}/surface_utils.cpp 135 | # ) 136 | 137 | ## Add cmake target dependencies of the library 138 | ## as an example, code may need to be generated before libraries 139 | ## either from message generation or dynamic reconfigure 140 | # add_dependencies(surface_utils ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 141 | 142 | ## Declare a C++ executable 143 | # add_executable(surface_utils_node src/surface_utils_node.cpp) 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(surface_utils_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(surface_utils_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS surface_utils surface_utils_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | install(DIRECTORY include/${PROJECT_NAME}/ 177 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 179 | PATTERN ".svn" EXCLUDE 180 | ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_surface_utils.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/SurfaceVisualizationController.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #ifndef PROJECT_PROGRESSLISTENER_HPP 6 | #define PROJECT_PROGRESSLISTENER_HPP 7 | 8 | // System includes 9 | #include 10 | #include 11 | 12 | // ROS includes 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // PCL Includes 21 | // pcl_ros/point_cloud.h enables publishing topics using PCL types 22 | #include 23 | 24 | // Local includes 25 | #include 26 | 27 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Affine3f) 28 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Affine3d) 29 | 30 | class SurfaceVisualizationController { 31 | typedef std::map PubMap; 32 | typedef std::shared_ptr NHPtr; 33 | 34 | public: 35 | // Use this constructor to disable SurfaceVisualizationController 36 | SurfaceVisualizationController() : nh_ptr_(nullptr), frame_(""), pubs_(nullptr) {} 37 | 38 | // Use this constructor to use SurfaceVisualizationController 39 | SurfaceVisualizationController(ros::NodeHandle *np, std::string frame) 40 | : nh_ptr_(std::make_shared(np)), frame_(frame), pubs_(std::make_shared()) {} 41 | 42 | // No copies allowed 43 | SurfaceVisualizationController(const SurfaceVisualizationController& other) = delete; 44 | SurfaceVisualizationController& operator=(SurfaceVisualizationController const&) = delete; 45 | 46 | template 47 | void with_publisher(std::string name, Func function) const { 48 | if (!nh_ptr_) return; 49 | 50 | std::map::iterator pub_it = pubs_->find(name); 51 | 52 | if (pub_it == pubs_->end()) { 53 | pub_it = pubs_->insert(std::make_pair(name, (*nh_ptr_)->advertise(name, 10))).first; 54 | } 55 | 56 | // Publishers are internally reference counted, making a copy here and on the next line is fine 57 | ros::Publisher pub = pub_it->second; 58 | 59 | // Lambda needs to be thread-safe (capture everything by value or by shared ptr which is captured by value) 60 | std::thread([name, function, pub]() { 61 | ros::Rate r(2); // 2Hz -> 0.5 seconds apart 62 | ros::Time stop_time = ros::Time::now() + ros::Duration(10); 63 | while (pub.getNumSubscribers() == 0 && ros::Time::now() < stop_time) { 64 | r.sleep(); 65 | } 66 | 67 | function(pub); 68 | 69 | }).detach(); 70 | } 71 | 72 | template 73 | void with_marker_publisher(Func function) const { 74 | with_publisher("surface_markers", function); 75 | } 76 | 77 | template 78 | void points(std::string name, const typename pcl::PointCloud::Ptr &points) const { 79 | with_publisher>(name, [=](const ros::Publisher &pub) { 80 | points->header.frame_id = frame_; 81 | pub.publish(points); 82 | }); 83 | } 84 | 85 | template 86 | void pair(std::string name, typename std::pair, pcl::PointIndices> pair) const { 87 | if (!nh_ptr_) return; 88 | 89 | auto points = boost::make_shared>(pair.first, pair.second.indices); 90 | this->points(name, points); 91 | } 92 | 93 | void normal_vectors(std::string name, pcl::PointCloud::Ptr &normals_cloud) const { 94 | with_publisher(name, [=](const ros::Publisher &pub) { 95 | geometry_msgs::PoseArray poses; 96 | poses.header.frame_id = frame_; 97 | 98 | Eigen::Affine3d ztf(Eigen::AngleAxisd(-M_PI / 2., Eigen::Vector3d::UnitY())); 99 | 100 | for (auto &pt : *normals_cloud) { 101 | geometry_msgs::Pose pose; 102 | geometry_msgs::Quaternion msg; 103 | 104 | // extracting surface normals 105 | tf::Vector3 axis_vector(pt.normal[0], pt.normal[1], pt.normal[2]); 106 | tf::Vector3 up_vector(0.0, 0.0, 1.0); 107 | 108 | tf::Vector3 right_vector = axis_vector.cross(up_vector); 109 | right_vector.normalized(); 110 | tf::Quaternion q(right_vector, -1.0 * acos(axis_vector.dot(up_vector))); 111 | q.normalize(); 112 | tf::quaternionTFToMsg(q, msg); 113 | 114 | // adding pose to pose array 115 | pose.position.x = pt.x; 116 | pose.position.y = pt.y; 117 | pose.position.z = pt.z; 118 | pose.orientation = msg; 119 | 120 | // PoseArrays always have the arrow on the x axis, but I want them on the z axis, so 121 | // transform each pose 122 | // This definitely duplicates some work, but whatever 123 | geometry_msgs::Pose result; 124 | Eigen::Affine3d tf; 125 | tf::poseMsgToEigen(pose, tf); 126 | tf::poseEigenToMsg(tf * ztf, result); 127 | 128 | poses.poses.push_back(result); 129 | } 130 | 131 | pub.publish(poses); 132 | }); 133 | } 134 | 135 | void vector(std::string name, Eigen::Vector3d direction, Eigen::Vector3d origin) const { 136 | geometry_msgs::Pose p; 137 | tf::pointEigenToMsg(origin, p.position); 138 | // Rviz's pose display puts an arrow on the x axis, so use the x axis to compute quaternion 139 | const auto q = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), direction); 140 | tf::quaternionEigenToMsg(q, p.orientation); 141 | pose(name, p); 142 | } 143 | 144 | void pose(std::string name, const geometry_msgs::PoseStamped &p) const { 145 | with_publisher(name, [=](const ros::Publisher &pub) { pub.publish(p); }); 146 | } 147 | 148 | void pose(std::string name, const geometry_msgs::Pose &p) const { 149 | geometry_msgs::PoseStamped ps; 150 | ps.header.frame_id = frame_; 151 | ps.pose = p; 152 | this->pose(name, ps); 153 | } 154 | 155 | void pose(std::string name, const Eigen::Affine3d &e) const { 156 | geometry_msgs::Pose p; 157 | tf::poseEigenToMsg(e, p); 158 | this->pose(name, p); 159 | } 160 | 161 | void pose(std::string name, const Eigen::Affine3f &e) const { 162 | geometry_msgs::Pose p; 163 | tf::poseEigenToMsg(e.cast(), p); 164 | this->pose(name, p); 165 | } 166 | 167 | void poses(std::string name, std::vector &p) const { 168 | with_publisher(name, [=](const ros::Publisher &pub) { 169 | geometry_msgs::PoseArray ps; 170 | ps.header.frame_id = frame_; 171 | // ps.poses = p; 172 | ps.poses.reserve(p.size()); 173 | 174 | // PoseArrays always have the arrow on the x axis, but I want them on the z axis, so transform each pose 175 | Eigen::Affine3d ztf(Eigen::AngleAxisd(-M_PI / 2., Eigen::Vector3d::UnitY())); 176 | std::transform(p.begin(), p.end(), std::back_inserter(ps.poses), [&ztf](const geometry_msgs::Pose &pose) { 177 | geometry_msgs::Pose result; 178 | Eigen::Affine3d tf; 179 | tf::poseMsgToEigen(pose, tf); 180 | tf::poseEigenToMsg(tf * ztf, result); 181 | return result; 182 | }); 183 | pub.publish(ps); 184 | }); 185 | } 186 | 187 | void poses(std::string name, std::vector &e) const { 188 | std::vector ps; 189 | ps.resize(e.size()); 190 | for (std::size_t i = 0; i < e.size(); i++) 191 | tf::poseEigenToMsg(e[i], ps[i]); 192 | this->poses(name, ps); 193 | } 194 | 195 | void poses(std::string name, std::vector &e) const { 196 | std::vector ps; 197 | ps.resize(e.size()); 198 | for (std::size_t i = 0; i < e.size(); i++) 199 | tf::poseEigenToMsg(e[i].cast(), ps[i]); 200 | this->poses(name, ps); 201 | } 202 | 203 | void polygons(std::string name, surface_types::Surface &surface) const { 204 | with_marker_publisher([=](const ros::Publisher &pub) { 205 | visualization_msgs::Marker m; 206 | m.header.frame_id = frame_; 207 | m.header.stamp = ros::Time(); 208 | m.ns = name; 209 | m.id = surface.id; 210 | m.type = visualization_msgs::Marker::LINE_LIST; 211 | m.action = visualization_msgs::Marker::ADD; 212 | // m.pose not needed 213 | m.scale.x = 0.005; 214 | // m.scale otherwise not needed 215 | m.color = surface.color; 216 | for (auto polygon : surface.polygons) { 217 | auto first = m.points.size(); 218 | // First add two copies of each point 219 | for (auto index : polygon.vertices) { 220 | geometry_msgs::Point p; 221 | p.x = surface.inliers[index].x; 222 | p.y = surface.inliers[index].y; 223 | p.z = surface.inliers[index].z; 224 | m.points.push_back(p); 225 | m.points.push_back(p); 226 | } 227 | // Then rotate all the new points 228 | std::rotate(m.points.begin() + first, m.points.begin() + first + 1, m.points.end()); 229 | } 230 | 231 | pub.publish(m); 232 | 233 | }); 234 | } 235 | 236 | void mesh(std::string name, surface_types::Surface &surface) const { 237 | with_marker_publisher([=](const ros::Publisher &pub) { 238 | visualization_msgs::Marker marker; 239 | marker.header.stamp = ros::Time::now(); 240 | marker.header.frame_id = frame_; 241 | marker.ns = name; 242 | marker.id = surface.id; 243 | marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 244 | marker.action = visualization_msgs::Marker::MODIFY; 245 | // perimeter.pose not needed 246 | marker.scale.x = 1; 247 | marker.scale.y = 1; 248 | marker.scale.z = 1; 249 | marker.color = surface.color; 250 | 251 | marker.points.reserve(surface.mesh.triangles.size() * 3); 252 | for (const shape_msgs::MeshTriangle &triangle : surface.mesh.triangles) { 253 | marker.points.push_back(surface.mesh.vertices[triangle.vertex_indices[0]]); 254 | marker.points.push_back(surface.mesh.vertices[triangle.vertex_indices[1]]); 255 | marker.points.push_back(surface.mesh.vertices[triangle.vertex_indices[2]]); 256 | } 257 | 258 | pub.publish(marker); 259 | }); 260 | } 261 | 262 | void plane_normal(std::string name, surface_types::Surface &surface) const { 263 | with_marker_publisher([=](const ros::Publisher &pub) { 264 | visualization_msgs::Marker marker; 265 | marker.header.stamp = ros::Time::now(); 266 | marker.header.frame_id = frame_; 267 | marker.ns = name; 268 | marker.id = surface.id; 269 | marker.type = visualization_msgs::Marker::ARROW; 270 | marker.action = visualization_msgs::Marker::MODIFY; 271 | // perimeter.pose not needed 272 | marker.scale.x = 0.05; // Shaft diameter 273 | marker.scale.y = 0.1; // Head diameter 274 | marker.scale.z = 0; // Head length, or 0 for default 275 | marker.color = surface.color; 276 | 277 | marker.points.resize(2); 278 | // Start point: d distance along the normal direction 279 | marker.points.front().x = -surface.model.values[3] * surface.model.values[0]; 280 | marker.points.front().y = -surface.model.values[3] * surface.model.values[1]; 281 | marker.points.front().z = -surface.model.values[3] * surface.model.values[2]; 282 | // End point: d + 0.1 distance along the normal direction 283 | marker.points.back().x = (-surface.model.values[3] + 0.5) * surface.model.values[0]; 284 | marker.points.back().y = (-surface.model.values[3] + 0.5) * surface.model.values[1]; 285 | marker.points.back().z = (-surface.model.values[3] + 0.5) * surface.model.values[2]; 286 | 287 | pub.publish(marker); 288 | }); 289 | } 290 | 291 | void pose(std::string name, surface_types::Surface &surface) const { 292 | with_marker_publisher([=](const ros::Publisher &pub) { 293 | visualization_msgs::Marker marker; 294 | marker.header.stamp = ros::Time::now(); 295 | marker.header.frame_id = frame_; 296 | marker.ns = name; 297 | marker.id = surface.id; 298 | marker.type = visualization_msgs::Marker::ARROW; 299 | marker.action = visualization_msgs::Marker::MODIFY; 300 | // perimeter.pose not needed 301 | marker.scale.x = 0.05; // Shaft diameter 302 | marker.scale.y = 0.1; // Head diameter 303 | marker.scale.z = 0; // Head length, or 0 for default 304 | marker.color = surface.color; 305 | 306 | marker.points.resize(2); 307 | // Start point: d distance along the normal direction 308 | marker.points.front().x = surface.pose.translation()[0]; 309 | marker.points.front().y = surface.pose.translation()[1]; 310 | marker.points.front().z = surface.pose.translation()[2]; 311 | // End point: d + 0.1 distance along the normal direction 312 | auto unit = surface.pose * Eigen::Vector3d(0, 0, 0.5); 313 | marker.points.back().x = unit[0]; 314 | marker.points.back().y = unit[1]; 315 | marker.points.back().z = unit[2]; 316 | 317 | pub.publish(marker); 318 | 319 | // Publish the x axis in red 320 | marker.ns = name + "_x"; 321 | marker.scale.x /= 2; 322 | marker.scale.y /= 2; 323 | marker.scale.z /= 2; 324 | marker.color.r = 1; 325 | marker.color.g = 0; 326 | marker.color.b = 0; 327 | marker.color.a = 1; 328 | // End point: d + 0.1 distance along the normal direction 329 | auto unitx = surface.pose * Eigen::Vector3d(0.25, 0, 0); 330 | marker.points.back().x = unitx[0]; 331 | marker.points.back().y = unitx[1]; 332 | marker.points.back().z = unitx[2]; 333 | 334 | pub.publish(marker); 335 | 336 | // Publish the y axis in red 337 | marker.ns = name + "_y"; 338 | marker.color.r = 0; 339 | marker.color.g = 1; 340 | marker.color.b = 0; 341 | marker.color.a = 1; 342 | // End point: d + 0.1 distance along the normal direction 343 | auto unity = surface.pose * Eigen::Vector3d(0, 0.25, 0); 344 | marker.points.back().x = unity[0]; 345 | marker.points.back().y = unity[1]; 346 | marker.points.back().z = unity[2]; 347 | 348 | pub.publish(marker); 349 | }); 350 | } 351 | 352 | void bounding_box(const Eigen::Affine3f ¢er, const Eigen::Vector3f &extents) { 353 | with_marker_publisher([=](const ros::Publisher &pub) { 354 | visualization_msgs::Marker m; 355 | m.header.frame_id = frame_; 356 | m.header.stamp = ros::Time(); 357 | m.ns = "bounding_box"; 358 | m.id = 0; 359 | m.type = visualization_msgs::Marker::LINE_LIST; 360 | m.action = visualization_msgs::Marker::ADD; 361 | tf::poseEigenToMsg(center.cast(), m.pose); 362 | m.scale.x = 0.01; 363 | // m.scale otherwise not needed 364 | m.color.r = 1; 365 | m.color.g = 1; 366 | m.color.b = 1; 367 | m.color.a = 1; 368 | 369 | geometry_msgs::Point pt; 370 | pt.x = extents[0]; 371 | pt.y = extents[1]; 372 | pt.z = extents[2]; 373 | 374 | // Top 4 lines 375 | m.points.push_back(pt); 376 | pt.y *= -1; 377 | m.points.push_back(pt); 378 | m.points.push_back(pt); 379 | pt.x *= -1; 380 | m.points.push_back(pt); 381 | m.points.push_back(pt); 382 | pt.y *= -1; 383 | m.points.push_back(pt); 384 | m.points.push_back(pt); 385 | pt.x *= -1; 386 | m.points.push_back(pt); 387 | 388 | // Bottom 4 lines 389 | pt.z *= -1; 390 | m.points.push_back(pt); 391 | pt.y *= -1; 392 | m.points.push_back(pt); 393 | m.points.push_back(pt); 394 | pt.x *= -1; 395 | m.points.push_back(pt); 396 | m.points.push_back(pt); 397 | pt.y *= -1; 398 | m.points.push_back(pt); 399 | m.points.push_back(pt); 400 | pt.x *= -1; 401 | m.points.push_back(pt); 402 | 403 | // Side 4 lines 404 | m.points.push_back(pt); 405 | pt.z *= -1; 406 | m.points.push_back(pt); 407 | pt.y *= -1; 408 | m.points.push_back(pt); 409 | pt.z *= -1; 410 | m.points.push_back(pt); 411 | pt.x *= -1; 412 | m.points.push_back(pt); 413 | pt.z *= -1; 414 | m.points.push_back(pt); 415 | pt.y *= -1; 416 | m.points.push_back(pt); 417 | pt.z *= -1; 418 | m.points.push_back(pt); 419 | 420 | pub.publish(m); 421 | }); 422 | } 423 | 424 | void marker(const visualization_msgs::Marker &marker) const { 425 | with_marker_publisher([=](const ros::Publisher &pub) { 426 | visualization_msgs::Marker m = marker; 427 | m.header.stamp = ros::Time::now(); 428 | m.header.frame_id = frame_; 429 | 430 | pub.publish(m); 431 | }); 432 | } 433 | 434 | void remove_marker(std::string ns, int32_t id) const { 435 | with_marker_publisher([=](const ros::Publisher &pub) { 436 | visualization_msgs::Marker dm; 437 | dm.action = visualization_msgs::Marker::DELETE; 438 | dm.ns = ns; 439 | dm.id = id; 440 | 441 | pub.publish(dm); 442 | }); 443 | } 444 | 445 | protected: 446 | NHPtr nh_ptr_; 447 | std::string frame_; 448 | mutable std::shared_ptr pubs_; 449 | }; 450 | 451 | #endif // PROJECT_PROGRESSLISTENER_HPP 452 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/color_generator.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/22/16. 3 | // 4 | 5 | #ifndef PROJECT_RANDOM_COLORS_HPP 6 | #define PROJECT_RANDOM_COLORS_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace random_colors { 12 | // Generate random colors with method described here: 13 | // http://martankerl.com/2009/12/09/how-to-create-random-colors-programmatically/ 14 | class color_generator { 15 | static constexpr double golden_ratio_conjugate = 0.618033988749895; 16 | 17 | double h; 18 | const double s; 19 | const double v; 20 | 21 | public: 22 | color_generator(double s, double v) : s(s), v(v) { 23 | std::uniform_real_distribution unif(0., 1.); 24 | std::random_device rand_dev; 25 | std::mt19937 rand_engine(rand_dev()); 26 | 27 | h = unif(rand_engine); 28 | } 29 | 30 | std::tuple hsv() { 31 | h += golden_ratio_conjugate; 32 | if (h > 1) h -= 1; // Equivalent to h %= 1 33 | 34 | return std::make_tuple(h, s, v); 35 | } 36 | 37 | std::tuple repeat_hsv() const { 38 | return std::make_tuple(h, s, v); 39 | } 40 | 41 | std::tuple rgb() { return hsv_to_rgb(hsv()); } 42 | 43 | std::tuple repeat_rgb() const { return hsv_to_rgb(repeat_hsv()); } 44 | 45 | std::tuple hsv_to_rgb(std::tuple hsv) const { 46 | double h, s, v, r, g, b; 47 | std::tie(h, s, v) = hsv; 48 | h *= 360; // Convert (0. < h < 1.) to (0. < h < 360.) 49 | 50 | double hh, p, q, t, ff; 51 | long i; 52 | 53 | if (s <= 0.0) { // Greyscale 54 | r = v; 55 | g = v; 56 | b = v; 57 | return std::make_tuple(r, g, b); 58 | } 59 | 60 | hh = h; 61 | if (hh >= 360.0) hh = 0.0; 62 | hh /= 60.0; 63 | i = (long)hh; 64 | ff = hh - i; 65 | p = v * (1.0 - s); 66 | q = v * (1.0 - (s * ff)); 67 | t = v * (1.0 - (s * (1.0 - ff))); 68 | 69 | switch (i) { 70 | case 0: 71 | r = v; 72 | g = t; 73 | b = p; 74 | break; 75 | case 1: 76 | r = q; 77 | g = v; 78 | b = p; 79 | break; 80 | case 2: 81 | r = p; 82 | g = v; 83 | b = t; 84 | break; 85 | 86 | case 3: 87 | r = p; 88 | g = q; 89 | b = v; 90 | break; 91 | case 4: 92 | r = t; 93 | g = p; 94 | b = v; 95 | break; 96 | case 5: 97 | default: 98 | r = v; 99 | g = p; 100 | b = q; 101 | break; 102 | } 103 | 104 | return std::make_tuple(r, g, b); 105 | } 106 | }; 107 | } 108 | 109 | #endif // PROJECT_RANDOM_COLORS_HPP 110 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/conversions.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/20/16. 3 | // 4 | 5 | #ifndef PROJECT_CONVERSIONS_HPP 6 | #define PROJECT_CONVERSIONS_HPP 7 | 8 | #include 9 | 10 | namespace surface_utils { 11 | 12 | 13 | pcl::PointXYZL toLabeledPoint(const pcl::PointXYZ &pt) { 14 | pcl::PointXYZL lpt; 15 | lpt.x = pt.x; 16 | lpt.y = pt.y; 17 | lpt.z = pt.z; 18 | return lpt; 19 | } 20 | 21 | } 22 | 23 | #endif //PROJECT_CONVERSIONS_HPP 24 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/eigen_helpers_conversions.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 9/7/16. 3 | // 4 | 5 | #ifndef PROJECT_EIGEN_HELPERS_CONVERSIONS_HPP_H 6 | #define PROJECT_EIGEN_HELPERS_CONVERSIONS_HPP_H 7 | 8 | namespace EigenHelpersConversions { 9 | 10 | 11 | inline Eigen::Affine3f GeometryPoseToEigenAffine3f(const geometry_msgs::Pose& pose) 12 | { 13 | Eigen::Translation3f trans(pose.position.x, pose.position.y, pose.position.z); 14 | Eigen::Quaternionf quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); 15 | Eigen::Affine3f eigen_pose = trans * quat; 16 | return eigen_pose; 17 | } 18 | 19 | inline Eigen::Vector3f GeometryVector3ToEigenVector3f(const geometry_msgs::Vector3& vector) 20 | { 21 | Eigen::Vector3f eigen_vector(vector.x, vector.y, vector.z); 22 | return eigen_vector; 23 | } 24 | } 25 | 26 | #endif //PROJECT_EIGEN_HELPERS_CONVERSIONS_HPP_H 27 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/geom_utils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/29/16. 3 | // 4 | 5 | #ifndef PROJECT_GEOM_UTILS_HPP 6 | #define PROJECT_GEOM_UTILS_HPP 7 | 8 | #include 9 | 10 | namespace surface_geom_utils { 11 | using namespace surface_types; 12 | 13 | float point_sqr_dist(float x1, float y1, float x2, float y2) { 14 | float dx = x1 - x2; 15 | float dy = y1 - y2; 16 | return dx * dx + dy * dy; 17 | } 18 | 19 | template 20 | bool is_xy_in_tiling(const Surface &surface, const PointCloud &pts, float x, float y, double dist) { 21 | auto sqr_dist = dist * dist; 22 | // For each (x, y) pair, skip if it's outside, or if it's within perpendicular_distance_ of any edge 23 | bool is_inside = false; 24 | // PiP algorithm: count segments that cross the ray from the test point along the +x axis 25 | // Inside if there is an odd number, outside if there is an even number 26 | for (auto &polygon : surface.polygons) { 27 | auto prev_vertex = polygon.vertices.back(); 28 | for (auto &vertex : polygon.vertices) { 29 | if (prev_vertex == vertex) continue; // This way it handles explicitly and implicitly closed polygons 30 | 31 | const auto v = pts[prev_vertex], w = pts[vertex]; 32 | // If segment is too close to the line, skip it 33 | // First, the easy case: within the given distance of the endpoints 34 | if (point_sqr_dist(x, y, v.x, v.y) < sqr_dist || point_sqr_dist(x, y, w.x, w.y) < sqr_dist) { 35 | return false; 36 | } 37 | // The actual check 38 | const auto t = ((x - v.x) * (w.x - v.x) + (y - v.y) * (w.y - v.y)) / point_sqr_dist(v.x, v.y, w.x, w.y); 39 | // point was inside the segment (if outside, it was either caught by the first case or is OK) 40 | if (0 < t && t < 1) { 41 | // proj is the closest point on the segment to the query point 42 | const auto proj_x = v.x + t * (w.x - v.x); 43 | const auto proj_y = v.y + t * (w.y - v.y); 44 | if (point_sqr_dist(x, y, proj_x, proj_y) < sqr_dist) { 45 | return false; 46 | } 47 | } 48 | 49 | // Point-in-polygon -- code from 50 | // http://bbs.dartmouth.edu/~fangq/MATH/download/source/Determining%20if%20a%20point%20lies%20on%20the%20interior%20of%20a%20polygon.htm 51 | if ((((v.y <= y) && (y < w.y)) || ((w.y <= y) && (y < v.y))) && 52 | (x < (w.x - v.x) * (y - v.y) / (w.y - v.y) + v.x)) { 53 | is_inside = !is_inside; 54 | } 55 | 56 | prev_vertex = vertex; 57 | } 58 | } 59 | 60 | return is_inside; 61 | } 62 | } 63 | 64 | #endif // PROJECT_GEOM_UTILS_HPP 65 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/pcl_utils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 3/16/16. 3 | // 4 | 5 | #ifndef SURFACES_UTILS_HPP 6 | #define SURFACES_UTILS_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace surfaces_pcl_utils { 27 | struct PCLHeaderHashNoSeq { 28 | std::size_t operator()(const pcl::PCLHeader &h) const { 29 | std::size_t seed = 0; 30 | boost::hash_combine(seed, h.frame_id); 31 | boost::hash_combine(seed, h.stamp); 32 | return seed; 33 | }; 34 | }; 35 | 36 | inline pcl::SacModel sacModelFromConfigInt(int config_model_type) { 37 | switch (config_model_type) { 38 | case 0: 39 | return pcl::SACMODEL_PLANE; 40 | case 1: 41 | return pcl::SACMODEL_LINE; 42 | case 2: 43 | return pcl::SACMODEL_CIRCLE2D; 44 | case 3: 45 | return pcl::SACMODEL_CIRCLE3D; 46 | case 4: 47 | return pcl::SACMODEL_SPHERE; 48 | case 5: 49 | return pcl::SACMODEL_CYLINDER; 50 | case 6: 51 | return pcl::SACMODEL_CONE; 52 | case 7: 53 | return pcl::SACMODEL_TORUS; 54 | case 8: 55 | return pcl::SACMODEL_PARALLEL_LINE; 56 | case 9: 57 | return pcl::SACMODEL_PERPENDICULAR_PLANE; 58 | case 10: 59 | return pcl::SACMODEL_PARALLEL_LINES; 60 | case 11: 61 | return pcl::SACMODEL_NORMAL_PLANE; 62 | case 12: 63 | return pcl::SACMODEL_NORMAL_SPHERE; 64 | case 13: 65 | return pcl::SACMODEL_REGISTRATION; 66 | case 14: 67 | return pcl::SACMODEL_REGISTRATION_2D; 68 | case 15: 69 | return pcl::SACMODEL_PARALLEL_PLANE; 70 | case 16: 71 | return pcl::SACMODEL_NORMAL_PARALLEL_PLANE; 72 | case 17: 73 | return pcl::SACMODEL_STICK; 74 | default: 75 | throw pcl::InvalidSACModelTypeException( 76 | "No SAC model for integer " + boost::lexical_cast(config_model_type)); 77 | } 78 | } 79 | 80 | inline Eigen::Affine3f tf_from_plane_model(float a, float b, float c, float d) { 81 | Eigen::Vector3f new_normal(a, b, c); 82 | Eigen::Vector3f old_normal(0, 0, 1); 83 | 84 | Eigen::Vector3f v = old_normal.cross(new_normal); 85 | float s2 = v.squaredNorm(); 86 | float cc = old_normal.dot(new_normal); 87 | Eigen::Matrix3f v_cross; 88 | v_cross << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; 89 | 90 | Eigen::Matrix3f rot = Eigen::Matrix3f::Identity() + v_cross + v_cross * v_cross * (1 - cc) / s2; 91 | 92 | Eigen::Affine3f arot(rot); 93 | 94 | // Create a transform where the rotation component is given by the rotation axis as the normal vector (a, b, c) 95 | // and some arbitrary angle about that axis and the translation component is -d in the z direction after 96 | // that rotation (not the original z direction, which is how transforms are usually defined). 97 | auto result = Eigen::Translation3f(0, 0, d) * arot.inverse(); 98 | 99 | return result; 100 | } 101 | 102 | inline Eigen::Affine3f tf_from_plane_model(const pcl_msgs::ModelCoefficients &plane) { 103 | return tf_from_plane_model(plane.values[0], plane.values[1], plane.values[2], plane.values[3]); 104 | } 105 | 106 | inline Eigen::Affine3f tf_from_plane_model(const pcl::ModelCoefficients &plane) { 107 | return tf_from_plane_model(plane.values[0], plane.values[1], plane.values[2], plane.values[3]); 108 | } 109 | 110 | template 111 | inline pcl::IndicesPtr all_indices(const typename pcl::PointCloud::ConstPtr cloud) { 112 | pcl::IndicesPtr indices = boost::make_shared >(); 113 | indices->resize(cloud->size()); 114 | std::iota(indices->begin(), indices->end(), 0); 115 | return indices; 116 | } 117 | 118 | template 119 | inline std::vector all_indices(const typename pcl::PointCloud &cloud) { 120 | std::vector indices; 121 | indices.resize(cloud.size()); 122 | std::iota(indices.begin(), indices.end(), 0); 123 | return indices; 124 | } 125 | 126 | inline std::size_t count_vertices(const std::vector &polygons) { 127 | return std::accumulate(polygons.begin(), polygons.end(), std::size_t(0), 128 | [](const std::size_t sum, const pcl::Vertices &v){ return sum + v.vertices.size(); }); 129 | } 130 | 131 | inline std::vector reindex(const std::vector &indexer, const std::vector &indices) { 132 | // Takes indices that relate to an original cloud and another set that relates to a cloud that was filtered 133 | // by the original and returns an indices set with the contents of the second that relates to the original cloud 134 | // (Turns cloud[indexer[indices[i]]] into cloud[reindexed[i]] for i = 0..indices.size()) 135 | if (indexer.size() <= indices.size()) { 136 | ROS_WARN("Indices is not smaller than reindexer, which is only valid if indices contains duplicates"); 137 | } 138 | std::vector reindexed; 139 | for (auto index : indices) { 140 | assert(index >= 0); 141 | 142 | if (static_cast(index) >= indexer.size()) { 143 | ROS_ERROR_STREAM("Error: index " << index << " is out of bounds of indexer"); 144 | } 145 | 146 | // Use .at to get the bounds check 147 | reindexed.push_back(indexer.at(static_cast(index))); 148 | } 149 | return reindexed; 150 | } 151 | 152 | inline pcl::PointIndices reindex(const pcl::PointIndices &indexer, const pcl::PointIndices &indices) { 153 | // Takes a PointIndices that relates to an original cloud and another that relates to a cloud that was filtered 154 | // by the original and returns a PointIndices with the contents of the second that relates to the original cloud 155 | // (Turns cloud[indexer[indices[i]]] into cloud[reindexed[i]] for i = 0..indices.size()) 156 | pcl::PointIndices reindexed; 157 | reindexed.header = indices.header; 158 | reindexed.indices = reindex(indexer.indices, indices.indices); 159 | return reindexed; 160 | } 161 | } 162 | 163 | #endif //SURFACES_UTILS_HPP 164 | -------------------------------------------------------------------------------- /surface_utils/include/surface_utils/smart_ptr.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by will on 6/21/16. 3 | // 4 | 5 | #ifndef PROJECT_SMART_PTR_HPP 6 | #define PROJECT_SMART_PTR_HPP 7 | 8 | #include 9 | 10 | struct null_deleter { 11 | void operator()(void const *) const {} 12 | }; 13 | 14 | template 15 | std::unique_ptr std_make_unique( Args&& ...args ) { 16 | return std::unique_ptr( new T( std::forward(args)... ) ); 17 | } 18 | 19 | #endif //PROJECT_SMART_PTR_HPP 20 | -------------------------------------------------------------------------------- /surface_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | surface_utils 4 | 0.0.0 5 | The surface_utils package 6 | 7 | 8 | 9 | 10 | will 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 | catkin 43 | roscpp 44 | pcl_ros 45 | pcl_msgs 46 | visualization_msgs 47 | geometry_msgs 48 | eigen_conversions 49 | roscpp 50 | pcl_ros 51 | pcl_msgs 52 | visualization_msgs 53 | geometry_msgs 54 | eigen_conversions 55 | 56 | 57 | 58 | 59 | 60 | 61 | --------------------------------------------------------------------------------