├── .clang-format ├── .run-clang-format ├── README.md ├── region_detection_core ├── CMakeLists.txt ├── README.md ├── cmake │ └── region_detection_core-config.cmake.in ├── config │ └── config.yaml ├── include │ └── region_detection_core │ │ ├── config_types.h │ │ ├── region_crop.h │ │ └── region_detector.h ├── package.xml └── src │ ├── region_crop.cpp │ ├── region_detector.cpp │ └── tests │ ├── adaptive_threshold_test.cpp │ ├── region_detection_test.cpp │ ├── threshold_grayscale_test.cpp │ └── threshold_in_range_test.cpp ├── region_detection_msgs ├── CMakeLists.txt ├── msg │ └── PoseSet.msg ├── package.xml └── srv │ ├── CropData.srv │ ├── DetectRegions.srv │ ├── GetSelectedRegions.srv │ └── ShowSelectableRegions.srv ├── region_detection_rclcpp ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── crop_data_server.cpp │ ├── interactive_region_selection.cpp │ └── region_detector_server.cpp └── region_detection_roscpp_demo ├── CMakeLists.txt ├── README.md ├── config ├── data_list.yaml ├── data_multiset.yaml ├── demo.rviz ├── region_detect.yaml └── region_detect_mult_imgs.yaml ├── launch ├── detect_regions_demo.launch └── detect_regions_multiset_demo.launch ├── package.xml └── src └── detect_regions_demo.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | AlignEscapedNewlinesLeft: false 5 | AlignTrailingComments: true 6 | AlignAfterOpenBracket: Align 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortFunctionsOnASingleLine: true 9 | AllowShortIfStatementsOnASingleLine: false 10 | AllowShortLoopsOnASingleLine: false 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakBeforeMultilineStrings: false 13 | AlwaysBreakTemplateDeclarations: true 14 | BinPackArguments: false 15 | BinPackParameters: false 16 | BreakBeforeBinaryOperators: false 17 | BreakBeforeTernaryOperators: false 18 | BreakConstructorInitializersBeforeComma: true 19 | ColumnLimit: 120 20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 21 | ConstructorInitializerIndentWidth: 2 22 | ContinuationIndentWidth: 4 23 | Cpp11BracedListStyle: false 24 | DerivePointerBinding: false 25 | ExperimentalAutoDetectBinPacking: false 26 | IndentCaseLabels: true 27 | IndentFunctionDeclarationAfterType: false 28 | IndentWidth: 2 29 | MaxEmptyLinesToKeep: 1 30 | NamespaceIndentation: None 31 | ObjCSpaceBeforeProtocolList: true 32 | PenaltyBreakBeforeFirstCallParameter: 19 33 | PenaltyBreakComment: 60 34 | PenaltyBreakFirstLessLess: 1000 35 | PenaltyBreakString: 1 36 | PenaltyExcessCharacter: 1000 37 | PenaltyReturnTypeOnItsOwnLine: 90 38 | PointerBindsToType: true 39 | SortIncludes: false 40 | SpaceAfterControlStatementKeyword: true 41 | SpaceAfterCStyleCast: false 42 | SpaceBeforeAssignmentOperators: true 43 | SpaceInEmptyParentheses: false 44 | SpacesBeforeTrailingComments: 2 45 | SpacesInAngles: false 46 | SpacesInCStyleCastParentheses: false 47 | SpacesInParentheses: false 48 | Standard: Auto 49 | TabWidth: 2 50 | UseTab: Never 51 | 52 | # Configure each individual brace in BraceWrapping 53 | BreakBeforeBraces: Custom 54 | 55 | # Control of individual brace wrapping cases 56 | BraceWrapping: { 57 | AfterClass: 'true' 58 | AfterControlStatement: 'true' 59 | AfterEnum : 'true' 60 | AfterFunction : 'true' 61 | AfterNamespace : 'true' 62 | AfterStruct : 'true' 63 | AfterUnion : 'true' 64 | BeforeCatch : 'true' 65 | BeforeElse : 'true' 66 | IndentBraces : 'false' 67 | } 68 | ... 69 | 70 | -------------------------------------------------------------------------------- /.run-clang-format: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \; 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Perception Based Region Detection 2 | ## Summary 3 | This library uses a variety of opencv and pcl filters to detect the contour of hand-drawn regions. 4 | 5 | --- 6 | 7 | ## Build 8 | ### ROS1 9 | For ROS1 the following packages are provided: 10 | 11 | - region_detection_core 12 | - region_detection_roscpp_demo 13 | 14 | From your catkin workspace run the following 15 | ```bash 16 | catkin build 17 | ``` 18 | 19 | ### ROS2 20 | For ROS2 the following packages are provided: 21 | - region_detection_core 22 | - region_detection_msgs 23 | - region_detection_rclcpp 24 | 25 | From your colcon workspace run the following 26 | ```bash 27 | colcon build --symlink-install 28 | ``` 29 | 30 | --- 31 | 32 | ## Use 33 | See each individual package for further instructions 34 | 35 | -------------------------------------------------------------------------------- /region_detection_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.0) 2 | project(region_detection_core VERSION 0.0.0 LANGUAGES CXX) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_CXX_EXTENSIONS OFF) 9 | endif() 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-w -Wextra -Wpedantic) 13 | endif() 14 | 15 | 16 | ## System dependencies are found with CMake's conventions 17 | find_package(Boost REQUIRED COMPONENTS system filesystem) 18 | find_package(OpenCV REQUIRED COMPONENTS core imgproc imgcodecs highgui) 19 | find_package(PCL REQUIRED COMPONENTS common filters surface segmentation) 20 | find_package(Eigen3 REQUIRED) 21 | find_package(console_bridge REQUIRED) 22 | find_package(yaml-cpp REQUIRED ) 23 | 24 | find_package(PkgConfig REQUIRED) 25 | pkg_check_modules(yaml_cpp REQUIRED yaml-cpp) 26 | 27 | 28 | ## Logging library 29 | if( Log4cxx_DIR ) 30 | find_package( Log4cxx NO_MODULE ) 31 | elseif( NOT Log4cxx_FOUND ) 32 | message(STATUS "Searching for log4cxx/logger.h") 33 | find_path( Log4cxx_INCLUDE_DIR log4cxx/logger.h ) 34 | 35 | message(STATUS "Searching for libLog4cxx") 36 | find_library( Log4cxx_LIBRARY log4cxx ) 37 | 38 | include( FindPackageHandleStandardArgs ) 39 | FIND_PACKAGE_HANDLE_STANDARD_ARGS( Log4cxx Log4cxx_INCLUDE_DIR Log4cxx_LIBRARY ) 40 | if( LOG4CXX_FOUND ) 41 | set( Log4cxx_FOUND TRUE ) 42 | endif() 43 | endif() 44 | 45 | ########### 46 | ## Build ## 47 | ########### 48 | 49 | # check if cxx_std_14 is found in the CMAKE_CXX_COMPILE_FEATURES list, set CXX_FEATURE_FOUND=-1 when it does not 50 | list(FIND CMAKE_CXX_COMPILE_FEATURES cxx_std_14 CXX_FEATURE_FOUND) 51 | 52 | ## Specify additional locations of header files 53 | include_directories( 54 | ${OPENCV_INCLUDE_DIRS} 55 | ${Boost_INCLUDE_DIRS} 56 | ${PCL_LIBRARIES} 57 | ${Log4cxx_INCLUDE_DIRS} 58 | ) 59 | 60 | # build executables 61 | add_executable(threshold_grayscale_test src/tests/threshold_grayscale_test.cpp) 62 | target_link_libraries(threshold_grayscale_test 63 | ${OpenCV_LIBS} 64 | ${Boost_LIBRARIES}) 65 | 66 | add_executable(threshold_in_range_test src/tests/threshold_in_range_test.cpp) 67 | target_link_libraries(threshold_in_range_test 68 | ${OpenCV_LIBS} 69 | ${Boost_LIBRARIES}) 70 | 71 | add_executable(adaptive_threshold_test src/tests/adaptive_threshold_test.cpp) 72 | target_link_libraries(adaptive_threshold_test 73 | ${OpenCV_LIBS} 74 | ${Boost_LIBRARIES}) 75 | 76 | # build library 77 | add_library(${PROJECT_NAME} SHARED 78 | src/region_detector.cpp 79 | src/region_crop.cpp 80 | ) 81 | target_link_libraries(${PROJECT_NAME} PUBLIC 82 | ${OpenCV_LIBS} 83 | ${Boost_LIBRARIES} 84 | ${Eigen3_LIBRARIES} 85 | ${PCL_LIBRARIES} 86 | ${Log4cxx_LIBRARY} 87 | yaml-cpp 88 | ) 89 | target_include_directories(${PROJECT_NAME} PUBLIC 90 | "$" 91 | "$") 92 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC 93 | ${OPENCV_INCLUDE_DIRS} 94 | ${Boost_INCLUDE_DIRS} 95 | ${Eigen3_INCLUDE_DIRS} 96 | ${PCL_INCLUDE_DIRS} 97 | ${Log4cxx_INCLUDE_DIRS} 98 | ${yaml-cpp_INCLUDE_DIRS} 99 | ) 100 | 101 | add_executable(region_detection_test 102 | src/tests/region_detection_test.cpp) 103 | target_link_libraries(region_detection_test 104 | ${OpenCV_LIBS} 105 | ${Boost_LIBRARIES} 106 | ${PROJECT_NAME}) 107 | 108 | set_target_properties(${PROJECT_NAME} PROPERTIES 109 | CXX_STANDARD 14 110 | CXX_STANDARD_REQUIRED YES 111 | CXX_EXTENSIONS NO) 112 | if(CXX_FEATURE_FOUND EQUAL "-1") 113 | target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14) 114 | else() 115 | target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_14) 116 | endif() 117 | 118 | 119 | 120 | ############# 121 | ## Install ## 122 | ############# 123 | install(DIRECTORY include/${PROJECT_NAME} 124 | DESTINATION include 125 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 126 | PATTERN ".svn" EXCLUDE 127 | ) 128 | 129 | install(TARGETS threshold_grayscale_test threshold_in_range_test adaptive_threshold_test region_detection_test 130 | DESTINATION bin) 131 | 132 | list (APPEND PACKAGE_LIBRARIES ${PROJECT_NAME}) 133 | install(TARGETS ${PACKAGE_LIBRARIES} 134 | EXPORT ${PROJECT_NAME}-targets 135 | DESTINATION lib) 136 | 137 | install(EXPORT ${PROJECT_NAME}-targets 138 | NAMESPACE ${PROJECT_NAME}:: DESTINATION lib/cmake/${PROJECT_NAME}) 139 | 140 | install(FILES package.xml DESTINATION share/${PROJECT_NAME}) 141 | 142 | install(DIRECTORY config 143 | DESTINATION share/${PROJECT_NAME}/ 144 | ) 145 | 146 | # Create cmake config files 147 | include(CMakePackageConfigHelpers) 148 | configure_package_config_file( 149 | ${CMAKE_CURRENT_LIST_DIR}/cmake/${PROJECT_NAME}-config.cmake.in 150 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake 151 | INSTALL_DESTINATION lib/cmake/${PROJECT_NAME} 152 | NO_CHECK_REQUIRED_COMPONENTS_MACRO) 153 | 154 | write_basic_package_version_file( 155 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake 156 | VERSION ${PROJECT_VERSION} COMPATIBILITY ExactVersion) 157 | 158 | install(FILES 159 | "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake" 160 | "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake" 161 | DESTINATION lib/cmake/${PROJECT_NAME}) 162 | 163 | export(EXPORT ${PROJECT_NAME}-targets NAMESPACE ${PROJECT_NAME}:: FILE 164 | ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-targets.cmake) 165 | 166 | ############# 167 | ## Testing ## 168 | ############# 169 | 170 | -------------------------------------------------------------------------------- /region_detection_core/README.md: -------------------------------------------------------------------------------- 1 | 2 | # Region Detection Core 3 | ### Summary 4 | This package contains the main library used for detecting the contours of hand-drawn regions made with a marker on a surface. 5 | 6 | --- 7 | ### RegionDetector: 8 | This is the main class implementation and takes 2d images and 3d point clouds as inputs and returns the 3d locations and of the points encompassing the detected contours. The color of the contours shall be dark and in high contrast with the surface. The images and point clouds are assumed to be of the same size so if the image is 480 x 640 then the point cloud size should match that. 9 | 10 | - Configuration 11 | The configuration file needed by the region detection contains various fields to configure the opencv and pcl filters. See [here](config/config.yaml) for an example 12 | - opencv: This section contains the **methods** field which is a list of filter methods to apply to the image; the order in which these are applied are from left to right. Following this are the configuration parameters specific to each method supported. The method supported as of now are the following: 13 | - INVERT 14 | - GRAYSCALE # converts the image into a single channel grayscale 15 | - THRESHOLD 16 | - DILATION 17 | - EROSION 18 | - CANNY 19 | - THINNING 20 | - RANGE 21 | - CLAHE 22 | - EQUALIZE_HIST 23 | - EQUALIZE_HIST_YUV 24 | - HSV 25 | - pcl2d: These are parameters used to configure various pcl filters. These filters are applied in pixel space and assume the the **z** value of each point is 0. 26 | - pcl: These are parameters used to configure various pcl filters. These filters are applied to the 3d data in the point cloud that corresponds to the contours detected in the 2d analysis. 27 | --- 28 | 29 | ### RegionCrop: 30 | This class crops data outside the contour of the regions, the reverse is also possible. 31 | - Configuration 32 | The configuration yaml file shall have the following form 33 | ```yaml 34 | scale_factor: 1.2 35 | plane_dist_threshold: 0.2 36 | heigth_limits_min: -0.1 37 | heigth_limits_max: 0.1 38 | dir_estimation_method: 1 # PLANE_NORMAL: 1, NORMAL_AVGR: 2, POSE_Z_AXIS: 3, USER_DEFINED: 4 39 | user_dir: [0.0, 0.0, 0.1] # Used when dir_estimation_method == 4 40 | view_point: [0.0, 0.0, 10.0] 41 | ``` 42 | 43 | --- 44 | ### Test Program 45 | The `region_detection_test` program leverages the `RegionDetector` class and takes a configuration and image file in order to detect the contours in the image. In order to run this program do the following: 46 | - Locate the executable: 47 | - For a colcon build, it should be in the `install/region_detection_core/bin/` directory 48 | - For a catkin build, it should be inside the `devel/bin` directory 49 | - Locate a yaml configuration and image file. 50 | - Run the following command 51 | ```bash 52 | ./install/region_detection_core/bin/region_detection_test 53 | ``` 54 | - In addition to that, a third optional argument can be passed in order to run the contour detection function after applying the opencv filters. If `1` is used then the contours will be drawn on the image with different colors to distinguish them apart. 55 | 56 | -------------------------------------------------------------------------------- /region_detection_core/cmake/region_detection_core-config.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | set(@PROJECT_NAME@_FOUND ON) 4 | set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include") 5 | set_and_check(@PROJECT_NAME@_LIBRARIES "${PACKAGE_PREFIX_DIR}/lib") 6 | 7 | include(CMakeFindDependencyMacro) 8 | 9 | if(${CMAKE_VERSION} VERSION_LESS "3.10.0") 10 | find_package(Boost) 11 | find_package(OpenCV REQUIRED COMPONENTS core imgproc imgcodecs highgui) 12 | find_package(PCL REQUIRED COMPONENTS common filters surface segmentation) 13 | find_package(Eigen3 REQUIRED) 14 | find_package(yaml-cpp REQUIRED) 15 | find_package(console_bridge REQUIRED) 16 | else() 17 | find_dependency(Boost) 18 | find_dependency(OpenCV REQUIRED COMPONENTS core imgproc imgcodecs highgui) 19 | find_dependency(PCL REQUIRED COMPONENTS common filters surface segmentation) 20 | find_dependency(Eigen3 REQUIRED) 21 | find_dependency(yaml-cpp REQUIRED) 22 | find_dependency(console_bridge REQUIRED) 23 | endif() 24 | 25 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake") -------------------------------------------------------------------------------- /region_detection_core/config/config.yaml: -------------------------------------------------------------------------------- 1 | opencv: 2 | #methods: ["CLAHE", "EQUALIZE_HIST", "EQUALIZE_HIST_YUV", "HSV", .. . 3 | # "INVERT", "GRAYSCALE", "THRESHOLD", "DILATION", "EROSION", "CANNY", "THINNING","RANGE"] 4 | methods: ["EQUALIZE_HIST", "GRAYSCALE", "EROSION","CLAHE", "DILATION","INVERT", "THRESHOLD", "THINNING"] 5 | debug_mode_enable: true 6 | debug_window_name: "DEBUG_REGION_DETECTION" 7 | debug_wait_key: false 8 | hsv: 9 | h: [0, 180] 10 | s: [100, 255] 11 | v: [0, 80] 12 | threshold: 13 | type: 0 14 | value: 180 15 | canny: 16 | lower_threshold: 200 17 | upper_threshold: 250 18 | aperture_size: 2 19 | dilation: 20 | elem: 2 # 0: Rect, 1: Cross, 2: Ellipse 21 | kernel_size: 2 # 2n + 1 22 | erosion: 23 | elem: 2 # 0: Rect, 1: Cross, 2: Ellipse 24 | kernel_size: 4 # 2n + 1 25 | contour: 26 | method: 1 27 | mode: 0 # See cv::RetrievalModes 28 | range: 29 | low: 190 30 | high: 255 31 | clahe: 32 | clip_limit: 4.0 33 | tile_grid_size: [4, 4] 34 | pcl2d: 35 | downsampling_radius: 4.0 # pixel units 36 | split_dist: 6.0 # pixel units 37 | closed_curve_max_dist: 6.0 # pixel units 38 | simplification_min_points: 10 # applies simplification only if the closed curve has 10 points or more 39 | simplification_alpha: 24.0 # pixel units, used in concave hull step 40 | pcl: 41 | debug_mode_enable: false 42 | max_merge_dist: 0.01 43 | closed_curve_max_dist: 0.01 44 | simplification_min_dist: 0.01 45 | split_dist: 0.1 46 | min_num_points: 10 47 | stat_removal: 48 | enable: true 49 | kmeans: 100 50 | stddev: 3.0 51 | normal_est: 52 | downsampling_radius: 0.01 53 | search_radius: 0.02 54 | kdtree_epsilon: 0.001 55 | viewpoint_xyz: [0.0, 0.0, 100.0] 56 | 57 | -------------------------------------------------------------------------------- /region_detection_core/include/region_detection_core/config_types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * config_types.h 3 | * 4 | * Created on: Jun 17, 2020 5 | * Author: Jorge Nicho 6 | * 7 | * Copyright 2020 Southwest Research Institute 8 | * 9 | * Licensed under the Apache License, Version 2.0 (the "License"); 10 | * you may not use this file except in compliance with the License. 11 | * You may obtain a copy of the License at 12 | * 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #ifndef INCLUDE_CONFIG_TYPES_H_ 23 | #define INCLUDE_CONFIG_TYPES_H_ 24 | 25 | #include 26 | #include 27 | 28 | namespace region_detection_core 29 | { 30 | namespace config_2d 31 | { 32 | struct ThresholdCfg 33 | { 34 | int value = 150; 35 | int type = cv::ThresholdTypes::THRESH_TRUNC; 36 | 37 | static const int MAX_VALUE = 255; 38 | static const int MAX_TYPE = cv::ThresholdTypes::THRESH_TOZERO_INV; 39 | static const int MAX_BINARY_VALUE = 255; 40 | }; 41 | 42 | struct MorphologicalCfg 43 | { 44 | int elem = 0; 45 | int kernel_size = 1; 46 | 47 | static const int MAX_ELEM = 2; 48 | static const int MAX_KERNEL_SIZE = 21; 49 | }; 50 | 51 | struct CannyCfg 52 | { 53 | int lower_threshold = 45; 54 | int upper_threshold = lower_threshold * 3; 55 | int aperture_size = 1; 56 | 57 | static const int MAX_ENABLE = 1; 58 | static const int MAX_LOWER_TH = 100; 59 | static const int MAX_UPPER_TH = 255; 60 | static const int MAX_APERTURE_SIZE = 3; 61 | }; 62 | 63 | struct CountourCfg 64 | { 65 | int mode = CV_RETR_EXTERNAL; 66 | int method = CV_CHAIN_APPROX_SIMPLE; 67 | 68 | static const int MAX_MODE = CV_RETR_TREE; 69 | static const int MAX_METHOD = CV_CHAIN_APPROX_TC89_KCOS; 70 | }; 71 | 72 | struct RangeCfg 73 | { 74 | int low; 75 | int high; 76 | }; 77 | 78 | struct HSVCfg 79 | { 80 | std::array h; 81 | std::array s; 82 | std::array v; 83 | }; 84 | 85 | struct CLAHECfg 86 | { 87 | double clip_limit; 88 | std::array tile_grid_size; 89 | }; 90 | } // namespace config_2d 91 | 92 | namespace config_3d 93 | { 94 | struct StatisticalRemovalCfg 95 | { 96 | bool enable = true; 97 | int kmeans = 50; 98 | double stddev = 1.0; 99 | }; 100 | struct DownsampleCfg 101 | { 102 | bool enable = true; 103 | double voxel_leafsize = 0.005; 104 | }; 105 | 106 | struct SequencingCfg 107 | { 108 | double kdtree_epsilon = 1e-5; 109 | double search_radius = 0.02; 110 | }; 111 | 112 | struct NormalEstimationCfg 113 | { 114 | double downsampling_radius = 0.01; 115 | double search_radius = 0.02; 116 | double kdtree_epsilon = 1e-5; 117 | std::array viewpoint_xyz = { 0.0, 0.0, 100.0 }; 118 | }; 119 | } // namespace config_3d 120 | } // namespace region_detection_core 121 | 122 | #endif /* INCLUDE_CONFIG_TYPES_H_ */ 123 | -------------------------------------------------------------------------------- /region_detection_core/include/region_detection_core/region_crop.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file region_crop.h 4 | * @date Jul 10, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #ifndef INCLUDE_REGION_DETECTION_CORE_REGION_CROP_H_ 37 | #define INCLUDE_REGION_DETECTION_CORE_REGION_CROP_H_ 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | namespace region_detection_core 44 | { 45 | enum class DirectionEstMethods : unsigned int 46 | { 47 | PLANE_NORMAL = 1, 48 | NORMAL_AVGR, 49 | POSE_Z_AXIS, 50 | USER_DEFINED 51 | }; 52 | 53 | struct RegionCropConfig 54 | { 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | double scale_factor = 1.0; 57 | double plane_dist_threshold = 0.1; 58 | std::pair heigth_limits = std::make_pair(-0.1, 0.1); 59 | DirectionEstMethods dir_estimation_method = DirectionEstMethods::PLANE_NORMAL; 60 | Eigen::Vector3d user_dir = Eigen::Vector3d::UnitZ(); 61 | Eigen::Vector3d view_point = Eigen::Vector3d(0, 0, 10.0); 62 | }; 63 | 64 | template 65 | class RegionCrop 66 | { 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 69 | typedef std::vector > EigenPose3dVector; 70 | 71 | RegionCrop(); 72 | virtual ~RegionCrop(); 73 | 74 | void setConfig(const RegionCropConfig& config); 75 | void setRegion(const EigenPose3dVector& closed_region); 76 | void setInput(const typename pcl::PointCloud::ConstPtr& cloud); 77 | std::vector filter(bool reverse = false); 78 | 79 | private: 80 | EigenPose3dVector closed_region_; 81 | RegionCropConfig config_; 82 | typename pcl::PointCloud::ConstPtr input_; 83 | }; 84 | 85 | } /* namespace region_detection_core */ 86 | 87 | #endif /* INCLUDE_REGION_DETECTION_CORE_REGION_CROP_H_ */ 88 | -------------------------------------------------------------------------------- /region_detection_core/include/region_detection_core/region_detector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file region_detector.h 4 | * @date Jun 4, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #ifndef INCLUDE_REGION_DETECTOR_H_ 37 | #define INCLUDE_REGION_DETECTOR_H_ 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include "region_detection_core/config_types.h" 51 | 52 | namespace region_detection_core 53 | { 54 | enum class Methods2D : int 55 | { 56 | GRAYSCALE = 0, 57 | INVERT, 58 | THRESHOLD, 59 | DILATION, 60 | EROSION, 61 | CANNY, 62 | THINNING, 63 | RANGE, 64 | HSV, 65 | EQUALIZE_HIST, 66 | EQUALIZE_HIST_YUV, 67 | CLAHE, 68 | }; 69 | 70 | static const std::map METHOD_CODES_MAPPINGS = { { "GRAYSCALE", Methods2D::GRAYSCALE }, 71 | { "INVERT", Methods2D::INVERT }, 72 | { "THRESHOLD", Methods2D::THRESHOLD }, 73 | { "DILATION", Methods2D::DILATION }, 74 | { "EROSION", Methods2D::EROSION }, 75 | { "CANNY", Methods2D::CANNY }, 76 | { "THINNING", Methods2D::THINNING }, 77 | { "RANGE", Methods2D::RANGE }, 78 | { "HSV", Methods2D::HSV }, 79 | { "EQUALIZE_HIST", Methods2D::EQUALIZE_HIST }, 80 | { "EQUALIZE_HIST_YUV", 81 | Methods2D::EQUALIZE_HIST_YUV }, 82 | { "CLAHE", Methods2D::CLAHE } }; 83 | 84 | struct RegionDetectionConfig 85 | { 86 | // OpenCV configurations 87 | struct OpenCVCfg 88 | { 89 | std::vector methods = {}; 90 | 91 | config_2d::ThresholdCfg threshold; 92 | config_2d::MorphologicalCfg dilation; 93 | config_2d::MorphologicalCfg erosion; 94 | config_2d::CannyCfg canny; 95 | config_2d::CountourCfg contour; 96 | config_2d::RangeCfg range; 97 | config_2d::HSVCfg hsv; 98 | config_2d::CLAHECfg clahe; 99 | 100 | bool debug_mode_enable = false; 101 | std::string debug_window_name = "DEBUG_REGION_DETECTION"; 102 | bool debug_wait_key = false; 103 | } opencv_cfg; 104 | 105 | struct PCL2DCfg 106 | { 107 | double downsampling_radius = 4.0; // pixel units 108 | double split_dist = 6.0; // pixel units 109 | double closed_curve_max_dist = 6.0; // pixel units 110 | int simplification_min_points = 10; // applies simplification only if the closed curve has 10 points or more 111 | double simplification_alpha = 24.0; // pixel units, used in concave hull step 112 | } pcl_2d_cfg; 113 | 114 | struct PCLCfg 115 | { 116 | config_3d::StatisticalRemovalCfg stat_removal; 117 | config_3d::NormalEstimationCfg normal_est; 118 | 119 | double max_merge_dist = 0.01; /** @brief in meters */ 120 | double closed_curve_max_dist = 0.01; /** @brief in meters */ 121 | double simplification_min_dist = 0.02; /** @brief in meters */ 122 | double split_dist = 0.1; /** @brief will split segments when the distance between consecutive points exceeds this 123 | value, in meters */ 124 | int min_num_points = 10; /** @brief segments must have at least this many points*/ 125 | 126 | bool debug_mode_enable = false; /** @brief not used at the moment */ 127 | } pcl_cfg; 128 | 129 | static RegionDetectionConfig loadFromFile(const std::string& yaml_file); 130 | static RegionDetectionConfig load(const std::string& yaml_str); 131 | }; 132 | 133 | class RegionDetector 134 | { 135 | public: 136 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 137 | 138 | struct DataBundle 139 | { 140 | cv::Mat image; 141 | pcl::PCLPointCloud2 cloud_blob; 142 | Eigen::Isometry3d transform; 143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 144 | }; 145 | 146 | typedef std::vector> DataBundleVec; 147 | 148 | typedef std::vector> EigenPose3dVector; 149 | struct RegionResults 150 | { 151 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 152 | std::vector closed_regions_poses; 153 | std::vector open_regions_poses; 154 | 155 | // additional results 156 | std::vector images; 157 | }; 158 | 159 | RegionDetector(const RegionDetectionConfig& config, log4cxx::LoggerPtr logger = nullptr); 160 | RegionDetector(log4cxx::LoggerPtr logger = nullptr); 161 | virtual ~RegionDetector(); 162 | 163 | log4cxx::LoggerPtr getLogger(); 164 | bool configure(const RegionDetectionConfig& config); 165 | bool configure(const std::string& yaml_str); 166 | bool configureFromFile(const std::string& yaml_file); 167 | const RegionDetectionConfig& getConfig(); 168 | 169 | /** 170 | * @brief computes contours from images 171 | * @param input Input image 172 | * @param contours_indices Output contours in pixel coordinates 173 | * @param output Output image 174 | * @return True on success, false otherwise 175 | */ 176 | bool compute2d(cv::Mat input, cv::Mat& output) const; 177 | 178 | /** 179 | * @brief computes contours from images and returns their indices 180 | * @param input Input image 181 | * @param contours_indices Output contours in pixel coordinates 182 | * @param output Output image 183 | * @param contours_indices The indices of the contours 184 | * @return True on success, false otherwise 185 | */ 186 | bool compute2d(cv::Mat input, cv::Mat& output, std::vector>& contours_indices) const; 187 | 188 | /** 189 | * @brief computes contours 190 | * @param input A vector of data structures containing point clouds and images 191 | * @param regions (Output) the detected regions 192 | * @return True on success, false otherwise 193 | */ 194 | bool compute(const DataBundleVec& input, RegionDetector::RegionResults& regions); 195 | 196 | static log4cxx::LoggerPtr createDefaultInfoLogger(const std::string& logger_name); 197 | static log4cxx::LoggerPtr createDefaultDebugLogger(const std::string& logger_name); 198 | 199 | private: 200 | /** 201 | * @class region_detection_core::RegionDetector::Result 202 | * @brief Convenience class that can be evaluated as a bool and contains an error message, used internally 203 | */ 204 | struct Result 205 | { 206 | /** 207 | * @brief Constructor for the response object 208 | * @param success Set to true if the requested action was completed, use false otherwise. 209 | * @param data Optional data that was generated from the requested transaction. 210 | */ 211 | Result(bool success = true, std::string msg = "") : success(success), msg(msg) {} 212 | 213 | Result(const Result& obj) : success(obj.success), msg(obj.msg) {} 214 | 215 | ~Result() {} 216 | 217 | Result& operator=(const Result& obj) 218 | { 219 | success = obj.success; 220 | msg = obj.msg; 221 | return *this; 222 | } 223 | 224 | Result& operator=(const bool& b) 225 | { 226 | this->success = b; 227 | return *this; 228 | } 229 | 230 | operator bool() const { return success; } 231 | 232 | bool success; 233 | std::string msg; 234 | }; 235 | 236 | // 2d methods 237 | void updateDebugWindow(const cv::Mat& im) const; 238 | 239 | RegionDetector::Result apply2dCanny(cv::Mat input, cv::Mat& output) const; 240 | RegionDetector::Result apply2dDilation(cv::Mat input, cv::Mat& output) const; 241 | RegionDetector::Result apply2dErosion(cv::Mat input, cv::Mat& output) const; 242 | RegionDetector::Result apply2dThreshold(cv::Mat input, cv::Mat& output) const; 243 | RegionDetector::Result apply2dInvert(cv::Mat input, cv::Mat& output) const; 244 | RegionDetector::Result apply2dGrayscale(cv::Mat input, cv::Mat& output) const; 245 | RegionDetector::Result apply2dRange(cv::Mat input, cv::Mat& output) const; 246 | RegionDetector::Result apply2dHSV(cv::Mat input, cv::Mat& output) const; 247 | RegionDetector::Result apply2dEqualizeHistYUV(cv::Mat input, cv::Mat& output) const; 248 | RegionDetector::Result apply2dEqualizeHist(cv::Mat input, cv::Mat& output) const; 249 | RegionDetector::Result apply2dCLAHE(cv::Mat input, cv::Mat& output) const; 250 | 251 | Result compute2dContours(cv::Mat input, std::vector>& contours_indices, cv::Mat& output) const; 252 | 253 | // 3d methods 254 | 255 | Result extractContoursFromCloud(const std::vector>& contours_indices, 256 | pcl::PointCloud::ConstPtr input, 257 | std::vector::Ptr>& contours_points); 258 | 259 | Result combineIntoClosedRegions(const std::vector::Ptr>& contours_points, 260 | std::vector::Ptr>& closed_curves, 261 | std::vector::Ptr>& open_curves); 262 | 263 | Result computePoses(pcl::PointCloud::ConstPtr source_normals_cloud, 264 | std::vector::Ptr>& closed_curves, 265 | std::vector& regions); 266 | 267 | Result computeNormals(const pcl::PointCloud::ConstPtr source_cloud, 268 | const std::vector::Ptr>& curves_points, 269 | std::vector::Ptr>& curves_normals); 270 | 271 | Result mergeCurves(pcl::PointCloud c1, 272 | pcl::PointCloud c2, 273 | pcl::PointCloud& merged); 274 | 275 | pcl::PointCloud sequence(pcl::PointCloud::ConstPtr points, double epsilon = 1e-5); 276 | std::vector::Ptr> split(const pcl::PointCloud& sequenced_points, 277 | double split_dist); 278 | 279 | void findClosedCurves(const std::vector::Ptr>& sequenced_points, 280 | double max_dist, 281 | std::vector::Ptr>& closed_curves_vec, 282 | std::vector::Ptr>& open_curves_vec); 283 | 284 | std::vector::Ptr> 285 | simplifyByMinimunLength(const std::vector::Ptr>& segments, double min_length); 286 | 287 | log4cxx::LoggerPtr logger_; 288 | std::shared_ptr cfg_; 289 | std::size_t window_counter_; 290 | }; 291 | 292 | } /* namespace region_detection_core */ 293 | 294 | #endif /* INCLUDE_REGION_DETECTOR_H_ */ 295 | -------------------------------------------------------------------------------- /region_detection_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | region_detection_core 4 | 0.0.0 5 | The region_detection_core package 6 | Jorge Nicho 7 | BSD3 8 | Jorge Nicho 9 | 10 | libconsole-bridge-dev 11 | libopencv-dev 12 | 13 | 14 | cmake 15 | 16 | 17 | -------------------------------------------------------------------------------- /region_detection_core/src/region_crop.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file region_crop.cpp 4 | * @date Jul 10, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #include 36 | 37 | #include "region_detection_core/region_crop.h" 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | 52 | #include 53 | #include 54 | 55 | static const double EPSILON = 1e-8; 56 | 57 | typedef std::vector> EigenPose3dVector_HIDDEN; 58 | 59 | pcl::PointCloud::Ptr computePlanarHullFromNormals(EigenPose3dVector_HIDDEN region_3d) 60 | { 61 | using namespace pcl; 62 | using namespace Eigen; 63 | 64 | PointCloud::Ptr planar_hull = boost::make_shared>(); 65 | 66 | // compute plane normal from averages 67 | pcl::Normal plane_normal; 68 | Vector3d normal_vec = std::accumulate(region_3d.begin(), 69 | region_3d.end(), 70 | Vector3d(0, 0, 0), 71 | [](Vector3d current_normal, Eigen::Isometry3d pose) -> Vector3d { 72 | Vector3d pose_normal; 73 | pose_normal.array() = pose.linear().col(2); 74 | pose_normal += current_normal; 75 | return pose_normal; 76 | }); 77 | normal_vec /= region_3d.size(); 78 | normal_vec.normalize(); 79 | 80 | // compute centroid 81 | Vector3d centroid = std::accumulate(region_3d.begin(), 82 | region_3d.end(), 83 | Vector3d(0, 0, 0), 84 | [](Vector3d current_centroid, const Eigen::Isometry3d& pose) { 85 | Vector3d region_location; 86 | region_location = pose.translation(); 87 | region_location += current_centroid; 88 | return region_location; 89 | }); 90 | centroid /= region_3d.size(); 91 | 92 | // defining plane 93 | double d = -centroid.dot(normal_vec); 94 | pcl::ModelCoefficients::Ptr coefficients = boost::make_shared(); 95 | coefficients->values.resize(4); 96 | coefficients->values[0] = normal_vec(0); 97 | coefficients->values[1] = normal_vec(1); 98 | coefficients->values[2] = normal_vec(2); 99 | coefficients->values[3] = d; 100 | 101 | // converting to point cloud 102 | PointCloud::Ptr region_cloud_3d = boost::make_shared>(); 103 | std::transform( 104 | region_3d.begin(), region_3d.end(), std::back_inserter(*region_cloud_3d), [](const Eigen::Isometry3d& pose) { 105 | PointXYZ p; 106 | p.getArray3fMap() = pose.translation().array().cast(); 107 | return p; 108 | }); 109 | 110 | // projecting onto plane 111 | pcl::ProjectInliers project_inliers; 112 | project_inliers.setModelType(pcl::SACMODEL_NORMAL_PLANE); 113 | project_inliers.setInputCloud(region_cloud_3d); 114 | project_inliers.setModelCoefficients(coefficients); 115 | project_inliers.filter(*planar_hull); 116 | 117 | return planar_hull; 118 | } 119 | 120 | pcl::PointCloud::Ptr computePlanarHullFromPlane(const EigenPose3dVector_HIDDEN& region_3d) 121 | { 122 | using namespace pcl; 123 | using namespace Eigen; 124 | 125 | PointCloud::Ptr planar_hull = boost::make_shared>(); 126 | 127 | // converting to point cloud 128 | PointCloud::Ptr region_cloud_3d = boost::make_shared>(); 129 | std::transform( 130 | region_3d.begin(), region_3d.end(), std::back_inserter(*region_cloud_3d), [](const Eigen::Isometry3d& pose) { 131 | PointXYZ p; 132 | p.getArray3fMap() = pose.translation().array().cast(); 133 | return p; 134 | }); 135 | 136 | // computing moi 137 | PointXYZ min_point, max_point, center_point; 138 | Matrix3f rotation; 139 | Eigen::Vector3f center, x_axis, y_axis, z_axis; 140 | pcl::MomentOfInertiaEstimation moi; 141 | moi.setInputCloud(region_cloud_3d); 142 | moi.compute(); 143 | moi.getOBB(min_point, max_point, center_point, rotation); 144 | 145 | // computing distance threshold 146 | Vector3f diff = max_point.getArray3fMap() - min_point.getArray3fMap(); 147 | double distance_threshold = diff.array().abs().maxCoeff(); 148 | 149 | // computing plane 150 | PointIndices indices; 151 | pcl::ModelCoefficients::Ptr coefficients = boost::make_shared(); 152 | pcl::SACSegmentation seg; 153 | seg.setOptimizeCoefficients(true); 154 | seg.setModelType(pcl::SACMODEL_PLANE); 155 | seg.setMethodType(pcl::SAC_RANSAC); 156 | seg.setDistanceThreshold(distance_threshold); 157 | seg.setInputCloud(region_cloud_3d); 158 | seg.segment(indices, *coefficients); 159 | 160 | // projecting onto plane 161 | pcl::ProjectInliers project_inliers; 162 | project_inliers.setModelType(pcl::SACMODEL_NORMAL_PLANE); 163 | project_inliers.setInputCloud(region_cloud_3d); 164 | project_inliers.setModelCoefficients(coefficients); 165 | project_inliers.filter(*planar_hull); 166 | 167 | return planar_hull; 168 | } 169 | 170 | pcl::PointCloud::Ptr computePlanarHullFromZVector(const EigenPose3dVector_HIDDEN& region_3d) 171 | { 172 | using namespace pcl; 173 | using namespace Eigen; 174 | 175 | PointCloud::Ptr planar_hull = boost::make_shared>(); 176 | 177 | // converting to point cloud 178 | PointCloud::Ptr region_cloud_3d = boost::make_shared>(); 179 | std::transform( 180 | region_3d.begin(), region_3d.end(), std::back_inserter(*region_cloud_3d), [](const Eigen::Isometry3d& pose) { 181 | PointXYZ p; 182 | p.getArray3fMap() = pose.translation().array().cast(); 183 | return p; 184 | }); 185 | 186 | // computing moi 187 | Eigen::Vector3f center, x_axis, y_axis, z_axis; 188 | pcl::MomentOfInertiaEstimation moi; 189 | moi.setInputCloud(region_cloud_3d); 190 | moi.compute(); 191 | moi.getEigenVectors(x_axis, y_axis, z_axis); 192 | moi.getMassCenter(center); 193 | 194 | // defining plane 195 | Vector3f normal_vec = z_axis.normalized(); 196 | double d = -center.dot(normal_vec); 197 | pcl::ModelCoefficients::Ptr coefficients = boost::make_shared(); 198 | coefficients->values.resize(4); 199 | coefficients->values[0] = normal_vec(0); 200 | coefficients->values[1] = normal_vec(1); 201 | coefficients->values[2] = normal_vec(2); 202 | coefficients->values[3] = d; 203 | 204 | // projecting onto plane 205 | pcl::ProjectInliers project_inliers; 206 | project_inliers.setModelType(pcl::SACMODEL_NORMAL_PLANE); 207 | project_inliers.setInputCloud(region_cloud_3d); 208 | project_inliers.setModelCoefficients(coefficients); 209 | project_inliers.filter(*planar_hull); 210 | 211 | return planar_hull; 212 | } 213 | 214 | pcl::PointCloud::Ptr computePlanarHullFromUserVector(const EigenPose3dVector_HIDDEN& region_3d, 215 | const Eigen::Vector3d& user_vec) 216 | { 217 | using namespace pcl; 218 | using namespace Eigen; 219 | 220 | PointCloud::Ptr planar_hull = boost::make_shared>(); 221 | 222 | // converting to point cloud 223 | PointCloud::Ptr region_cloud_3d = boost::make_shared>(); 224 | std::transform( 225 | region_3d.begin(), region_3d.end(), std::back_inserter(*region_cloud_3d), [](const Eigen::Isometry3d& pose) { 226 | PointXYZ p; 227 | p.getArray3fMap() = pose.translation().array().cast(); 228 | return p; 229 | }); 230 | 231 | // computing moi 232 | Eigen::Vector3f center; 233 | pcl::MomentOfInertiaEstimation moi; 234 | moi.setInputCloud(region_cloud_3d); 235 | moi.compute(); 236 | moi.getMassCenter(center); 237 | 238 | // defining plane 239 | Vector3f normal_vec = user_vec.normalized().cast(); 240 | double d = -center.dot(normal_vec); 241 | pcl::ModelCoefficients::Ptr coefficients = boost::make_shared(); 242 | coefficients->values.resize(4); 243 | coefficients->values[0] = normal_vec(0); 244 | coefficients->values[1] = normal_vec(1); 245 | coefficients->values[2] = normal_vec(2); 246 | coefficients->values[3] = d; 247 | 248 | // projecting onto plane 249 | pcl::ProjectInliers project_inliers; 250 | project_inliers.setModelType(pcl::SACMODEL_NORMAL_PLANE); 251 | project_inliers.setInputCloud(region_cloud_3d); 252 | project_inliers.setModelCoefficients(coefficients); 253 | project_inliers.filter(*planar_hull); 254 | 255 | return planar_hull; 256 | } 257 | 258 | template 259 | void scaleCloud(const double scale_factor, typename pcl::PointCloud& cloud) 260 | { 261 | Eigen::Vector4f centroid; 262 | pcl::PointCloud demeaned_cloud; 263 | pcl::compute3DCentroid(cloud, centroid); 264 | pcl::demeanPointCloud(cloud, centroid, demeaned_cloud); 265 | 266 | // scaling all points now 267 | for (std::size_t i = 0; i < demeaned_cloud.size(); i++) 268 | { 269 | PointT p = demeaned_cloud[i]; 270 | p.x = scale_factor * p.x; 271 | p.y = scale_factor * p.y; 272 | p.z = scale_factor * p.z; 273 | demeaned_cloud[i] = p; 274 | } 275 | 276 | // transforming demeaned cloud back to original centroid 277 | Eigen::Affine3f transform = pcl::getTransformation(centroid.x(), centroid.y(), centroid.z(), 0, 0, 0); 278 | pcl::transformPointCloud(demeaned_cloud, cloud, transform); 279 | } 280 | 281 | namespace region_detection_core 282 | { 283 | template 284 | RegionCrop::RegionCrop() : input_(nullptr) 285 | { 286 | } 287 | 288 | template 289 | RegionCrop::~RegionCrop() 290 | { 291 | } 292 | 293 | template 294 | inline void RegionCrop::setRegion(const EigenPose3dVector_HIDDEN& closed_region) 295 | { 296 | using namespace Eigen; 297 | Vector3d p0, pf; 298 | p0 = closed_region.front().translation(); 299 | pf = closed_region.back().translation(); 300 | 301 | double diff = (pf - p0).norm(); 302 | if (diff > EPSILON) 303 | { 304 | throw std::runtime_error("region end points are too far from each other, region isn't closed"); 305 | } 306 | closed_region_ = closed_region; 307 | } 308 | 309 | template 310 | inline void RegionCrop::setConfig(const RegionCropConfig& config) 311 | { 312 | // check method 313 | static const std::vector valid_options = { DirectionEstMethods::NORMAL_AVGR, 314 | DirectionEstMethods::PLANE_NORMAL, 315 | DirectionEstMethods::POSE_Z_AXIS, 316 | DirectionEstMethods::USER_DEFINED }; 317 | DirectionEstMethods method_flag = config.dir_estimation_method; 318 | if (std::find(valid_options.begin(), valid_options.end(), method_flag) == valid_options.end()) 319 | { 320 | std::string err_msg = boost::str(boost::format("The flag %i is not a supported Direction Estimation Method") % 321 | static_cast(config.dir_estimation_method)); 322 | throw std::runtime_error(err_msg); 323 | } 324 | 325 | // check height limits 326 | if (config.heigth_limits.first >= config.heigth_limits.second) 327 | { 328 | throw std::runtime_error("Height Limits min value is greater than or equal to max"); 329 | } 330 | 331 | config_ = config; 332 | } 333 | 334 | template 335 | void RegionCrop::setInput(const typename pcl::PointCloud::ConstPtr& cloud) 336 | { 337 | input_ = cloud; 338 | } 339 | 340 | template 341 | std::vector region_detection_core::RegionCrop::filter(bool reverse) 342 | { 343 | using namespace pcl; 344 | if (!input_) 345 | { 346 | throw std::runtime_error("Input cloud pointer is null"); 347 | } 348 | 349 | // creating planar hull 350 | PointCloud::Ptr planar_hull = boost::make_shared>(); 351 | 352 | switch (config_.dir_estimation_method) 353 | { 354 | case DirectionEstMethods::NORMAL_AVGR: 355 | planar_hull = computePlanarHullFromNormals(closed_region_); 356 | break; 357 | case DirectionEstMethods::PLANE_NORMAL: 358 | planar_hull = computePlanarHullFromPlane(closed_region_); 359 | break; 360 | case DirectionEstMethods::POSE_Z_AXIS: 361 | planar_hull = computePlanarHullFromZVector(closed_region_); 362 | break; 363 | case DirectionEstMethods::USER_DEFINED: 364 | planar_hull = computePlanarHullFromUserVector(closed_region_, config_.user_dir); 365 | break; 366 | default: 367 | std::string err_msg = boost::str(boost::format("Direction Estimation Method %i is not supported") % 368 | static_cast(config_.dir_estimation_method)); 369 | throw std::runtime_error(err_msg); 370 | } 371 | 372 | // scaling planar hull 373 | scaleCloud(config_.scale_factor, *planar_hull); 374 | 375 | // extracting region within polygon 376 | PointIndices inlier_indices; 377 | typename PointCloud::Ptr planar_hull_t = boost::make_shared>(); 378 | pcl::copyPointCloud(*planar_hull, *planar_hull_t); 379 | ExtractPolygonalPrismData extract_polygon; 380 | extract_polygon.setHeightLimits(config_.heigth_limits.first, config_.heigth_limits.second); 381 | extract_polygon.setViewPoint(config_.view_point.x(), config_.view_point.y(), config_.view_point.z()); 382 | extract_polygon.setInputPlanarHull(planar_hull_t); 383 | extract_polygon.setInputCloud(input_); 384 | extract_polygon.segment(inlier_indices); 385 | 386 | std::vector indices_vec = inlier_indices.indices; 387 | if (reverse) 388 | { 389 | std::vector all_indices_vec, diff; 390 | all_indices_vec.resize(input_->size()); 391 | std::iota(all_indices_vec.begin(), all_indices_vec.end(), 0); 392 | std::sort(indices_vec.begin(), indices_vec.end()); 393 | std::set_difference(all_indices_vec.begin(), 394 | all_indices_vec.end(), 395 | indices_vec.begin(), 396 | indices_vec.end(), 397 | std::inserter(diff, diff.begin())); 398 | indices_vec = diff; 399 | } 400 | 401 | return indices_vec; 402 | } 403 | 404 | #define PCL_INSTANTIATE_RegionCrop(T) template class PCL_EXPORTS RegionCrop; 405 | 406 | PCL_INSTANTIATE(RegionCrop, PCL_XYZ_POINT_TYPES); 407 | 408 | } /* namespace region_detection_core */ 409 | -------------------------------------------------------------------------------- /region_detection_core/src/region_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file region_detector.cpp 4 | * @date Jun 4, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #include "region_detection_core/region_detector.h" 57 | 58 | static const std::map DILATION_TYPES = { { 0, cv::MORPH_RECT }, 59 | { 1, cv::MORPH_CROSS }, 60 | { 2, cv::MORPH_ELLIPSE } }; 61 | static cv::RNG RANDOM_NUM_GEN(12345); 62 | static const int MIN_PIXEL_DISTANCE = 1; // used during interpolation in pixel space 63 | static const double MIN_POINT_DIST = 1e-8; 64 | 65 | log4cxx::LoggerPtr createDefaultLogger(const std::string& logger_name) 66 | { 67 | using namespace log4cxx; 68 | PatternLayoutPtr pattern_layout(new PatternLayout("[\%-5p] [\%c](L:\%L): \%m\%n")); 69 | ConsoleAppenderPtr console_appender(new ConsoleAppender(pattern_layout)); 70 | log4cxx::LoggerPtr logger(Logger::getLogger(logger_name)); 71 | logger->addAppender(console_appender); 72 | logger->setLevel(Level::getInfo()); 73 | return logger; 74 | } 75 | 76 | Eigen::Matrix3d toRotationMatrix(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) 77 | { 78 | using namespace Eigen; 79 | Matrix3d rot; 80 | rot.block(0, 0, 1, 3) = Vector3d(vx.x(), vy.x(), vz.x()).array().transpose(); 81 | rot.block(1, 0, 1, 3) = Vector3d(vx.y(), vy.y(), vz.y()).array().transpose(); 82 | rot.block(2, 0, 1, 3) = Vector3d(vx.z(), vy.z(), vz.z()).array().transpose(); 83 | return rot; 84 | } 85 | 86 | template 87 | std::vector linspace(T a, T b, size_t N) 88 | { 89 | T h = (b - a) / static_cast(N - 1); 90 | std::vector xs(N); 91 | typename std::vector::iterator x; 92 | T val; 93 | for (x = xs.begin(), val = a; x != xs.end(); ++x, val += h) 94 | *x = val; 95 | return xs; 96 | } 97 | 98 | /** 99 | * Perform one thinning iteration. 100 | * Normally you wouldn't call this function directly from your code. 101 | * 102 | * @param im Binary image with range = 0-1 103 | * @param iter 0=even, 1=odd 104 | */ 105 | void thinningGuoHallIteration(cv::Mat& im, int iter) 106 | { 107 | cv::Mat marker = cv::Mat::zeros(im.size(), CV_8UC1); 108 | 109 | for (int i = 1; i < im.rows; i++) 110 | { 111 | for (int j = 1; j < im.cols; j++) 112 | { 113 | uchar p2 = im.at(i - 1, j); 114 | uchar p3 = im.at(i - 1, j + 1); 115 | uchar p4 = im.at(i, j + 1); 116 | uchar p5 = im.at(i + 1, j + 1); 117 | uchar p6 = im.at(i + 1, j); 118 | uchar p7 = im.at(i + 1, j - 1); 119 | uchar p8 = im.at(i, j - 1); 120 | uchar p9 = im.at(i - 1, j - 1); 121 | 122 | int C = (!p2 & (p3 | p4)) + (!p4 & (p5 | p6)) + (!p6 & (p7 | p8)) + (!p8 & (p9 | p2)); 123 | int N1 = (p9 | p2) + (p3 | p4) + (p5 | p6) + (p7 | p8); 124 | int N2 = (p2 | p3) + (p4 | p5) + (p6 | p7) + (p8 | p9); 125 | int N = N1 < N2 ? N1 : N2; 126 | int m = iter == 0 ? ((p6 | p7 | !p9) & p8) : ((p2 | p3 | !p5) & p4); 127 | 128 | if (C == 1 && (N >= 2 && N <= 3) & m == 0) 129 | marker.at(i, j) = 1; 130 | } 131 | } 132 | 133 | im &= ~marker; 134 | } 135 | 136 | void thinningGuoHall(cv::Mat& im) 137 | { 138 | im /= 255; 139 | 140 | cv::Mat prev = cv::Mat::zeros(im.size(), CV_8UC1); 141 | cv::Mat diff; 142 | 143 | do 144 | { 145 | thinningGuoHallIteration(im, 0); 146 | thinningGuoHallIteration(im, 1); 147 | cv::absdiff(im, prev, diff); 148 | im.copyTo(prev); 149 | } while (cv::countNonZero(diff) > 0); 150 | 151 | im *= 255; 152 | } 153 | 154 | pcl::PointCloud convert2DContourToCloud(const std::vector& contour_2d) 155 | { 156 | pcl::PointCloud contour_3d; 157 | pcl::PointXYZ p_3d; 158 | for (const auto& p_2d : contour_2d) 159 | { 160 | p_3d.x = p_2d.x; 161 | p_3d.y = p_2d.y; 162 | p_3d.z = 0.0; 163 | contour_3d.push_back(p_3d); 164 | } 165 | return contour_3d; 166 | } 167 | 168 | std::vector convertCloudTo2DContour(const pcl::PointCloud& contour_3d) 169 | { 170 | std::vector contour_2d; 171 | cv::Point p_2d; 172 | for (const auto& p_3d : contour_3d) 173 | { 174 | p_2d.x = p_3d.x; 175 | p_2d.y = p_3d.y; 176 | contour_2d.push_back(p_2d); 177 | } 178 | return contour_2d; 179 | } 180 | 181 | template 182 | void removeInfinite(pcl::PointCloud& cloud) 183 | { 184 | using namespace pcl; 185 | 186 | PointIndices::Ptr inliers = boost::make_shared(); 187 | std::vector finite_indices; 188 | for (std::size_t i = 0; i < cloud.size(); i++) 189 | { 190 | if (isFinite(cloud[i])) 191 | { 192 | inliers->indices.push_back(i); 193 | } 194 | } 195 | pcl::ExtractIndices extract; 196 | PointCloud finite_cloud; 197 | auto temp_cloud = cloud.makeShared(); 198 | extract.setInputCloud(temp_cloud); 199 | extract.setIndices(inliers); 200 | extract.setNegative(false); 201 | extract.filter(finite_cloud); 202 | copyPointCloud(finite_cloud, cloud); 203 | } 204 | 205 | void dowsampleCloud(pcl::PointCloud& cloud, double leafsize = 1.0) 206 | { 207 | using namespace pcl; 208 | VoxelGrid voxelizer; 209 | voxelizer.setLeafSize(leafsize, leafsize, leafsize); 210 | voxelizer.setInputCloud(cloud.makeShared()); 211 | cloud.clear(); 212 | voxelizer.filter(cloud); 213 | } 214 | 215 | pcl::PointCloud::Ptr 216 | concaveHullSimplification(const pcl::PointCloud::ConstPtr closed_polygon, double segment_length) 217 | { 218 | pcl::PointCloud::Ptr simplified_polygon = boost::make_shared>(); 219 | pcl::ConcaveHull chull; 220 | chull.setInputCloud(closed_polygon); 221 | chull.setAlpha(segment_length); 222 | chull.reconstruct(*simplified_polygon); 223 | return simplified_polygon; 224 | } 225 | 226 | namespace region_detection_core 227 | { 228 | RegionDetectionConfig RegionDetectionConfig::loadFromFile(const std::string& yaml_file) 229 | { 230 | YAML::Node root = YAML::LoadFile(yaml_file); 231 | std::stringstream yaml_stream; 232 | yaml_stream << root; 233 | return load(yaml_stream.str()); 234 | } 235 | 236 | RegionDetectionConfig RegionDetectionConfig::load(const std::string& yaml_str) 237 | { 238 | RegionDetectionConfig cfg; 239 | { 240 | YAML::Node root = YAML::Load(yaml_str); 241 | 242 | YAML::Node opencv_node = root["opencv"]; 243 | RegionDetectionConfig::OpenCVCfg& opencv_cfg = cfg.opencv_cfg; 244 | 245 | opencv_cfg.methods = opencv_node["methods"].as>(); 246 | 247 | opencv_cfg.threshold.type = opencv_node["threshold"]["type"].as(); 248 | opencv_cfg.threshold.value = opencv_node["threshold"]["value"].as(); 249 | 250 | opencv_cfg.canny.lower_threshold = opencv_node["canny"]["lower_threshold"].as(); 251 | opencv_cfg.canny.upper_threshold = opencv_node["canny"]["upper_threshold"].as(); 252 | opencv_cfg.canny.aperture_size = opencv_node["canny"]["aperture_size"].as(); 253 | 254 | opencv_cfg.dilation.elem = opencv_node["dilation"]["elem"].as(); 255 | opencv_cfg.dilation.kernel_size = opencv_node["dilation"]["kernel_size"].as(); 256 | 257 | opencv_cfg.erosion.elem = opencv_node["erosion"]["elem"].as(); 258 | opencv_cfg.erosion.kernel_size = opencv_node["erosion"]["kernel_size"].as(); 259 | 260 | opencv_cfg.contour.method = opencv_node["contour"]["method"].as(); 261 | opencv_cfg.contour.mode = opencv_node["contour"]["mode"].as(); 262 | 263 | opencv_cfg.range.low = opencv_node["range"]["low"].as(); 264 | opencv_cfg.range.high = opencv_node["range"]["high"].as(); 265 | 266 | std::vector h_vals = opencv_node["hsv"]["h"].as>(); 267 | std::vector s_vals = opencv_node["hsv"]["s"].as>(); 268 | std::vector v_vals = opencv_node["hsv"]["v"].as>(); 269 | std::copy(h_vals.begin(), h_vals.end(), opencv_cfg.hsv.h.begin()); 270 | std::copy(s_vals.begin(), s_vals.end(), opencv_cfg.hsv.s.begin()); 271 | std::copy(v_vals.begin(), v_vals.end(), opencv_cfg.hsv.v.begin()); 272 | 273 | opencv_cfg.debug_mode_enable = opencv_node["debug_mode_enable"].as(); 274 | opencv_cfg.debug_window_name = opencv_node["debug_window_name"].as(); 275 | opencv_cfg.debug_wait_key = opencv_node["debug_wait_key"].as(); 276 | 277 | opencv_cfg.clahe.clip_limit = opencv_node["clahe"]["clip_limit"].as(); 278 | std::vector tile_size_vals = opencv_node["clahe"]["tile_grid_size"].as>(); 279 | std::copy(tile_size_vals.begin(), tile_size_vals.end(), opencv_cfg.clahe.tile_grid_size.begin()); 280 | 281 | YAML::Node pcl2d_node = root["pcl2d"]; 282 | RegionDetectionConfig::PCL2DCfg& pcl2d_cfg = cfg.pcl_2d_cfg; 283 | pcl2d_cfg.downsampling_radius = pcl2d_node["downsampling_radius"].as(); 284 | pcl2d_cfg.split_dist = pcl2d_node["split_dist"].as(); 285 | pcl2d_cfg.closed_curve_max_dist = pcl2d_node["closed_curve_max_dist"].as(); 286 | pcl2d_cfg.simplification_min_points = pcl2d_node["simplification_min_points"].as(); 287 | pcl2d_cfg.simplification_alpha = pcl2d_node["simplification_alpha"].as(); 288 | 289 | YAML::Node pcl_node = root["pcl"]; 290 | RegionDetectionConfig::PCLCfg& pcl_cfg = cfg.pcl_cfg; 291 | pcl_cfg.debug_mode_enable = pcl_node["debug_mode_enable"].as(); 292 | pcl_cfg.max_merge_dist = pcl_node["max_merge_dist"].as(); 293 | pcl_cfg.closed_curve_max_dist = pcl_node["closed_curve_max_dist"].as(); 294 | pcl_cfg.simplification_min_dist = pcl_node["simplification_min_dist"].as(); 295 | pcl_cfg.split_dist = pcl_node["split_dist"].as(); 296 | pcl_cfg.min_num_points = pcl_node["min_num_points"].as(); 297 | 298 | pcl_cfg.stat_removal.enable = pcl_node["stat_removal"]["enable"].as(); 299 | pcl_cfg.stat_removal.kmeans = pcl_node["stat_removal"]["kmeans"].as(); 300 | pcl_cfg.stat_removal.stddev = pcl_node["stat_removal"]["stddev"].as(); 301 | 302 | pcl_cfg.normal_est.kdtree_epsilon = pcl_node["normal_est"]["kdtree_epsilon"].as(); 303 | pcl_cfg.normal_est.search_radius = pcl_node["normal_est"]["search_radius"].as(); 304 | std::vector viewpoint_vals = pcl_node["normal_est"]["viewpoint_xyz"].as>(); 305 | pcl_cfg.normal_est.downsampling_radius = pcl_node["normal_est"]["downsampling_radius"].as(); 306 | std::copy(viewpoint_vals.begin(), viewpoint_vals.end(), pcl_cfg.normal_est.viewpoint_xyz.begin()); 307 | } 308 | return cfg; 309 | } 310 | 311 | pcl::PointCloud RegionDetector::sequence(pcl::PointCloud::ConstPtr points, double epsilon) 312 | { 313 | using namespace pcl; 314 | std::vector::Ptr> sequenced_points_vec; 315 | 316 | // build reordering kd tree 317 | pcl::KdTreeFLANN sequencing_kdtree; 318 | sequencing_kdtree.setEpsilon(epsilon); 319 | sequencing_kdtree.setSortedResults(true); 320 | 321 | auto& cloud = *points; 322 | std::vector sequenced_indices, unsequenced_indices; 323 | sequenced_indices.reserve(cloud.size()); 324 | unsequenced_indices.resize(cloud.size()); 325 | std::iota(unsequenced_indices.begin(), unsequenced_indices.end(), 0); 326 | 327 | // cloud of all sequenced points 328 | pcl::PointCloud sequenced_points; 329 | sequenced_points.reserve(cloud.size()); 330 | 331 | // search variables 332 | int search_point_idx = 0; 333 | const int max_iters = cloud.size(); 334 | PointXYZ search_point = cloud[search_point_idx]; 335 | PointXYZ start_point = search_point; 336 | PointXYZ closest_point; 337 | int iter_count = 0; 338 | 339 | // now reorder based on proximity 340 | while (iter_count <= max_iters) 341 | { 342 | iter_count++; 343 | 344 | // remove from active 345 | unsequenced_indices.erase(std::remove(unsequenced_indices.begin(), unsequenced_indices.end(), search_point_idx)); 346 | 347 | if (unsequenced_indices.empty()) 348 | { 349 | break; 350 | } 351 | 352 | // set tree inputs; 353 | IndicesConstPtr cloud_indices = boost::make_shared>(unsequenced_indices); 354 | sequencing_kdtree.setInputCloud(points, cloud_indices); 355 | sequencing_kdtree.setSortedResults(true); 356 | 357 | // find next point 358 | const int k_points = 1; 359 | std::vector k_indices(k_points); 360 | std::vector k_sqr_distances(k_points); 361 | int points_found = sequencing_kdtree.nearestKSearch(search_point, k_points, k_indices, k_sqr_distances); 362 | if (points_found < k_points) 363 | { 364 | std::string err_msg = boost::str(boost::format("NearestKSearch Search did not find any points close to [%f, %f, " 365 | "%f]") % 366 | search_point.x % search_point.y % search_point.z); 367 | LOG4CXX_WARN(logger_, err_msg); 368 | break; 369 | } 370 | // saving search point 371 | if (sequenced_indices.empty()) 372 | { 373 | sequenced_indices.push_back(search_point_idx); // insert first point 374 | sequenced_points.push_back(search_point); 375 | } 376 | 377 | // insert new point if it has not been visited 378 | if (std::find(sequenced_indices.begin(), sequenced_indices.end(), k_indices[0]) != sequenced_indices.end()) 379 | { 380 | // there should be no more points to add 381 | LOG4CXX_WARN(logger_, "Found repeated point during reordering stage, should not happen but proceeding"); 382 | continue; 383 | } 384 | 385 | // check if found point is further away than the start point 386 | closest_point = cloud[k_indices[0]]; 387 | start_point = cloud[sequenced_indices[0]]; 388 | Eigen::Vector3d diff = (start_point.getArray3fMap() - closest_point.getArray3fMap()).cast(); 389 | if (diff.norm() < std::sqrt(k_sqr_distances[0])) 390 | { 391 | // reversing will allow adding point from the other end 392 | std::reverse(sequenced_indices.begin(), sequenced_indices.end()); 393 | std::reverse(sequenced_points.begin(), sequenced_points.end()); 394 | } 395 | 396 | // set next search point variables 397 | search_point_idx = k_indices[0]; 398 | search_point = closest_point; 399 | 400 | // add to visited 401 | sequenced_indices.push_back(k_indices[0]); 402 | sequenced_points.push_back(closest_point); 403 | } 404 | 405 | LOG4CXX_DEBUG(logger_, "Sequenced " << sequenced_points.size() << " points from " << cloud.size()); 406 | return sequenced_points; 407 | } 408 | 409 | std::vector::Ptr> 410 | RegionDetector::split(const pcl::PointCloud& sequenced_points, double split_dist) 411 | { 412 | using namespace pcl; 413 | 414 | // splitting 415 | std::vector::Ptr> sequenced_points_vec; 416 | std::size_t start_idx = 0; 417 | std::size_t end_idx; 418 | for (std::size_t i = 0; i < sequenced_points.size(); i++) 419 | { 420 | end_idx = i; 421 | if (i < sequenced_points.size() - 1) 422 | { 423 | const PointXYZ& p_current = sequenced_points[i]; 424 | const PointXYZ& p_next = sequenced_points[i + 1]; 425 | Eigen::Vector3d diff = (p_next.getArray3fMap() - p_current.getArray3fMap()).cast(); 426 | if ((diff.norm() < split_dist)) 427 | { 428 | continue; 429 | } 430 | } 431 | 432 | // save segment 433 | PointCloud::Ptr segment_points = boost::make_shared>(); 434 | for (std::size_t p_idx = start_idx; p_idx <= end_idx; p_idx++) 435 | { 436 | auto& p_current = sequenced_points[p_idx]; 437 | if (p_idx > start_idx) 438 | { 439 | auto& p_prev = segment_points->back(); 440 | Eigen::Vector3d diff = (p_current.getArray3fMap() - p_prev.getArray3fMap()).cast(); 441 | if (diff.norm() < MIN_POINT_DIST) 442 | { 443 | // too close do not add 444 | continue; 445 | } 446 | } 447 | segment_points->push_back(p_current); 448 | } 449 | 450 | LOG4CXX_DEBUG(logger_, 451 | "Creating sequence [" << start_idx << ", " << end_idx << "] with " << segment_points->size() 452 | << " points"); 453 | if (segment_points->size() == 0) 454 | { 455 | LOG4CXX_DEBUG(logger_, "Ignoring empty segment"); 456 | continue; 457 | } 458 | sequenced_points_vec.push_back(segment_points); 459 | start_idx = i + 1; 460 | } 461 | 462 | LOG4CXX_DEBUG(logger_, 463 | "Computed " << sequenced_points_vec.size() << " sequences from a segment of " << sequenced_points.size() 464 | << " points"); 465 | return sequenced_points_vec; 466 | } 467 | 468 | void RegionDetector::findClosedCurves(const std::vector::Ptr>& sequenced_curves_vec, 469 | double max_dist, 470 | std::vector::Ptr>& closed_curves_vec, 471 | std::vector::Ptr>& open_curves_vec) 472 | { 473 | // check if closed, it is assumed that the points have already been sequenced 474 | for (pcl::PointCloud::Ptr curve_points : sequenced_curves_vec) 475 | { 476 | Eigen::Vector3d diff = 477 | (curve_points->front().getArray3fMap() - curve_points->back().getArray3fMap()).cast(); 478 | if (diff.norm() < max_dist) 479 | { 480 | // copying first point to end of cloud to close the curve 481 | // TODO: Evaluate if line below line should be restored 482 | // curve_points->push_back(curve_points->front()); 483 | 484 | // saving 485 | closed_curves_vec.push_back(curve_points); 486 | 487 | LOG4CXX_DEBUG(logger_, "Found closed curve with " << curve_points->size() << " points"); 488 | } 489 | else 490 | { 491 | open_curves_vec.push_back(curve_points); 492 | LOG4CXX_DEBUG(logger_, "Found open curve with " << curve_points->size() << " points"); 493 | } 494 | } 495 | } 496 | 497 | RegionDetector::RegionDetector(log4cxx::LoggerPtr logger) 498 | : logger_(logger ? logger : createDefaultInfoLogger("Default")) 499 | { 500 | RegionDetectionConfig cfg; 501 | if (!configure(cfg)) 502 | { 503 | throw std::runtime_error("Invalid configuration"); 504 | } 505 | } 506 | 507 | RegionDetector::~RegionDetector() {} 508 | 509 | RegionDetector::RegionDetector(const RegionDetectionConfig& config, log4cxx::LoggerPtr logger) 510 | : logger_(logger ? logger : createDefaultInfoLogger("Default")) 511 | { 512 | if (!configure(config)) 513 | { 514 | throw std::runtime_error("Invalid configuration"); 515 | } 516 | } 517 | 518 | log4cxx::LoggerPtr RegionDetector::createDefaultInfoLogger(const std::string& logger_name) 519 | { 520 | using namespace log4cxx; 521 | PatternLayoutPtr pattern_layout(new PatternLayout("[\%-5p] [\%c](L:\%L): \%m\%n")); 522 | ConsoleAppenderPtr console_appender(new ConsoleAppender(pattern_layout)); 523 | log4cxx::LoggerPtr logger(Logger::getLogger(logger_name)); 524 | logger->addAppender(console_appender); 525 | logger->setLevel(Level::getInfo()); 526 | return logger; 527 | } 528 | 529 | log4cxx::LoggerPtr RegionDetector::createDefaultDebugLogger(const std::string& logger_name) 530 | { 531 | using namespace log4cxx; 532 | PatternLayoutPtr pattern_layout(new PatternLayout("[\%-5p] [\%c](L:\%L): \%m\%n")); 533 | ConsoleAppenderPtr console_appender(new ConsoleAppender(pattern_layout)); 534 | log4cxx::LoggerPtr logger(Logger::getLogger(logger_name)); 535 | logger->addAppender(console_appender); 536 | logger->setLevel(Level::getDebug()); 537 | return logger; 538 | } 539 | 540 | bool RegionDetector::configure(const RegionDetectionConfig& config) 541 | { 542 | cfg_ = std::make_shared(config); 543 | return cfg_ != nullptr; 544 | } 545 | 546 | bool RegionDetector::configureFromFile(const std::string& yaml_file) 547 | { 548 | return configure(RegionDetectionConfig::loadFromFile(yaml_file)); 549 | } 550 | 551 | bool RegionDetector::configure(const std::string& yaml_str) { return configure(RegionDetectionConfig::load(yaml_str)); } 552 | 553 | log4cxx::LoggerPtr RegionDetector::getLogger() { return logger_; } 554 | 555 | const RegionDetectionConfig& RegionDetector::getConfig() { return *cfg_; } 556 | 557 | void RegionDetector::updateDebugWindow(const cv::Mat& im) const 558 | { 559 | using namespace cv; 560 | const RegionDetectionConfig::OpenCVCfg& opencv_cfg = cfg_->opencv_cfg; 561 | 562 | if (!opencv_cfg.debug_mode_enable) 563 | { 564 | return; 565 | } 566 | 567 | // check if window is open 568 | const std::string wname = opencv_cfg.debug_window_name + std::to_string(window_counter_); 569 | if (cv::getWindowProperty(wname, cv::WND_PROP_VISIBLE) <= 0) 570 | { 571 | // create window then 572 | namedWindow(wname, WINDOW_AUTOSIZE); 573 | } 574 | 575 | imshow(wname, im.clone()); 576 | cv::waitKey(100); 577 | 578 | if (opencv_cfg.debug_wait_key) 579 | { 580 | cv::waitKey(); 581 | } 582 | } 583 | 584 | RegionDetector::Result RegionDetector::apply2dGrayscale(cv::Mat input, cv::Mat& output) const 585 | { 586 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 587 | 588 | if (input.channels() == 1) 589 | { 590 | LOG4CXX_WARN(logger_, "Input image is already of one channel, skipping Grayscale Conversion"); 591 | return true; 592 | } 593 | 594 | cv::cvtColor(input, output, cv::COLOR_BGR2GRAY, 1); 595 | LOG4CXX_DEBUG(logger_, "2D analysis: Grayscale Conversion"); 596 | return true; 597 | } 598 | 599 | RegionDetector::Result RegionDetector::apply2dRange(cv::Mat input, cv::Mat& output) const 600 | { 601 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 602 | cv::inRange(input.clone(), cv::Scalar(config.range.low), cv::Scalar(config.range.high), output); 603 | return true; 604 | } 605 | 606 | RegionDetector::Result RegionDetector::apply2dHSV(cv::Mat input, cv::Mat& output) const 607 | { 608 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 609 | 610 | cv::cvtColor(input, output, cv::COLOR_BGR2HSV); 611 | cv::Mat frame_threshold; 612 | inRange(output, 613 | cv::Scalar(config.hsv.h[0], config.hsv.s[0], config.hsv.v[0]), 614 | cv::Scalar(config.hsv.h[1], config.hsv.s[1], config.hsv.v[1]), 615 | frame_threshold); 616 | output = frame_threshold; 617 | return true; 618 | } 619 | 620 | RegionDetector::Result RegionDetector::apply2dEqualizeHistYUV(cv::Mat input, cv::Mat& output) const 621 | { 622 | cv::cvtColor(input, output, CV_BGR2YUV); 623 | std::vector channels; 624 | cv::split(input, channels); 625 | cv::equalizeHist(channels[0], channels[0]); 626 | cv::merge(channels, output); 627 | cv::cvtColor(output, output, CV_YUV2BGR); 628 | return true; 629 | } 630 | 631 | RegionDetector::Result RegionDetector::apply2dEqualizeHist(cv::Mat input, cv::Mat& output) const 632 | { 633 | std::vector channels; 634 | cv::split(input, channels); 635 | cv::equalizeHist(channels[0], channels[0]); 636 | cv::merge(channels, output); 637 | return true; 638 | } 639 | 640 | RegionDetector::Result RegionDetector::apply2dCLAHE(cv::Mat input, cv::Mat& output) const 641 | { 642 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 643 | auto clahe = cv::createCLAHE(config.clahe.clip_limit, 644 | cv::Size(config.clahe.tile_grid_size[0], config.clahe.tile_grid_size[1])); 645 | clahe->apply(input, output); 646 | return true; 647 | } 648 | 649 | RegionDetector::Result RegionDetector::apply2dInvert(cv::Mat input, cv::Mat& output) const 650 | { 651 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 652 | cv::Mat inverted = cv::Scalar_(255) - input; 653 | output = inverted.clone(); 654 | LOG4CXX_ERROR(logger_, "2D analysis: Inversion"); 655 | return true; 656 | } 657 | 658 | RegionDetector::Result RegionDetector::apply2dThreshold(cv::Mat input, cv::Mat& output) const 659 | { 660 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 661 | cv::threshold(input, output, config.threshold.value, config.threshold.MAX_BINARY_VALUE, config.threshold.type); 662 | LOG4CXX_ERROR(logger_, "2D analysis: threshold with value of " << config.threshold.value); 663 | return true; 664 | } 665 | 666 | RegionDetector::Result RegionDetector::apply2dDilation(cv::Mat input, cv::Mat& output) const 667 | { 668 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 669 | bool success; 670 | std::string err_msg; 671 | 672 | // check values 673 | if (config.dilation.kernel_size <= 0) 674 | { 675 | success = false; 676 | err_msg = "invalid dilation size"; 677 | LOG4CXX_ERROR(logger_, err_msg) 678 | return Result(success, err_msg); 679 | } 680 | 681 | // select dilation type 682 | if (DILATION_TYPES.count(config.dilation.elem) == 0) 683 | { 684 | success = false; 685 | err_msg = "invalid dilation element"; 686 | LOG4CXX_ERROR(logger_, err_msg) 687 | return Result(success, err_msg); 688 | } 689 | int dilation_type = DILATION_TYPES.at(config.dilation.elem); 690 | cv::Mat element = 691 | cv::getStructuringElement(dilation_type, 692 | cv::Size(2 * config.dilation.kernel_size + 1, 2 * config.dilation.kernel_size + 1), 693 | cv::Point(config.dilation.kernel_size, config.dilation.kernel_size)); 694 | cv::dilate(input, output, element); 695 | return true; 696 | } 697 | 698 | RegionDetector::Result RegionDetector::apply2dErosion(cv::Mat input, cv::Mat& output) const 699 | { 700 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 701 | bool success; 702 | std::string err_msg; 703 | 704 | // check values 705 | if (config.dilation.kernel_size <= 0) 706 | { 707 | success = false; 708 | err_msg = "invalid dilation size"; 709 | LOG4CXX_ERROR(logger_, err_msg) 710 | return Result(success, err_msg); 711 | } 712 | 713 | // select dilation type 714 | if (DILATION_TYPES.count(config.dilation.elem) == 0) 715 | { 716 | success = false; 717 | err_msg = "invalid dilation element"; 718 | LOG4CXX_ERROR(logger_, err_msg) 719 | return Result(success, err_msg); 720 | } 721 | int dilation_type = DILATION_TYPES.at(config.dilation.elem); 722 | cv::Mat element = 723 | cv::getStructuringElement(dilation_type, 724 | cv::Size(2 * config.erosion.kernel_size + 1, 2 * config.erosion.kernel_size + 1), 725 | cv::Point(config.erosion.kernel_size, config.erosion.kernel_size)); 726 | cv::erode(input, output, element); 727 | return true; 728 | } 729 | 730 | RegionDetector::Result RegionDetector::apply2dCanny(cv::Mat input, cv::Mat& output) const 731 | { 732 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 733 | cv::Mat detected_edges; 734 | int aperture_size = 2 * config.canny.aperture_size + 1; 735 | aperture_size = aperture_size < 3 ? 3 : aperture_size; 736 | cv::Canny(input, detected_edges, config.canny.lower_threshold, config.canny.upper_threshold, aperture_size, true); 737 | output = detected_edges.clone(); 738 | return true; 739 | } 740 | 741 | RegionDetector::Result RegionDetector::compute2dContours(cv::Mat input, 742 | std::vector>& contours_indices, 743 | cv::Mat& output) const 744 | { 745 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 746 | 747 | Result res = compute2d(input, output); 748 | if (!res) 749 | { 750 | return res; 751 | } 752 | 753 | // ======================== Contour Detection ======================== 754 | std::vector hierarchy; 755 | try 756 | { 757 | cv::findContours(output.clone(), contours_indices, hierarchy, config.contour.mode, config.contour.method); 758 | } 759 | catch (cv::Exception& ex) 760 | { 761 | return Result(false, boost::str(boost::format("Failed finding contours with error: %s") % ex.what())); 762 | } 763 | 764 | cv::Mat drawing = cv::Mat::zeros(output.size(), CV_8UC3); 765 | LOG4CXX_INFO(logger_, "Contour analysis found " << contours_indices.size() << " contours"); 766 | for (int i = 0; i < contours_indices.size(); i++) 767 | { 768 | cv::Scalar color = 769 | cv::Scalar(RANDOM_NUM_GEN.uniform(0, 255), RANDOM_NUM_GEN.uniform(0, 255), RANDOM_NUM_GEN.uniform(0, 255)); 770 | double area = cv::contourArea(contours_indices[i]); 771 | double arc_length = cv::arcLength(contours_indices[i], false); 772 | cv::drawContours(drawing, contours_indices, i, color, 2, 8, hierarchy, 0, cv::Point()); 773 | std::string contour_summary = boost::str(boost::format("c[%i]: s: %i, area: %f, arc %f; (p0: %i, pf: %i); h: %i") % 774 | i % contours_indices[i].size() % area % arc_length % 775 | contours_indices[i].front() % contours_indices[i].back() % hierarchy[i]); 776 | } 777 | updateDebugWindow(drawing); 778 | 779 | output = drawing.clone(); 780 | LOG4CXX_DEBUG(logger_, "Completed 2D analysis"); 781 | return true; 782 | } 783 | 784 | bool RegionDetector::compute2d(cv::Mat input, cv::Mat& output) const 785 | { 786 | namespace ph = std::placeholders; 787 | using Func2D = std::function; 788 | 789 | bool success = false; 790 | std::string err_msg; 791 | output = input; 792 | const RegionDetectionConfig::OpenCVCfg& config = cfg_->opencv_cfg; 793 | 794 | std::map function_mappings = { 795 | { Methods2D::GRAYSCALE, std::bind(&RegionDetector::apply2dGrayscale, this, ph::_1, ph::_2) }, 796 | { Methods2D::INVERT, std::bind(&RegionDetector::apply2dInvert, this, ph::_1, ph::_2) }, 797 | { Methods2D::THRESHOLD, std::bind(&RegionDetector::apply2dThreshold, this, ph::_1, ph::_2) }, 798 | { Methods2D::DILATION, std::bind(&RegionDetector::apply2dDilation, this, ph::_1, ph::_2) }, 799 | { Methods2D::EROSION, std::bind(&RegionDetector::apply2dErosion, this, ph::_1, ph::_2) }, 800 | { Methods2D::CANNY, std::bind(&RegionDetector::apply2dCanny, this, ph::_1, ph::_2) }, 801 | { Methods2D::THINNING, 802 | [](cv::Mat input, cv::Mat& output) -> Result { 803 | input.copyTo(output); 804 | thinningGuoHall(output); 805 | } }, 806 | { Methods2D::RANGE, std::bind(&RegionDetector::apply2dRange, this, ph::_1, ph::_2) }, 807 | { Methods2D::HSV, std::bind(&RegionDetector::apply2dHSV, this, ph::_1, ph::_2) }, 808 | { Methods2D::EQUALIZE_HIST_YUV, std::bind(&RegionDetector::apply2dEqualizeHistYUV, this, ph::_1, ph::_2) }, 809 | { Methods2D::EQUALIZE_HIST, std::bind(&RegionDetector::apply2dEqualizeHist, this, ph::_1, ph::_2) }, 810 | { Methods2D::CLAHE, std::bind(&RegionDetector::apply2dCLAHE, this, ph::_1, ph::_2) } 811 | }; 812 | 813 | for (std::string& method_name : cfg_->opencv_cfg.methods) 814 | { 815 | if (METHOD_CODES_MAPPINGS.count(method_name) > 0) 816 | { 817 | try 818 | { 819 | Result res = function_mappings.at(METHOD_CODES_MAPPINGS.at(method_name))(input, output); 820 | if (!res) 821 | { 822 | return res; 823 | } 824 | input = output; 825 | updateDebugWindow(output); 826 | } 827 | catch (cv::Exception& e) 828 | { 829 | LOG4CXX_ERROR(logger_, "Operation " << method_name << " failed with error " << e.what()); 830 | } 831 | } 832 | else 833 | { 834 | std::string err_msg = boost::str(boost::format("2D Method %s is not valid") % method_name); 835 | LOG4CXX_ERROR(logger_, err_msg); 836 | } 837 | } 838 | return true; 839 | } 840 | 841 | bool RegionDetector::compute2d(cv::Mat input, 842 | cv::Mat& output, 843 | std::vector>& contours_indices) const 844 | { 845 | return compute2dContours(input, contours_indices, output); 846 | } 847 | 848 | bool RegionDetector::compute(const RegionDetector::DataBundleVec& input, RegionDetector::RegionResults& regions) 849 | { 850 | using namespace pcl; 851 | std::vector::Ptr> closed_contours_points, open_contours_points; 852 | pcl::PointCloud::Ptr normals = boost::make_shared>(); 853 | 854 | Result res; 855 | window_counter_ = 0; 856 | for (const DataBundle& data : input) 857 | { 858 | window_counter_++; 859 | 860 | // ============================== Open CV =================================== // 861 | LOG4CXX_DEBUG(logger_, "Computing 2d contours"); 862 | cv::Mat output; 863 | std::vector> contours_indices; 864 | res = compute2dContours(data.image, contours_indices, output); 865 | regions.images.push_back(output); 866 | if (!res) 867 | { 868 | return false; 869 | } 870 | 871 | // interpolating to fill gaps 872 | for (std::size_t i = 0; i < contours_indices.size(); i++) 873 | { 874 | std::vector interpolated_indices; 875 | const std::vector& indices = contours_indices[i]; 876 | interpolated_indices.push_back(indices.front()); 877 | for (std::size_t j = 1; j < indices.size(); j++) 878 | { 879 | const cv::Point& p1 = indices[j - 1]; 880 | const cv::Point& p2 = indices[j]; 881 | 882 | int x_coord_dist = std::abs(p2.x - p1.x); 883 | int y_coord_dist = std::abs(p2.y - p1.y); 884 | int max_coord_dist = x_coord_dist > y_coord_dist ? x_coord_dist : y_coord_dist; 885 | if (max_coord_dist <= MIN_PIXEL_DISTANCE) 886 | { 887 | interpolated_indices.push_back(p2); 888 | continue; 889 | } 890 | int num_elements = max_coord_dist + 1; 891 | std::vector x_coord = linspace(p1.x, p2.x, num_elements); 892 | std::vector y_coord = linspace(p1.y, p2.y, num_elements); 893 | cv::Point p; 894 | for (std::size_t k = 0; k < num_elements; k++) 895 | { 896 | std::tie(p.x, p.y) = std::make_tuple(x_coord[k], y_coord[k]); 897 | interpolated_indices.push_back(p); 898 | } 899 | } 900 | contours_indices[i] = interpolated_indices; 901 | } 902 | 903 | // ============================== PCL 2D (pixel coordinates z= 0) =================================== // 904 | // converting to cloud type for further analysis 905 | std::vector> contours_indices_clouds_vec; 906 | for (std::size_t i = 0; i < contours_indices.size(); i++) 907 | { 908 | pcl::PointCloud contour_indices_cloud; 909 | contour_indices_cloud = convert2DContourToCloud(contours_indices[i]); 910 | contours_indices_clouds_vec.push_back(contour_indices_cloud); 911 | } 912 | 913 | // downsampling 914 | const RegionDetectionConfig::PCL2DCfg& pcl2d_cfg = cfg_->pcl_2d_cfg; 915 | int search_radius_2d = 4; 916 | for (std::size_t i = 0; i < contours_indices_clouds_vec.size(); i++) 917 | { 918 | if (pcl2d_cfg.downsampling_radius > 0) 919 | { 920 | dowsampleCloud(contours_indices_clouds_vec[i], pcl2d_cfg.downsampling_radius); 921 | } 922 | } 923 | 924 | // sequence 925 | for (std::size_t i = 0; i < contours_indices_clouds_vec.size(); i++) 926 | { 927 | contours_indices_clouds_vec[i] = sequence(contours_indices_clouds_vec[i].makeShared()); 928 | } 929 | 930 | // split 931 | std::vector::Ptr> contours_indices_cloud_vec; 932 | for (std::size_t i = 0; i < contours_indices_clouds_vec.size(); i++) 933 | { 934 | std::vector::Ptr> temp_indices_cloud_vec = 935 | split(contours_indices_clouds_vec[i], pcl2d_cfg.split_dist); 936 | contours_indices_cloud_vec.insert( 937 | contours_indices_cloud_vec.end(), temp_indices_cloud_vec.begin(), temp_indices_cloud_vec.end()); 938 | } 939 | 940 | // find closed curves 941 | std::vector::Ptr> closed_indices_curves_vec, open_indices_curves_vec; 942 | findClosedCurves(contours_indices_cloud_vec, 943 | pcl2d_cfg.closed_curve_max_dist, 944 | closed_indices_curves_vec, 945 | open_indices_curves_vec); 946 | 947 | // simplification of closed curves 948 | for (std::size_t i = 0; i < closed_indices_curves_vec.size(); i++) 949 | { 950 | int pre_simplified_size = closed_indices_curves_vec[i]->size(); 951 | if (pre_simplified_size < pcl2d_cfg.simplification_min_points) 952 | { 953 | continue; 954 | } 955 | closed_indices_curves_vec[i] = 956 | concaveHullSimplification(closed_indices_curves_vec[i], pcl2d_cfg.simplification_alpha); 957 | LOG4CXX_DEBUG(logger_, 958 | "Concave hull simplified cloud from " << pre_simplified_size << " to " 959 | << closed_indices_curves_vec[i]->size()); 960 | *closed_indices_curves_vec[i] = sequence(closed_indices_curves_vec[i]->makeShared()); 961 | closed_indices_curves_vec[i]->push_back(closed_indices_curves_vec[i]->front()); 962 | } 963 | 964 | // combining closed and open back into single vec 965 | contours_indices_cloud_vec.clear(); 966 | contours_indices_cloud_vec.insert( 967 | contours_indices_cloud_vec.end(), closed_indices_curves_vec.begin(), closed_indices_curves_vec.end()); 968 | contours_indices_cloud_vec.insert( 969 | contours_indices_cloud_vec.end(), open_indices_curves_vec.begin(), open_indices_curves_vec.end()); 970 | 971 | // converting to cv points 972 | contours_indices.clear(); 973 | for (auto& cloud : contours_indices_cloud_vec) 974 | { 975 | std::vector temp_indices = convertCloudTo2DContour(*cloud); 976 | contours_indices.push_back(temp_indices); 977 | } 978 | 979 | // ============================== PCL 3D (x, y and z coordinates) =================================== // 980 | 981 | // converting input cloud blob into point cloud of specified point type 982 | pcl::PointCloud::Ptr input_cloud = boost::make_shared>(); 983 | pcl::fromPCLPointCloud2(data.cloud_blob, *input_cloud); 984 | 985 | // transform cloud 986 | pcl::transformPointCloud(*input_cloud, *input_cloud, data.transform.cast()); 987 | 988 | // extract contours 3d points from 2d pixel locations 989 | std::vector::Ptr> contours_points; 990 | LOG4CXX_DEBUG(logger_, "Extracting contours from 3d data"); 991 | res = extractContoursFromCloud(contours_indices, input_cloud, contours_points); 992 | if (!res) 993 | { 994 | LOG4CXX_ERROR(logger_, "Failed to extract 3d data"); 995 | return false; 996 | } 997 | 998 | // cleaning data 999 | for (auto& contour : contours_points) 1000 | { 1001 | // removing nans 1002 | std::vector nan_indices = {}; 1003 | LOG4CXX_DEBUG(logger_, "NaN Removal"); 1004 | contour->is_dense = false; 1005 | pcl::removeNaNFromPointCloud(*contour, *contour, nan_indices); 1006 | 1007 | // removing infinite 1008 | LOG4CXX_DEBUG(logger_, "Infinite Removal"); 1009 | removeInfinite(*contour); 1010 | 1011 | // statistical outlier removal 1012 | if (cfg_->pcl_cfg.stat_removal.enable) 1013 | { 1014 | LOG4CXX_DEBUG(logger_, "Statistical Outlier Removal"); 1015 | pcl::StatisticalOutlierRemoval sor; 1016 | sor.setInputCloud(contour->makeShared()); 1017 | sor.setMeanK(cfg_->pcl_cfg.stat_removal.kmeans); 1018 | sor.setStddevMulThresh(cfg_->pcl_cfg.stat_removal.stddev); 1019 | sor.filter(*contour); 1020 | } 1021 | 1022 | /* TODO:Disrupts the order of the points 1023 | if(cfg_->pcl_cfg.downsample_leaf_size > 0) 1024 | { 1025 | dowsampleCloud(*contour,cfg_->pcl_cfg.downsample_leaf_size); 1026 | *contour = sequence(contour->makeShared(),1e-5); 1027 | }*/ 1028 | } 1029 | 1030 | LOG4CXX_DEBUG(logger_, "Computing normals"); 1031 | std::vector::Ptr> contours_point_normals; 1032 | res = computeNormals(input_cloud, contours_points, contours_point_normals); 1033 | if (!res) 1034 | { 1035 | return false; 1036 | } 1037 | 1038 | // adding found closed contours 1039 | std::vector::Ptr> current_closed_contour_points; 1040 | current_closed_contour_points.assign(contours_points.begin(), 1041 | std::next(contours_points.begin(), closed_indices_curves_vec.size())); 1042 | for (pcl::PointCloud::Ptr cloud : current_closed_contour_points) 1043 | { 1044 | std::vector::Ptr> split_clouds = split(*cloud, cfg_->pcl_cfg.split_dist); 1045 | if (split_clouds.size() == 1) 1046 | { 1047 | // no split occurred so keeping as closed curve and copying first point to end in order to close the curve 1048 | cloud->push_back(cloud->front()); 1049 | closed_contours_points.push_back(cloud); 1050 | } 1051 | else if (split_clouds.size() > 1) 1052 | { 1053 | // got splitted so inserting in open contours vector 1054 | open_contours_points.insert(open_contours_points.end(), split_clouds.begin(), split_clouds.end()); 1055 | } 1056 | else if (split_clouds.empty()) 1057 | { 1058 | LOG4CXX_ERROR(logger_, "Splitting failed to return at least one curve"); 1059 | return false; 1060 | } 1061 | } 1062 | 1063 | // adding open contours 1064 | std::vector::Ptr> current_open_contour_points; 1065 | current_open_contour_points.assign(std::next(contours_points.begin(), closed_indices_curves_vec.size()), 1066 | contours_points.end()); 1067 | for (pcl::PointCloud::Ptr cloud : current_open_contour_points) 1068 | { 1069 | std::vector::Ptr> split_clouds = split(*cloud, cfg_->pcl_cfg.split_dist); 1070 | open_contours_points.insert(open_contours_points.end(), split_clouds.begin(), split_clouds.end()); 1071 | } 1072 | 1073 | /* TODO: Remove 1074 | * if(!closed_indices_curves_vec.empty()) 1075 | { 1076 | closed_contours_points.insert(closed_contours_points.end(), 1077 | contours_points.begin(), 1078 | std::next(contours_points.begin(),closed_indices_curves_vec.size())); 1079 | } 1080 | 1081 | if(!open_indices_curves_vec.empty()) 1082 | { 1083 | open_contours_points.insert(open_contours_points.end(), 1084 | std::next(contours_points.begin(),closed_indices_curves_vec.size()), 1085 | contours_points.end()); 1086 | }*/ 1087 | 1088 | // adding point normals 1089 | for (auto& cn : contours_point_normals) 1090 | { 1091 | std::vector removed_indices; 1092 | cn->is_dense = false; 1093 | pcl::removeNaNNormalsFromPointCloud(*cn, *cn, removed_indices); 1094 | removeInfinite(*cn); 1095 | (*normals) += *cn; 1096 | } 1097 | } 1098 | 1099 | // combining open curves to form closed ones 1100 | std::vector::Ptr> closed_curves_points, open_curves_points; 1101 | LOG4CXX_DEBUG(logger_, "Computing closed contours from " << open_contours_points.size() << " open curves"); 1102 | res = combineIntoClosedRegions(open_contours_points, closed_curves_points, open_curves_points); 1103 | 1104 | // adding to existing vector of closed and open curves 1105 | closed_contours_points.insert(closed_contours_points.end(), closed_curves_points.begin(), closed_curves_points.end()); 1106 | open_contours_points = open_curves_points; 1107 | 1108 | // simplifying by length 1109 | closed_contours_points = simplifyByMinimunLength(closed_contours_points, cfg_->pcl_cfg.simplification_min_dist); 1110 | open_contours_points = simplifyByMinimunLength(open_contours_points, cfg_->pcl_cfg.simplification_min_dist); 1111 | 1112 | // filter out those with too few points 1113 | closed_contours_points.erase(std::remove_if(closed_contours_points.begin(), 1114 | closed_contours_points.end(), 1115 | [this](pcl::PointCloud::Ptr& c) { 1116 | return c->size() < cfg_->pcl_cfg.min_num_points; 1117 | }), 1118 | closed_contours_points.end()); 1119 | 1120 | open_contours_points.erase(std::remove_if(open_contours_points.begin(), 1121 | open_contours_points.end(), 1122 | [this](pcl::PointCloud::Ptr& c) { 1123 | return c->size() < cfg_->pcl_cfg.min_num_points; 1124 | }), 1125 | open_contours_points.end()); 1126 | 1127 | LOG4CXX_DEBUG(logger_, "Computing curves normals"); 1128 | computePoses(normals, open_contours_points, regions.open_regions_poses); 1129 | computePoses(normals, closed_contours_points, regions.closed_regions_poses); 1130 | 1131 | std::string msg = boost::str(boost::format("Found %i closed regions and %i open regions") % 1132 | regions.closed_regions_poses.size() % regions.open_regions_poses.size()); 1133 | if (regions.closed_regions_poses.empty()) 1134 | { 1135 | LOG4CXX_ERROR(logger_, msg); 1136 | } 1137 | else 1138 | { 1139 | LOG4CXX_INFO(logger_, msg); 1140 | } 1141 | return !regions.closed_regions_poses.empty(); 1142 | } 1143 | 1144 | std::vector::Ptr> 1145 | RegionDetector::simplifyByMinimunLength(const std::vector::Ptr>& segments, 1146 | double min_length) 1147 | { 1148 | using namespace pcl; 1149 | std::vector::Ptr> simplified_segments; 1150 | for (const auto& segment : segments) 1151 | { 1152 | PointCloud::Ptr simplified_segment = boost::make_shared>(); 1153 | simplified_segment->push_back(segment->front()); 1154 | for (std::size_t i = 1; i < segment->size() - 1; i++) 1155 | { 1156 | const auto& p0 = simplified_segment->back(); 1157 | const auto& p1 = segment->at(i); 1158 | Eigen::Vector3f diff = (p0.getArray3fMap() - p1.getArray3fMap()); 1159 | if (diff.norm() > min_length) 1160 | { 1161 | simplified_segment->push_back(p1); 1162 | } 1163 | } 1164 | simplified_segment->push_back(segment->back()); 1165 | 1166 | simplified_segments.push_back(simplified_segment); 1167 | } 1168 | return simplified_segments; 1169 | } 1170 | 1171 | RegionDetector::Result 1172 | RegionDetector::extractContoursFromCloud(const std::vector>& contour_indices, 1173 | pcl::PointCloud::ConstPtr input, 1174 | std::vector::Ptr>& contours_points) 1175 | { 1176 | // check for organized point clouds 1177 | if (!input->isOrganized()) 1178 | { 1179 | std::string err_msg = "Point Cloud not organized"; 1180 | LOG4CXX_ERROR(logger_, err_msg) 1181 | return Result(false, err_msg); 1182 | } 1183 | 1184 | if (contour_indices.empty()) 1185 | { 1186 | std::string err_msg = "Input contour indices vector is empty"; 1187 | LOG4CXX_ERROR(logger_, err_msg) 1188 | return Result(false, err_msg); 1189 | } 1190 | 1191 | pcl::PointCloud temp_contour_points; 1192 | for (const std::vector& indices : contour_indices) 1193 | { 1194 | if (indices.empty()) 1195 | { 1196 | std::string err_msg = "Empty indices vector was passed"; 1197 | LOG4CXX_ERROR(logger_, err_msg) 1198 | return Result(false, err_msg); 1199 | } 1200 | 1201 | temp_contour_points.clear(); 1202 | for (const cv::Point& idx : indices) 1203 | { 1204 | if (idx.x >= input->width || idx.y >= input->height) 1205 | { 1206 | std::string err_msg = "2D indices exceed point cloud size"; 1207 | LOG4CXX_ERROR(logger_, err_msg) 1208 | return Result(false, err_msg); 1209 | } 1210 | temp_contour_points.push_back(input->at(idx.x, idx.y)); 1211 | } 1212 | contours_points.push_back(boost::make_shared>(temp_contour_points)); 1213 | } 1214 | 1215 | return Result(!contours_points.empty(), "Empty cloud after extraction of 3D points"); 1216 | } 1217 | 1218 | RegionDetector::Result 1219 | RegionDetector::combineIntoClosedRegions(const std::vector::Ptr>& contours_points, 1220 | std::vector::Ptr>& closed_curves, 1221 | std::vector::Ptr>& open_curves) 1222 | { 1223 | using namespace pcl; 1224 | 1225 | std::vector::Ptr> output_contours_points; 1226 | output_contours_points.assign(contours_points.begin(), contours_points.end()); 1227 | 1228 | std::vector merged_curves_indices; 1229 | // find closed curves 1230 | for (std::size_t i = 0; i < output_contours_points.size(); i++) 1231 | { 1232 | if (std::find(merged_curves_indices.begin(), merged_curves_indices.end(), i) != merged_curves_indices.end()) 1233 | { 1234 | // already merged 1235 | LOG4CXX_DEBUG(logger_, "Curve " << i << " has already been merged"); 1236 | continue; 1237 | } 1238 | 1239 | // get curve 1240 | PointCloud::Ptr curve_points = output_contours_points[i]->makeShared(); 1241 | LOG4CXX_DEBUG(logger_, "Attempting to merge Curve " << i << " with " << curve_points->size() << " points"); 1242 | 1243 | // create merge candidate index list 1244 | std::vector merge_candidate_indices; 1245 | merge_candidate_indices.resize(output_contours_points.size()); 1246 | std::iota(merge_candidate_indices.begin(), merge_candidate_indices.end(), 0); 1247 | 1248 | // remove current curve 1249 | merge_candidate_indices.erase(merge_candidate_indices.begin() + i); 1250 | 1251 | // search for close curves to merge with 1252 | bool merged_curves = false; 1253 | do 1254 | { 1255 | merged_curves = false; 1256 | 1257 | // merge with other unmerged curves 1258 | for (int idx : merge_candidate_indices) 1259 | { 1260 | if (std::find(merged_curves_indices.begin(), merged_curves_indices.end(), idx) != merged_curves_indices.end()) 1261 | { 1262 | // already merged 1263 | LOG4CXX_DEBUG(logger_, "\tCurve " << idx << " has already been merged"); 1264 | continue; 1265 | } 1266 | 1267 | PointCloud::Ptr merged_points = boost::make_shared>(); 1268 | PointCloud::Ptr next_curve_points = output_contours_points[idx]; 1269 | if (mergeCurves(*curve_points, *next_curve_points, *merged_points)) 1270 | { 1271 | *curve_points = *merged_points; 1272 | merged_curves_indices.push_back(i); 1273 | merged_curves_indices.push_back(idx); 1274 | merged_curves = true; 1275 | LOG4CXX_DEBUG(logger_, 1276 | "Merged Curve " << idx << " with " << next_curve_points->size() << " points to curve " << i 1277 | << ", final curve has " << curve_points->size() << " points"); 1278 | } 1279 | 1280 | // removing repeated 1281 | std::sort(merged_curves_indices.begin(), merged_curves_indices.end()); 1282 | auto it = std::unique(merged_curves_indices.begin(), merged_curves_indices.end()); 1283 | merged_curves_indices.resize(std::distance(merged_curves_indices.begin(), it)); 1284 | } 1285 | 1286 | } while (merged_curves); 1287 | 1288 | // TODO: Remove sequencing again 1289 | //*curve_points = sequence(curve_points->makeShared(),1e-5); 1290 | 1291 | // check if closed 1292 | Eigen::Vector3d diff = 1293 | (curve_points->front().getArray3fMap() - curve_points->back().getArray3fMap()).cast(); 1294 | if (diff.norm() < cfg_->pcl_cfg.closed_curve_max_dist) 1295 | { 1296 | // copying first point to end of cloud to close the curve 1297 | curve_points->push_back(curve_points->front()); 1298 | 1299 | // saving 1300 | closed_curves.push_back(curve_points); 1301 | LOG4CXX_DEBUG(logger_, "Found closed curve with " << curve_points->size() << " points"); 1302 | } 1303 | else 1304 | { 1305 | open_curves.push_back(curve_points); 1306 | LOG4CXX_DEBUG(logger_, "Copied curve " << i << " into open curves vector"); 1307 | } 1308 | 1309 | merged_curves_indices.push_back(i); 1310 | } 1311 | 1312 | // copying open curves that were not merged 1313 | for (std::size_t i = 0; i < output_contours_points.size(); i++) 1314 | { 1315 | if (std::find(merged_curves_indices.begin(), merged_curves_indices.end(), i) != merged_curves_indices.end()) 1316 | { 1317 | continue; 1318 | } 1319 | open_curves.push_back(output_contours_points[i]); 1320 | LOG4CXX_DEBUG(logger_, "Copied unmerged curve " << i << " into open curves vector"); 1321 | } 1322 | 1323 | if (closed_curves.empty()) 1324 | { 1325 | std::string err_msg = "Found no closed curves"; 1326 | LOG4CXX_ERROR(logger_, err_msg) 1327 | return Result(false, err_msg); 1328 | } 1329 | LOG4CXX_INFO(logger_, "Found " << closed_curves.size() << " closed curves"); 1330 | return true; 1331 | } 1332 | 1333 | RegionDetector::Result RegionDetector::mergeCurves(pcl::PointCloud c1, 1334 | pcl::PointCloud c2, 1335 | pcl::PointCloud& merged) 1336 | { 1337 | std::vector end_points_distances(4); 1338 | 1339 | // compute end point distances 1340 | Eigen::Vector3d dist; 1341 | dist = (c1.front().getArray3fMap() - c2.front().getArray3fMap()).cast(); 1342 | end_points_distances[0] = dist.norm(); 1343 | 1344 | dist = (c1.front().getArray3fMap() - c2.back().getArray3fMap()).cast(); 1345 | end_points_distances[1] = dist.norm(); 1346 | 1347 | dist = (c1.back().getArray3fMap() - c2.front().getArray3fMap()).cast(); 1348 | end_points_distances[2] = dist.norm(); 1349 | 1350 | dist = (c1.back().getArray3fMap() - c2.back().getArray3fMap()).cast(); 1351 | end_points_distances[3] = dist.norm(); 1352 | 1353 | std::vector::iterator min_pos = std::min_element(end_points_distances.begin(), end_points_distances.end()); 1354 | if (*min_pos > cfg_->pcl_cfg.max_merge_dist) 1355 | { 1356 | // curves are too far, not merging 1357 | return false; 1358 | } 1359 | 1360 | std::size_t merge_method = std::distance(end_points_distances.begin(), min_pos); 1361 | switch (merge_method) 1362 | { 1363 | case 0: // front2 to front1 1364 | std::reverse(c2.begin(), c2.end()); 1365 | c1.insert(c1.begin(), c2.begin(), c2.end()); 1366 | break; 1367 | 1368 | case 1: // back2 to front1 1369 | c1.insert(c1.begin(), c2.begin(), c2.end()); 1370 | break; 1371 | 1372 | case 2: // back1 to front2 1373 | c1.insert(c1.end(), c2.begin(), c2.end()); 1374 | break; 1375 | 1376 | case 3: // back1 to back2 1377 | std::reverse(c2.begin(), c2.end()); 1378 | c1.insert(c1.end(), c2.begin(), c2.end()); 1379 | break; 1380 | } 1381 | 1382 | // TODO: remove 1383 | // merged = sequence(c1.makeShared()); 1384 | merged = c1; 1385 | return true; 1386 | } 1387 | 1388 | RegionDetector::Result 1389 | RegionDetector::computeNormals(const pcl::PointCloud::ConstPtr source_cloud, 1390 | const std::vector::Ptr>& curves_points, 1391 | std::vector::Ptr>& curves_normals) 1392 | { 1393 | const config_3d::NormalEstimationCfg& cfg = cfg_->pcl_cfg.normal_est; 1394 | 1395 | // downsample first 1396 | pcl::PointCloud::Ptr source_cloud_downsampled = source_cloud->makeShared(); 1397 | dowsampleCloud(*source_cloud_downsampled, cfg.downsampling_radius); 1398 | 1399 | // first compute normals 1400 | pcl::PointCloud::Ptr source_cloud_normals(new pcl::PointCloud); 1401 | pcl::NormalEstimation ne; 1402 | ne.setInputCloud(source_cloud_downsampled); 1403 | ne.setViewPoint(cfg.viewpoint_xyz[0], cfg.viewpoint_xyz[1], cfg.viewpoint_xyz[2]); 1404 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 1405 | ne.setSearchMethod(tree); 1406 | ne.setRadiusSearch(cfg.search_radius); 1407 | ne.compute(*source_cloud_normals); 1408 | 1409 | // create kdtree to search cloud with normals 1410 | pcl::KdTreeFLANN kdtree; 1411 | kdtree.setEpsilon(cfg.kdtree_epsilon); 1412 | kdtree.setInputCloud(source_cloud_downsampled); 1413 | 1414 | const int MAX_NUM_POINTS = 1; 1415 | std::vector nearest_indices(MAX_NUM_POINTS); 1416 | std::vector nearest_distances(MAX_NUM_POINTS); 1417 | for (auto& curve : curves_points) 1418 | { 1419 | // search point and copy its normal 1420 | pcl::PointCloud::Ptr curve_normals = boost::make_shared>(); 1421 | curve_normals->reserve(curve->size()); 1422 | for (auto& search_p : *curve) 1423 | { 1424 | int nearest_found = kdtree.nearestKSearch(search_p, MAX_NUM_POINTS, nearest_indices, nearest_distances); 1425 | if (nearest_found <= 0) 1426 | { 1427 | std::string err_msg = "Found no points near curve, can not get normal vector"; 1428 | LOG4CXX_ERROR(logger_, err_msg) 1429 | return Result(false, err_msg); 1430 | } 1431 | pcl::PointNormal pn; 1432 | pcl::copyPoint(source_cloud_normals->at(nearest_indices.front()), pn); 1433 | pcl::copyPoint(search_p, pn); 1434 | curve_normals->push_back(pn); 1435 | } 1436 | curves_normals.push_back(curve_normals); 1437 | } 1438 | return true; 1439 | } 1440 | 1441 | RegionDetector::Result RegionDetector::computePoses(pcl::PointCloud::ConstPtr source_normal_cloud, 1442 | std::vector::Ptr>& curves_points, 1443 | std::vector& curves_poses) 1444 | { 1445 | using namespace Eigen; 1446 | const config_3d::NormalEstimationCfg& cfg = cfg_->pcl_cfg.normal_est; 1447 | 1448 | // create kdtree to search cloud with normals 1449 | pcl::PointCloud::Ptr source_points = boost::make_shared>(); 1450 | pcl::copyPointCloud(*source_normal_cloud, *source_points); 1451 | pcl::KdTreeFLANN kdtree; 1452 | kdtree.setEpsilon(cfg.kdtree_epsilon); 1453 | kdtree.setInputCloud(source_points); 1454 | 1455 | const unsigned int MAX_NUM_POINTS = 1; 1456 | std::vector nearest_indices(MAX_NUM_POINTS); 1457 | std::vector nearest_distances(MAX_NUM_POINTS); 1458 | int curve_idx = -1; 1459 | for (auto& curve : curves_points) 1460 | { 1461 | curve_idx++; 1462 | 1463 | // search point and copy its normal 1464 | pcl::PointCloud::Ptr curve_normals = boost::make_shared>(); 1465 | curve_normals->reserve(curve->size()); 1466 | for (auto& search_p : *curve) 1467 | { 1468 | int nearest_found = kdtree.nearestKSearch(search_p, MAX_NUM_POINTS, nearest_indices, nearest_distances); 1469 | if (nearest_found <= 0) 1470 | { 1471 | std::string err_msg = boost::str(boost::format("Kdtree found no nearby points during pose computation")); 1472 | LOG4CXX_ERROR(logger_, err_msg); 1473 | return Result(false, err_msg); 1474 | } 1475 | pcl::Normal p; 1476 | pcl::copyPoint(source_normal_cloud->at(nearest_indices.front()), p); 1477 | curve_normals->push_back(p); 1478 | } 1479 | 1480 | EigenPose3dVector curve_poses; 1481 | pcl::PointNormal p1, p2; 1482 | Isometry3d pose; 1483 | Vector3d x_dir, z_dir, y_dir; 1484 | double dir = 1.0; 1485 | for (std::size_t i = 0; i < curve->size(); i++) 1486 | { 1487 | std::size_t idx_current = i; 1488 | std::size_t idx_next = i + 1; 1489 | dir = 1.0; 1490 | 1491 | if (idx_next >= curve->size()) 1492 | { 1493 | idx_current = i; 1494 | idx_next = i - 1; 1495 | dir = -1.0; 1496 | } 1497 | 1498 | pcl::copyPoint(curve->at(idx_current), p1); 1499 | pcl::copyPoint(curve_normals->at(idx_current), p1); 1500 | pcl::copyPoint(curve->at(idx_next), p2); 1501 | pcl::copyPoint(curve_normals->at(idx_next), p2); 1502 | 1503 | x_dir = dir * (p2.getVector3fMap() - p1.getVector3fMap()).normalized().cast(); 1504 | z_dir = Vector3d(p1.normal_x, p1.normal_y, p1.normal_z).normalized(); 1505 | y_dir = z_dir.cross(x_dir).normalized(); 1506 | z_dir = x_dir.cross(y_dir).normalized(); 1507 | 1508 | pose = Translation3d(p1.getVector3fMap().cast()); 1509 | pose.matrix().block<3, 3>(0, 0) = toRotationMatrix(x_dir, y_dir, z_dir); 1510 | curve_poses.push_back(pose); 1511 | } 1512 | curves_poses.push_back(curve_poses); 1513 | } 1514 | 1515 | return true; 1516 | } 1517 | 1518 | } /* namespace region_detection_core */ 1519 | -------------------------------------------------------------------------------- /region_detection_core/src/tests/adaptive_threshold_test.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Threshold.cpp 3 | * @brief Sample code that shows how to use the diverse threshold options offered by OpenCV 4 | * @author OpenCV team 5 | */ 6 | 7 | #include "opencv2/imgproc.hpp" 8 | #include "opencv2/imgcodecs.hpp" 9 | #include "opencv2/highgui.hpp" 10 | #include 11 | #include 12 | 13 | using namespace cv; 14 | using std::cout; 15 | 16 | /// Global variables 17 | 18 | int threshold_value = 0; 19 | int threshold_type = cv::ThresholdTypes::THRESH_BINARY; 20 | int threshold_method = cv::AdaptiveThresholdTypes::ADAPTIVE_THRESH_MEAN_C; 21 | int block_size = 1; 22 | 23 | int const max_value = 255; 24 | int const max_type = cv::ThresholdTypes::THRESH_BINARY_INV; 25 | int const max_method = cv::AdaptiveThresholdTypes::ADAPTIVE_THRESH_GAUSSIAN_C; 26 | int const max_block_size = 5; 27 | 28 | Mat src, src_gray, dst, inverted; 29 | const char* window_name = "Threshold Demo"; 30 | 31 | int dilation_elem = 0; 32 | int dilation_size = 0; 33 | int const max_elem = 2; 34 | int const max_kernel_size = 21; 35 | 36 | const char* trackbar_type = "Type: \n 0: Binary \n 1: Binary Inverted \n 2: Truncate \n 3: To Zero \n 4: To Zero " 37 | "Inverted"; 38 | const char* trackbar_value = "Value"; 39 | 40 | //![updateImage] 41 | /** 42 | * @function updateImage 43 | */ 44 | static void updateImage(int, void*) 45 | { 46 | /* 0: Binary 47 | 1: Binary Inverted 48 | 2: Threshold Truncated 49 | 3: Threshold to Zero 50 | 4: Threshold to Zero Inverted 51 | */ 52 | inverted = cv::Scalar_(255) - src_gray; 53 | if (dilation_size > 0) 54 | { 55 | int dilation_type = 0; 56 | if (dilation_elem == 0) 57 | { 58 | dilation_type = MORPH_RECT; 59 | } 60 | else if (dilation_elem == 1) 61 | { 62 | dilation_type = MORPH_CROSS; 63 | } 64 | else if (dilation_elem == 2) 65 | { 66 | dilation_type = MORPH_ELLIPSE; 67 | } 68 | Mat element = getStructuringElement( 69 | dilation_type, Size(2 * dilation_size + 1, 2 * dilation_size + 1), Point(dilation_size, dilation_size)); 70 | decltype(inverted) dilation_dst; 71 | dilate(inverted, dilation_dst, element); 72 | dst = dilation_dst.clone(); 73 | } 74 | else 75 | { 76 | std::cout << "Skipping dilation" << std::endl; 77 | dst = inverted.clone(); 78 | } 79 | 80 | block_size = block_size == 0 ? 1 : block_size; 81 | // threshold_type = threshold_type == max_type ? cv::ThresholdTypes::THRESH_TRIANGLE : threshold_type; 82 | 83 | int block_size_val = block_size * 2 + 1; 84 | adaptiveThreshold(dst.clone(), dst, threshold_value, threshold_method, threshold_type, block_size_val, 0); 85 | 86 | imshow(window_name, dst); 87 | } 88 | //![updateImage] 89 | 90 | /** 91 | * @function main 92 | */ 93 | int main(int argc, char** argv) 94 | { 95 | namespace fs = boost::filesystem; 96 | //! [load] 97 | String image_name; 98 | if (argc > 1) 99 | { 100 | image_name = argv[1]; 101 | } 102 | 103 | if (!fs::exists(fs::path(image_name.c_str()))) 104 | { 105 | std::cout << "Failed to load file " << image_name.c_str() << std::endl; 106 | return -1; 107 | } 108 | 109 | src = imread(image_name, IMREAD_COLOR); // Load an image 110 | 111 | if (src.empty()) 112 | { 113 | cout << "Cannot read the image: " << image_name << std::endl; 114 | return -1; 115 | } 116 | 117 | // cvtColor( src, src_gray, COLOR_BGR2GRAY ); // Convert the image to Gray 118 | cvtColor(src, src_gray, COLOR_RGB2GRAY); 119 | 120 | //! [load] 121 | 122 | //! [window] 123 | namedWindow(window_name, WINDOW_AUTOSIZE); // Create a window to display results 124 | //! [window] 125 | 126 | //! [trackbar] 127 | createTrackbar(trackbar_value, window_name, &threshold_value, max_value, updateImage); // Create a Trackbar to choose 128 | // Threshold value 129 | 130 | createTrackbar(trackbar_type, window_name, &threshold_type, max_type, updateImage); // Create a Trackbar to choose 131 | // type of Threshold 132 | 133 | createTrackbar("Method", window_name, &threshold_method, max_method, updateImage); // Create a Trackbar to choose 134 | // Threshold value 135 | 136 | createTrackbar( 137 | "Block Size: 2 x (val) + 1", window_name, &block_size, max_block_size, updateImage); // Create a Trackbar to 138 | // choose Threshold value 139 | 140 | createTrackbar("Element:\n 0: Rect \n 1: Cross \n 2: Ellipse", window_name, &dilation_elem, max_elem, updateImage); 141 | createTrackbar("Kernel size:\n 2n +1", window_name, &dilation_size, max_kernel_size, updateImage); 142 | //! [trackbar] 143 | 144 | updateImage(0, 0); // Call the function to initialize 145 | 146 | /// Wait until the user finishes the program 147 | waitKey(); 148 | return 0; 149 | } 150 | /* 151 | * adaptive_threshold_test.cpp 152 | * 153 | * Created on: Jun 3, 2020 154 | * Author: jnicho 155 | */ 156 | -------------------------------------------------------------------------------- /region_detection_core/src/tests/region_detection_test.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "region_detection_core/region_detector.h" 7 | 8 | using namespace region_detection_core; 9 | 10 | int main(int argc, char** argv) 11 | { 12 | namespace fs = boost::filesystem; 13 | 14 | auto logger = RegionDetector::createDefaultDebugLogger("DEBUG"); 15 | if (argc < 3) 16 | { 17 | LOG4CXX_ERROR(logger, "Needs config file and image arguments"); 18 | return -1; 19 | } 20 | 21 | std::vector files = { argv[1], argv[2] }; 22 | if (!std::all_of(files.begin(), files.end(), [](const std::string& f) { return fs::exists(fs::path(f)); })) 23 | { 24 | LOG4CXX_ERROR(logger, "File does not exists"); 25 | return -1; 26 | } 27 | 28 | bool compute_contours = false; 29 | if (argc == 4) 30 | { 31 | compute_contours = boost::lexical_cast(argv[3]); 32 | } 33 | 34 | std::string config_file = argv[1]; 35 | std::string img_file = argv[2]; 36 | 37 | cv::Mat output; 38 | RegionDetector region_detector(logger); 39 | int key; 40 | do 41 | { 42 | if (!region_detector.configureFromFile(config_file)) 43 | { 44 | LOG4CXX_ERROR(logger, "Failed to load configuration from file " << config_file); 45 | return -1; 46 | } 47 | std::vector > contours_indices; 48 | cv::Mat input = cv::imread(img_file, cv::IMREAD_COLOR); // Load an image 49 | if (compute_contours) 50 | { 51 | region_detector.compute2d(input, output, contours_indices); 52 | } 53 | else 54 | { 55 | region_detector.compute2d(input, output); 56 | } 57 | 58 | std::cout << "Pres ESC to exit" << std::endl; 59 | key = cv::waitKey(); 60 | } while (key != 27); // escape 61 | 62 | std::cout << "Key pressed " << key << std::endl; 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /region_detection_core/src/tests/threshold_grayscale_test.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Threshold.cpp 3 | * @brief Sample code that shows how to use the diverse threshold options offered by OpenCV 4 | * @author OpenCV team 5 | */ 6 | 7 | #include "opencv2/imgproc.hpp" 8 | #include "opencv2/imgcodecs.hpp" 9 | #include "opencv2/highgui.hpp" 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace cv; 15 | using std::cout; 16 | 17 | /// Global variables 18 | 19 | int threshold_enabled = 0; 20 | int threshold_value = 150; 21 | int threshold_type = cv::ThresholdTypes::THRESH_TRUNC; 22 | int const max_value = 255; 23 | 24 | int const max_threshold_enabled = 1; 25 | int const max_type = cv::ThresholdTypes::THRESH_TOZERO_INV; 26 | int const max_binary_value = 255; 27 | 28 | int dilation_enabled = 1; 29 | int dilation_elem = 0; 30 | int dilation_size = 1; 31 | int const max_elem = 2; 32 | int const max_kernel_size = 21; 33 | 34 | int thinning_enabled = 1; 35 | 36 | struct CountourConfig 37 | { 38 | int enable = 1; 39 | int mode = CV_RETR_EXTERNAL; 40 | int method = CV_CHAIN_APPROX_SIMPLE; 41 | 42 | static const int MAX_ENABLE = 1; 43 | static const int MAX_MODE = CV_RETR_TREE; 44 | static const int MAX_METHOD = CV_CHAIN_APPROX_TC89_KCOS; 45 | }; 46 | static CountourConfig contour_cfg; 47 | 48 | struct CannyConfig 49 | { 50 | int enable = 1; 51 | int lower_threshold = 45; 52 | int upper_threshold = lower_threshold * 3; 53 | int aperture_size = 1; 54 | 55 | static const int MAX_ENABLE = 1; 56 | static const int MAX_LOWER_TH = 100; 57 | static const int MAX_UPPER_TH = 255; 58 | static const int MAX_APERTURE_SIZE = 3; 59 | }; 60 | static CannyConfig canny_cfg; 61 | 62 | static std::string RESULTS_DIR; 63 | static int SAVE_IMAGE = 0; 64 | 65 | cv::RNG rng(12345); 66 | 67 | Mat src, src_gray, dst, inverted; 68 | const char* window_name = "Countour detection"; 69 | const char* trackbar_type = "Threshold Type: \n 0: Binary, 1: Binary Inverted, 2: Truncate, 3: To Zero, 4: To Zero " 70 | "Inverted\n"; 71 | const char* trackbar_value = "Threshold Value"; 72 | 73 | /** 74 | * Perform one thinning iteration. 75 | * Normally you wouldn't call this function directly from your code. 76 | * 77 | * @param im Binary image with range = 0-1 78 | * @param iter 0=even, 1=odd 79 | */ 80 | void thinningGuoHallIteration(cv::Mat& im, int iter) 81 | { 82 | cv::Mat marker = cv::Mat::zeros(im.size(), CV_8UC1); 83 | 84 | for (int i = 1; i < im.rows; i++) 85 | { 86 | for (int j = 1; j < im.cols; j++) 87 | { 88 | uchar p2 = im.at(i - 1, j); 89 | uchar p3 = im.at(i - 1, j + 1); 90 | uchar p4 = im.at(i, j + 1); 91 | uchar p5 = im.at(i + 1, j + 1); 92 | uchar p6 = im.at(i + 1, j); 93 | uchar p7 = im.at(i + 1, j - 1); 94 | uchar p8 = im.at(i, j - 1); 95 | uchar p9 = im.at(i - 1, j - 1); 96 | 97 | int C = (!p2 & (p3 | p4)) + (!p4 & (p5 | p6)) + (!p6 & (p7 | p8)) + (!p8 & (p9 | p2)); 98 | int N1 = (p9 | p2) + (p3 | p4) + (p5 | p6) + (p7 | p8); 99 | int N2 = (p2 | p3) + (p4 | p5) + (p6 | p7) + (p8 | p9); 100 | int N = N1 < N2 ? N1 : N2; 101 | int m = iter == 0 ? ((p6 | p7 | !p9) & p8) : ((p2 | p3 | !p5) & p4); 102 | 103 | if (C == 1 && (N >= 2 && N <= 3) & m == 0) 104 | marker.at(i, j) = 1; 105 | } 106 | } 107 | 108 | im &= ~marker; 109 | } 110 | 111 | void thinningGuoHall(cv::Mat& im) 112 | { 113 | im /= 255; 114 | 115 | cv::Mat prev = cv::Mat::zeros(im.size(), CV_8UC1); 116 | cv::Mat diff; 117 | 118 | do 119 | { 120 | thinningGuoHallIteration(im, 0); 121 | thinningGuoHallIteration(im, 1); 122 | cv::absdiff(im, prev, diff); 123 | im.copyTo(prev); 124 | } while (cv::countNonZero(diff) > 0); 125 | 126 | im *= 255; 127 | } 128 | 129 | //![updateImage] 130 | /** 131 | * @function updateImage 132 | */ 133 | static void updateImage(int, void*) 134 | { 135 | namespace fs = boost::filesystem; 136 | /* 0: Binary 137 | 1: Binary Inverted 138 | 2: Threshold Truncated 139 | 3: Threshold to Zero 140 | 4: Threshold to Zero Inverted 141 | */ 142 | inverted = cv::Scalar_(255) - src_gray; 143 | dst = inverted.clone(); 144 | 145 | if (dilation_enabled) 146 | { 147 | int dilation_type = 0; 148 | if (dilation_elem == 0) 149 | { 150 | dilation_type = MORPH_RECT; 151 | } 152 | else if (dilation_elem == 1) 153 | { 154 | dilation_type = MORPH_CROSS; 155 | } 156 | else if (dilation_elem == 2) 157 | { 158 | dilation_type = MORPH_ELLIPSE; 159 | } 160 | 161 | Mat element = getStructuringElement( 162 | dilation_type, Size(2 * dilation_size + 1, 2 * dilation_size + 1), Point(dilation_size, dilation_size)); 163 | dilate(inverted, dst, element); 164 | } 165 | else 166 | { 167 | std::cout << "Skipping Dilation" << std::endl; 168 | } 169 | 170 | // threshold( src_gray, dst, threshold_value, max_binary_value, threshold_type ); 171 | if (threshold_enabled == 1) 172 | { 173 | threshold(dst.clone(), dst, threshold_value, max_binary_value, threshold_type); 174 | } 175 | else 176 | { 177 | std::cout << "Skipping Threshold" << std::endl; 178 | } 179 | 180 | // thining 181 | // thinningGuoHall(dst); 182 | 183 | // canny edge detection 184 | if (canny_cfg.enable == 1) 185 | { 186 | /// Canny detector 187 | decltype(dst) detected_edges; 188 | int aperture_size = 2 * canny_cfg.aperture_size + 1; 189 | aperture_size = aperture_size < 3 ? 3 : aperture_size; 190 | cv::Canny(dst.clone(), detected_edges, canny_cfg.lower_threshold, canny_cfg.upper_threshold, aperture_size); 191 | dst = detected_edges.clone(); 192 | } 193 | else 194 | { 195 | std::cout << "Skipping Canny" << std::endl; 196 | } 197 | 198 | // thining 199 | if (thinning_enabled == 1) 200 | { 201 | thinningGuoHall(dst); 202 | } 203 | 204 | // contour 205 | if (contour_cfg.enable == 1) 206 | { 207 | std::vector > contours; 208 | std::vector hierarchy; 209 | cv::findContours(dst.clone(), contours, hierarchy, contour_cfg.mode, contour_cfg.method); 210 | 211 | Mat drawing = Mat::zeros(dst.size(), CV_8UC3); 212 | std::cout << "Contour found " << contours.size() << " contours" << std::endl; 213 | for (int i = 0; i < contours.size(); i++) 214 | { 215 | Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); 216 | double area = cv::contourArea(contours[i]); 217 | double arc_length = cv::arcLength(contours[i], false); 218 | drawContours(drawing, contours, i, color, 2, 8, hierarchy, 0, Point()); 219 | std::string summary = 220 | boost::str(boost::format("c[%i]: s: %i, area: %f, arc %f; (p0: %i, pf: %i); h: %i") % i % contours[i].size() % 221 | area % arc_length % contours[i].front() % contours[i].back() % hierarchy[i]); 222 | std::cout << summary << std::endl; 223 | } 224 | dst = drawing.clone(); 225 | } 226 | else 227 | { 228 | std::cout << "Skipping Contour" << std::endl; 229 | } 230 | 231 | imshow(window_name, dst); 232 | 233 | if (SAVE_IMAGE == 1) 234 | { 235 | SAVE_IMAGE = 0; 236 | 237 | std::string save_path = (fs::path(RESULTS_DIR) / fs::path("results.png")).string(); 238 | imwrite(save_path, dst); 239 | std::cout << "Saved image in path " << save_path << std::endl; 240 | } 241 | } 242 | //![Threshold_Demo] 243 | 244 | /** 245 | * @function main 246 | */ 247 | int main(int argc, char** argv) 248 | { 249 | namespace fs = boost::filesystem; 250 | //! [load] 251 | String image_name; 252 | if (argc > 1) 253 | { 254 | image_name = argv[1]; 255 | } 256 | 257 | if (!fs::exists(fs::path(image_name.c_str()))) 258 | { 259 | std::cout << "Failed to load file " << image_name.c_str() << std::endl; 260 | return -1; 261 | } 262 | RESULTS_DIR = fs::path(image_name).parent_path().string(); 263 | 264 | src = imread(image_name, IMREAD_COLOR); // Load an image 265 | 266 | if (src.empty()) 267 | { 268 | cout << "Cannot read the image: " << image_name << std::endl; 269 | return -1; 270 | } 271 | 272 | // cvtColor( src, src_gray, COLOR_BGR2GRAY ); // Convert the image to Gray 273 | cvtColor(src, src_gray, COLOR_RGB2GRAY); 274 | 275 | //! [window] 276 | namedWindow(window_name, WINDOW_AUTOSIZE); // Create a window to display results 277 | 278 | //! [Threshold trackbars] 279 | createTrackbar("Threshold Enable", 280 | window_name, 281 | &threshold_enabled, 282 | max_threshold_enabled, 283 | updateImage); // Create a Trackbar to choose type of Threshold 284 | 285 | createTrackbar(trackbar_type, window_name, &threshold_type, max_type, updateImage); // Create a Trackbar to choose 286 | // type of Threshold 287 | 288 | createTrackbar(trackbar_value, window_name, &threshold_value, max_value, updateImage); // Create a Trackbar to choose 289 | // Threshold value 290 | 291 | //! [Canny trackbar] 292 | createTrackbar("Canny Enable", window_name, &canny_cfg.enable, canny_cfg.MAX_ENABLE, updateImage); 293 | createTrackbar("Canny Lower Threshold", window_name, &canny_cfg.lower_threshold, canny_cfg.MAX_LOWER_TH, updateImage); 294 | createTrackbar("Canny Upper Threshold", window_name, &canny_cfg.upper_threshold, canny_cfg.MAX_UPPER_TH, updateImage); 295 | createTrackbar( 296 | "Canny Aperture Size 2n + 1", window_name, &canny_cfg.aperture_size, canny_cfg.MAX_APERTURE_SIZE, updateImage); 297 | 298 | //! [Thinning trackbar] 299 | createTrackbar("Thinning Enable", window_name, &thinning_enabled, 1, updateImage); 300 | 301 | //! [Dilation trackbars] 302 | createTrackbar("Dilation Enabled", window_name, &dilation_enabled, 1, updateImage); 303 | createTrackbar( 304 | "Dilation Element:\n 0: Rect , 1: Cross, 2: Ellipse", window_name, &dilation_elem, max_elem, updateImage); 305 | createTrackbar("Dilation Kernel size:\n 2n +1", window_name, &dilation_size, max_kernel_size, updateImage); 306 | 307 | //! [Contour trackbar] 308 | createTrackbar("Contour Enable", window_name, &contour_cfg.enable, contour_cfg.MAX_ENABLE, updateImage); 309 | createTrackbar("Contour Method", window_name, &contour_cfg.method, contour_cfg.MAX_METHOD, updateImage); 310 | createTrackbar("Contour Mode", window_name, &contour_cfg.mode, contour_cfg.MAX_MODE, updateImage); 311 | 312 | createTrackbar("Save Image", window_name, &SAVE_IMAGE, 1, updateImage); 313 | 314 | updateImage(0, 0); // Call the function to initialize 315 | 316 | /// Wait until the user finishes the program 317 | waitKey(); 318 | return 0; 319 | } 320 | -------------------------------------------------------------------------------- /region_detection_core/src/tests/threshold_in_range_test.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/imgproc.hpp" 2 | #include "opencv2/highgui.hpp" 3 | #include 4 | 5 | #include 6 | 7 | using namespace cv; 8 | 9 | /** Global Variables */ 10 | int dilation_elem = 0; 11 | int dilation_size = 0; 12 | int const max_elem = 2; 13 | int const max_kernel_size = 21; 14 | const int max_value_H = 360 / 2; 15 | const int max_value = 255; 16 | const String window_capture_name = "Video Capture"; 17 | const String window_detection_name = "Object Detection"; 18 | int low_H = 0, low_S = 0, low_V = 0; 19 | int high_H = max_value_H, high_S = max_value, high_V = max_value; 20 | static cv::Mat im_frame; 21 | 22 | static void updateImage() 23 | { 24 | Mat inverted, frame_HSV, frame_threshold; 25 | 26 | // inverting 27 | // inverted = ~im_frame; 28 | inverted = im_frame; 29 | 30 | // Convert from BGR to HSV colorspace 31 | cvtColor(inverted, frame_HSV, COLOR_BGR2HSV); 32 | 33 | decltype(frame_HSV) dilation_dst = frame_HSV; 34 | 35 | /* 36 | int dilation_type = 0; 37 | if( dilation_elem == 0 ){ dilation_type = MORPH_RECT; } 38 | else if( dilation_elem == 1 ){ dilation_type = MORPH_CROSS; } 39 | else if( dilation_elem == 2) { dilation_type = MORPH_ELLIPSE; } 40 | Mat element = getStructuringElement( dilation_type, 41 | Size( 2*dilation_size + 1, 2*dilation_size+1 ), 42 | Point( dilation_size, dilation_size ) ); 43 | dilate( frame_HSV, dilation_dst, element ); 44 | */ 45 | 46 | // Detect the object based on HSV Range Values 47 | inRange(dilation_dst, Scalar(low_H, low_S, low_V), Scalar(high_H, high_S, high_V), frame_threshold); 48 | //! [while] 49 | 50 | //! [show] 51 | // Show the frames 52 | // imshow(window_capture_name, frame); 53 | imshow(window_detection_name, frame_threshold); 54 | } 55 | 56 | //! [low] 57 | static void on_low_H_thresh_trackbar(int, void*) 58 | { 59 | low_H = min(high_H - 1, low_H); 60 | setTrackbarPos("Low H", window_detection_name, low_H); 61 | updateImage(); 62 | } 63 | //! [low] 64 | 65 | //! [high] 66 | static void on_high_H_thresh_trackbar(int, void*) 67 | { 68 | high_H = max(high_H, low_H + 1); 69 | setTrackbarPos("High H", window_detection_name, high_H); 70 | updateImage(); 71 | } 72 | 73 | //! [high] 74 | static void on_low_S_thresh_trackbar(int, void*) 75 | { 76 | low_S = min(high_S - 1, low_S); 77 | setTrackbarPos("Low S", window_detection_name, low_S); 78 | updateImage(); 79 | } 80 | 81 | static void on_high_S_thresh_trackbar(int, void*) 82 | { 83 | high_S = max(high_S, low_S + 1); 84 | setTrackbarPos("High S", window_detection_name, high_S); 85 | updateImage(); 86 | } 87 | 88 | static void on_low_V_thresh_trackbar(int, void*) 89 | { 90 | low_V = min(high_V - 1, low_V); 91 | setTrackbarPos("Low V", window_detection_name, low_V); 92 | updateImage(); 93 | } 94 | 95 | static void on_high_V_thresh_trackbar(int, void*) 96 | { 97 | high_V = max(high_V, low_V + 1); 98 | setTrackbarPos("High V", window_detection_name, high_V); 99 | updateImage(); 100 | } 101 | 102 | int main(int argc, char* argv[]) 103 | { 104 | namespace fs = boost::filesystem; 105 | //! [load] 106 | String image_name; 107 | if (argc > 1) 108 | { 109 | image_name = argv[1]; 110 | } 111 | 112 | if (!fs::exists(fs::path(image_name.c_str()))) 113 | { 114 | std::cout << "Failed to load file " << image_name.c_str() << std::endl; 115 | return -1; 116 | } 117 | 118 | im_frame = imread(image_name, IMREAD_COLOR); // Load an image 119 | 120 | if (im_frame.empty()) 121 | { 122 | std::cout << "Cannot read the image: " << image_name << std::endl; 123 | return -1; 124 | } 125 | 126 | //! [window] 127 | // namedWindow(window_capture_name); 128 | namedWindow(window_detection_name); 129 | //! [window] 130 | 131 | //! [trackbar] 132 | // Trackbars to set thresholds for HSV values 133 | createTrackbar("Low H", window_detection_name, &low_H, max_value_H, on_low_H_thresh_trackbar); 134 | createTrackbar("High H", window_detection_name, &high_H, max_value_H, on_high_H_thresh_trackbar); 135 | createTrackbar("Low S", window_detection_name, &low_S, max_value, on_low_S_thresh_trackbar); 136 | createTrackbar("High S", window_detection_name, &high_S, max_value, on_high_S_thresh_trackbar); 137 | createTrackbar("Low V", window_detection_name, &low_V, max_value, on_low_V_thresh_trackbar); 138 | createTrackbar("High V", window_detection_name, &high_V, max_value, on_high_V_thresh_trackbar); 139 | 140 | createTrackbar( 141 | "Element:\n 0: Rect \n 1: Cross \n 2: Ellipse", window_detection_name, &dilation_elem, max_elem, [](int, void*) { 142 | updateImage(); 143 | }); 144 | 145 | createTrackbar("Kernel size:\n 2n +1", window_detection_name, &dilation_size, max_kernel_size, [](int, void*) { 146 | updateImage(); 147 | }); 148 | //! [trackbar] 149 | 150 | /* 151 | Mat frame, frame_HSV, frame_threshold; 152 | 153 | // Convert from BGR to HSV colorspace 154 | cvtColor(frame, frame_HSV, COLOR_BGR2HSV); 155 | // Detect the object based on HSV Range Values 156 | inRange(frame_HSV, Scalar(low_H, low_S, low_V), Scalar(high_H, high_S, high_V), frame_threshold); 157 | //! [while] 158 | 159 | //! [show] 160 | // Show the frames 161 | //imshow(window_capture_name, frame); 162 | imshow(window_detection_name, frame_threshold); 163 | */ 164 | updateImage(); 165 | 166 | waitKey(); 167 | return 0; 168 | } 169 | -------------------------------------------------------------------------------- /region_detection_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(region_detection_msgs) 3 | 4 | if ($ENV{ROS_VERSION} EQUAL "2") 5 | message(WARNING "ROS Version $ENV{ROS_VERSION} found, building for ros2") 6 | 7 | # Default to C99 8 | if(NOT CMAKE_C_STANDARD) 9 | set(CMAKE_C_STANDARD 99) 10 | endif() 11 | 12 | # Default to C++14 13 | if(NOT CMAKE_CXX_STANDARD) 14 | set(CMAKE_CXX_STANDARD 14) 15 | endif() 16 | 17 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 18 | add_compile_options(-Wall -Wextra -Wpedantic) 19 | endif() 20 | 21 | set(SRV_DEPS 22 | sensor_msgs 23 | std_msgs 24 | geometry_msgs 25 | ) 26 | 27 | # find dependencies 28 | find_package(ament_cmake REQUIRED) 29 | find_package(rosidl_default_generators REQUIRED) 30 | find_package(builtin_interfaces REQUIRED) 31 | find_package(sensor_msgs REQUIRED) 32 | find_package(std_msgs REQUIRED) 33 | find_package(geometry_msgs REQUIRED) 34 | 35 | set(MSG_FILES 36 | msg/PoseSet.msg 37 | ) 38 | 39 | set(SRV_FILES 40 | srv/CropData.srv 41 | srv/DetectRegions.srv 42 | srv/ShowSelectableRegions.srv 43 | srv/GetSelectedRegions.srv 44 | ) 45 | 46 | rosidl_generate_interfaces(${PROJECT_NAME} 47 | ${MSG_FILES} 48 | ${SRV_FILES} 49 | DEPENDENCIES ${SRV_DEPS} 50 | ) 51 | 52 | if(BUILD_TESTING) 53 | find_package(ament_lint_auto REQUIRED) 54 | # the following line skips the linter which checks for copyrights 55 | # uncomment the line when a copyright and license is not present in all source files 56 | #set(ament_cmake_copyright_FOUND TRUE) 57 | # the following line skips cpplint (only works in a git repo) 58 | # uncomment the line when this package is not in a git repo 59 | #set(ament_cmake_cpplint_FOUND TRUE) 60 | ament_lint_auto_find_test_dependencies() 61 | endif() 62 | 63 | ament_export_dependencies(rosidl_default_runtime) 64 | ament_package() 65 | 66 | return() 67 | endif() 68 | 69 | -------------------------------------------------------------------------------- /region_detection_msgs/msg/PoseSet.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseArray[] pose_arrays 2 | -------------------------------------------------------------------------------- /region_detection_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | region_detection_msgs 5 | 0.0.0 6 | Messages and service to leverage the region detection core functionality 7 | Jorge Nicho 8 | BSD 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | builtin_interfaces 14 | sensor_msgs 15 | std_msgs 16 | geometry_msgs 17 | 18 | rosidl_default_runtime 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | rosidl_interface_packages 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /region_detection_msgs/srv/CropData.srv: -------------------------------------------------------------------------------- 1 | # inputs 2 | 3 | geometry_msgs/PoseArray[] crop_regions 4 | geometry_msgs/PoseArray[] input_data 5 | 6 | --- 7 | # outputs 8 | 9 | PoseSet[] cropped_data 10 | bool succeeded 11 | string err_msg 12 | 13 | -------------------------------------------------------------------------------- /region_detection_msgs/srv/DetectRegions.srv: -------------------------------------------------------------------------------- 1 | # Inputs 2 | sensor_msgs/Image[] images 3 | sensor_msgs/PointCloud2[] clouds 4 | geometry_msgs/TransformStamped[] transforms # transforms the pointclouds into the toolpaths frame id 5 | 6 | --- 7 | 8 | # outputs 9 | geometry_msgs/PoseArray[] detected_regions 10 | bool succeeded 11 | string err_msg -------------------------------------------------------------------------------- /region_detection_msgs/srv/GetSelectedRegions.srv: -------------------------------------------------------------------------------- 1 | --- 2 | int32[] selected_regions_indices -------------------------------------------------------------------------------- /region_detection_msgs/srv/ShowSelectableRegions.srv: -------------------------------------------------------------------------------- 1 | # inputs 2 | geometry_msgs/PoseArray[] selectable_regions 3 | bool start_selected 4 | 5 | --- -------------------------------------------------------------------------------- /region_detection_rclcpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(region_detection_rclcpp) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(PCL REQUIRED COMPONENTS io) 19 | 20 | # find dependencies 21 | find_package(rclcpp REQUIRED) 22 | find_package(rclcpp_components REQUIRED) 23 | find_package(region_detection_msgs REQUIRED) 24 | find_package(interactive_markers REQUIRED) 25 | find_package(region_detection_core REQUIRED) 26 | find_package(pcl_conversions REQUIRED) 27 | find_package(sensor_msgs REQUIRED) 28 | find_package(tf2_ros REQUIRED) 29 | find_package(tf2_geometry_msgs REQUIRED) 30 | find_package(visualization_msgs REQUIRED) 31 | find_package(cv_bridge REQUIRED) 32 | find_package(tf2_eigen REQUIRED) 33 | 34 | ### Build 35 | add_executable(interactive_region_selection src/interactive_region_selection.cpp) 36 | target_include_directories(interactive_region_selection PUBLIC 37 | "$" 38 | "$") 39 | target_include_directories(interactive_region_selection SYSTEM PUBLIC) 40 | target_link_libraries(interactive_region_selection 41 | region_detection_core::region_detection_core) 42 | ament_target_dependencies(interactive_region_selection 43 | rclcpp rclcpp_components 44 | region_detection_msgs 45 | visualization_msgs 46 | tf2_eigen 47 | std_msgs 48 | interactive_markers) 49 | 50 | add_executable(region_detector_server src/region_detector_server.cpp) 51 | target_include_directories(region_detector_server PUBLIC 52 | "$" 53 | "$") 54 | target_include_directories(region_detector_server SYSTEM PUBLIC) 55 | target_link_libraries(region_detector_server 56 | ${PCL_LIBRARIES} 57 | region_detection_core::region_detection_core) 58 | ament_target_dependencies(region_detector_server 59 | rclcpp rclcpp_components 60 | region_detection_msgs 61 | visualization_msgs 62 | pcl_conversions 63 | cv_bridge 64 | tf2_eigen 65 | std_msgs 66 | sensor_msgs) 67 | 68 | add_executable(crop_data_server src/crop_data_server.cpp) 69 | target_include_directories(crop_data_server PUBLIC 70 | "$" 71 | "$") 72 | target_include_directories(crop_data_server SYSTEM PUBLIC) 73 | target_link_libraries(crop_data_server 74 | region_detection_core::region_detection_core) 75 | ament_target_dependencies(crop_data_server 76 | rclcpp rclcpp_components 77 | region_detection_msgs 78 | pcl_conversions 79 | tf2_eigen 80 | std_msgs 81 | sensor_msgs) 82 | 83 | 84 | ### Install 85 | install(TARGETS interactive_region_selection region_detector_server crop_data_server 86 | DESTINATION lib/${PROJECT_NAME}) 87 | 88 | if(BUILD_TESTING) 89 | find_package(ament_lint_auto REQUIRED) 90 | # the following line skips the linter which checks for copyrights 91 | # uncomment the line when a copyright and license is not present in all source files 92 | #set(ament_cmake_copyright_FOUND TRUE) 93 | # the following line skips cpplint (only works in a git repo) 94 | # uncomment the line when this package is not in a git repo 95 | #set(ament_cmake_cpplint_FOUND TRUE) 96 | ament_lint_auto_find_test_dependencies() 97 | endif() 98 | 99 | ament_package() 100 | -------------------------------------------------------------------------------- /region_detection_rclcpp/README.md: -------------------------------------------------------------------------------- 1 | # Region Detection Rclcpp 2 | ### Summary 3 | This package contains ROS2 nodes that make the functionality in the `region_detection_core` package available to a ROS2 environment. 4 | 5 | --- 6 | ### Nodes 7 | #### region_detector_server: 8 | Detects contours from 2d images and 3d point clouds 9 | - Parameters: 10 | - region_detection_cfg_file: absolute path the the config file 11 | - Services 12 | - detect_regions: service that detects the contours of the regions found in the input images and point clouds. 13 | - Publications: 14 | - detected_regions: Marker arrays that help visualize the detected regions 15 | 16 | #### interactive_region_selection 17 | Shows the region contours as clickable interactive markers in Rviz 18 | - Parameters: 19 | - region_height: the height of each region shown 20 | - Services: 21 | - show_selectable_regions: Shows the specified regions as clickable markers in rviz 22 | - get_selected_regions: Returns the indices of the regions that are currently selected 23 | 24 | #### crop_data_server 25 | Crops data using the region contours as a boundary 26 | - Parameters: 27 | - region_crop: absolute path to the configuration file containing the configuration parameters 28 | - Services: 29 | - crop_data: crops the data that falls outside the region contour. -------------------------------------------------------------------------------- /region_detection_rclcpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | region_detection_rclcpp 5 | 0.0.0 6 | Various nodes that allow detecting regions and cropping data 7 | Jorge Nicho 8 | BSD 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_components 14 | region_detection_core 15 | region_detection_msgs 16 | interactive_markers 17 | pcl_conversions 18 | sensor_msgs 19 | tf2_ros 20 | tf2_geometry_msgs 21 | visualization_msgs 22 | cv_bridge 23 | tf2_eigen 24 | 25 | launch_xml 26 | 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /region_detection_rclcpp/src/crop_data_server.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file crop_data_server.cpp 4 | * @date Aug 4, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | 50 | static const std::string CROP_DATA_SERVICE = "crop_data"; 51 | 52 | class CropDataServer 53 | { 54 | public: 55 | CropDataServer(std::shared_ptr node) : node_(node), logger_(node->get_logger()) 56 | { 57 | // creating service 58 | crop_data_server_ = 59 | node->create_service(CROP_DATA_SERVICE, 60 | std::bind(&CropDataServer::cropDataCallback, 61 | this, 62 | std::placeholders::_1, 63 | std::placeholders::_2, 64 | std::placeholders::_3)); 65 | 66 | // check parameters 67 | loadRegionCropConfig(); 68 | } 69 | 70 | ~CropDataServer() {} 71 | 72 | private: 73 | region_detection_core::RegionCropConfig loadRegionCropConfig() 74 | { 75 | region_detection_core::RegionCropConfig cfg; 76 | const std::string param_ns = "region_crop."; 77 | cfg.scale_factor = node_->get_parameter(param_ns + "scale_factor").as_double(); 78 | cfg.plane_dist_threshold = node_->get_parameter(param_ns + "plane_dist_threshold").as_double(); 79 | double heigth_limits_min = node_->get_parameter(param_ns + "heigth_limits_min").as_double(); 80 | double heigth_limits_max = node_->get_parameter(param_ns + "heigth_limits_max").as_double(); 81 | cfg.dir_estimation_method = static_cast( 82 | node_->get_parameter(param_ns + "dir_estimation_method").as_int()); 83 | std::vector user_dir = node_->get_parameter(param_ns + "user_dir").as_double_array(); 84 | std::vector view_point = node_->get_parameter(param_ns + "view_point").as_double_array(); 85 | 86 | cfg.heigth_limits = std::make_pair(heigth_limits_min, heigth_limits_max); 87 | cfg.user_dir = Eigen::Map(user_dir.data(), user_dir.size()); 88 | cfg.view_point = Eigen::Map(view_point.data(), view_point.size()); 89 | 90 | return cfg; 91 | } 92 | 93 | void cropDataCallback(const std::shared_ptr request_header, 94 | const std::shared_ptr request, 95 | const std::shared_ptr response) 96 | { 97 | using namespace region_detection_core; 98 | using namespace region_detection_msgs::msg; 99 | using namespace pcl; 100 | using Cloud = PointCloud; 101 | 102 | (void)request_header; 103 | 104 | // use detected regions to crop 105 | RegionCropConfig region_crop_cfg = loadRegionCropConfig(); 106 | RegionCrop region_crop; 107 | region_crop.setConfig(region_crop_cfg); 108 | for (std::size_t i = 0; i < request->crop_regions.size(); i++) 109 | { 110 | // converting region into datatype 111 | RegionCrop::EigenPose3dVector crop_region; 112 | const geometry_msgs::msg::PoseArray& crop_region_poses = request->crop_regions[i]; 113 | std::transform(crop_region_poses.poses.begin(), 114 | crop_region_poses.poses.end(), 115 | std::back_inserter(crop_region), 116 | [](const geometry_msgs::msg::Pose& pose) { 117 | Eigen::Isometry3d eig_pose; 118 | tf2::fromMsg(pose, eig_pose); 119 | return eig_pose; 120 | }); 121 | 122 | PoseSet cropped_dataset; 123 | region_crop.setRegion(crop_region); 124 | for (std::size_t j = 0; j < request->input_data.size(); j++) 125 | { 126 | const geometry_msgs::msg::PoseArray& segment_poses = request->input_data[j]; 127 | 128 | Cloud::Ptr segments_points = boost::make_shared(); 129 | segments_points->reserve(segment_poses.poses.size()); 130 | 131 | std::transform(segment_poses.poses.begin(), 132 | segment_poses.poses.end(), 133 | std::back_inserter(*segments_points), 134 | [](const geometry_msgs::msg::Pose& pose) { 135 | PointXYZ p; 136 | p.x = pose.position.x; 137 | p.y = pose.position.y; 138 | p.z = pose.position.z; 139 | return p; 140 | }); 141 | 142 | region_crop.setInput(segments_points); 143 | std::vector inlier_indices = region_crop.filter(); 144 | 145 | if (inlier_indices.empty()) 146 | { 147 | continue; 148 | } 149 | RCLCPP_INFO(logger_, "Found %i inliers in segment %j within region %i", inlier_indices.size(), j, i); 150 | 151 | // extracting poinst in raster 152 | geometry_msgs::msg::PoseArray cropped_segment_poses; 153 | std::for_each(inlier_indices.begin(), inlier_indices.end(), [&cropped_segment_poses, &segment_poses](int idx) { 154 | cropped_segment_poses.poses.push_back(segment_poses.poses[idx]); 155 | }); 156 | cropped_dataset.pose_arrays.push_back(cropped_segment_poses); 157 | } 158 | response->cropped_data.push_back(cropped_dataset); 159 | } 160 | 161 | if (response->cropped_data.empty()) 162 | { 163 | response->succeeded = false; 164 | response->err_msg = "Failed to crop toolpaths"; 165 | RCLCPP_ERROR_STREAM(logger_, response->err_msg); 166 | } 167 | 168 | response->succeeded = true; 169 | } 170 | 171 | // ros interfaces 172 | rclcpp::Service::SharedPtr crop_data_server_; 173 | std::shared_ptr node_; 174 | rclcpp::Logger logger_; 175 | }; 176 | 177 | int main(int argc, char** argv) 178 | { 179 | // force flush of the stdout buffer. 180 | // this ensures a correct sync of all prints 181 | // even when executed simultaneously within the launch file. 182 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 183 | 184 | rclcpp::init(argc, argv); 185 | rclcpp::NodeOptions options; 186 | options.automatically_declare_parameters_from_overrides(true); 187 | std::shared_ptr node = std::make_shared("crop_data_server", options); 188 | CropDataServer region_detector(node); 189 | rclcpp::spin(node); 190 | return 0; 191 | } 192 | -------------------------------------------------------------------------------- /region_detection_rclcpp/src/interactive_region_selection.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file interactive_region_selection.cpp 4 | * @date Aug 4, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | 56 | #include 57 | #include 58 | 59 | static const std::string REGION_ID_PREFIX = "region_"; 60 | static const std::string SHOW_SELECTABLE_REGIONS_SERVICE = "show_selectable_regions"; 61 | static const std::string GET_SELECTED_REGIONS_SERVICE = "get_selected_regions"; 62 | 63 | static const std::string DEFAULT_FRAME_ID = "world"; 64 | 65 | namespace selection_colors_rgba 66 | { 67 | using RGBA = std::tuple; 68 | static const RGBA SELECTED = { 1.0, 1.0, 0.0, 1 }; 69 | static const RGBA UNSELECTED = { 0.3, 0.3, 0.3, 1 }; 70 | } // namespace selection_colors_rgba 71 | 72 | class InteractiveRegionManager 73 | { 74 | public: 75 | InteractiveRegionManager(std::shared_ptr node) 76 | : node_(node), interactive_marker_server_("InteractiveRegionManager", node) 77 | { 78 | // loading parameters 79 | region_height_ = node_->get_parameter("region_height").as_double(); 80 | 81 | // creating service 82 | show_selectable_regions_server_ = node->create_service( 83 | SHOW_SELECTABLE_REGIONS_SERVICE, 84 | std::bind(&InteractiveRegionManager::showSelectableRegionsCallback, 85 | this, 86 | std::placeholders::_1, 87 | std::placeholders::_2, 88 | std::placeholders::_3)); 89 | get_selected_regions_server_ = node->create_service( 90 | GET_SELECTED_REGIONS_SERVICE, 91 | std::bind(&InteractiveRegionManager::getSelectedRegionsCallback, 92 | this, 93 | std::placeholders::_1, 94 | std::placeholders::_2, 95 | std::placeholders::_3)); 96 | } 97 | 98 | ~InteractiveRegionManager() {} 99 | 100 | private: 101 | void showSelectableRegionsCallback( 102 | const std::shared_ptr request_header, 103 | const std::shared_ptr request, 104 | const std::shared_ptr response) 105 | { 106 | (void)request_header; 107 | (void)response; 108 | setRegions(request->selectable_regions, request->start_selected); 109 | } 110 | 111 | void 112 | getSelectedRegionsCallback(const std::shared_ptr request_header, 113 | const std::shared_ptr request, 114 | const std::shared_ptr response) 115 | { 116 | (void)request_header; 117 | (void)request; 118 | response->selected_regions_indices = getSelectedRegions(); 119 | } 120 | 121 | void buttonClickCallback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback) 122 | { 123 | using namespace visualization_msgs::msg; 124 | if (feedback->event_type != visualization_msgs::msg::InteractiveMarkerFeedback::BUTTON_CLICK) 125 | { 126 | return; 127 | } 128 | 129 | visualization_msgs::msg::InteractiveMarker int_marker; 130 | if (!interactive_marker_server_.get(feedback->marker_name, int_marker)) 131 | { 132 | RCLCPP_WARN(node_->get_logger(), "The marker with id %s was not found", feedback->client_id.c_str()); 133 | return; 134 | } 135 | bool selected = int_marker.controls.front().markers.front().action == Marker::ADD; 136 | 137 | int_marker.controls.front().markers.front().action = !selected ? Marker::ADD : Marker::DELETE; 138 | std_msgs::msg::ColorRGBA rgba_msg; 139 | std::tie(rgba_msg.r, rgba_msg.g, rgba_msg.b, rgba_msg.a) = 140 | !selected ? selection_colors_rgba::SELECTED : selection_colors_rgba::UNSELECTED; 141 | int_marker.controls.front().markers.front().color = rgba_msg; 142 | 143 | interactive_marker_server_.insert(int_marker); 144 | interactive_marker_server_.applyChanges(); 145 | } 146 | 147 | void setRegions(const std::vector& regions, bool selected) 148 | { 149 | using namespace Eigen; 150 | using namespace interactive_markers; 151 | 152 | // clear all interactive markers 153 | interactive_marker_server_.clear(); 154 | interactive_marker_server_.applyChanges(); 155 | if(regions.empty()) 156 | { 157 | return; 158 | } 159 | 160 | // create template marker 161 | visualization_msgs::msg::Marker marker; 162 | marker.scale.x = marker.scale.y = marker.scale.z = 1; 163 | marker.type = marker.TRIANGLE_LIST; 164 | marker.action = selected ? marker.ADD : marker.DELETE; // marking selection flag 165 | marker.header.frame_id = 166 | regions.front().header.frame_id.empty() ? DEFAULT_FRAME_ID : regions.front().header.frame_id; 167 | 168 | std::tie(marker.color.r, marker.color.g, marker.color.b, marker.color.a) = 169 | selected ? selection_colors_rgba::SELECTED : selection_colors_rgba::UNSELECTED; 170 | 171 | InteractiveMarkerServer::FeedbackCallback button_callback_ = InteractiveMarkerServer::FeedbackCallback( 172 | boost::bind(&InteractiveRegionManager::buttonClickCallback, this, _1)); 173 | 174 | // creating triangles 175 | geometry_msgs::msg::Point p1, p2, p3, p4; 176 | for (std::size_t i = 0; i < regions.size(); i++) 177 | { 178 | RCLCPP_INFO(node_->get_logger(), "Adding region %i with height %f", i, region_height_); 179 | marker.points.clear(); 180 | geometry_msgs::msg::PoseArray region = regions[i]; 181 | for (std::size_t j = 1; j < region.poses.size(); j++) 182 | { 183 | // converting to eigen pose 184 | Eigen::Affine3d pose1, pose2; 185 | tf2::fromMsg(region.poses[j - 1], pose1); 186 | tf2::fromMsg(region.poses[j], pose2); 187 | 188 | // computing points 189 | Vector3d eig_point; 190 | eig_point = pose1 * (0.5 * region_height_ * Vector3d::UnitZ()); 191 | p1 = tf2::toMsg(eig_point); 192 | 193 | eig_point = pose2 * (0.5 * region_height_ * Vector3d::UnitZ()); 194 | p2 = tf2::toMsg(eig_point); 195 | 196 | eig_point = pose2 * (-0.5 * region_height_ * Vector3d::UnitZ()); 197 | p3 = tf2::toMsg(eig_point); 198 | 199 | eig_point = pose1 * (-0.5 * region_height_ * Vector3d::UnitZ()); 200 | p4 = tf2::toMsg(eig_point); 201 | 202 | // creating triangles 203 | std::vector triangle_points = { p1, p2, p3, p3, p4, p1 }; 204 | std::for_each(triangle_points.begin(), triangle_points.end(), [&marker](const geometry_msgs::msg::Point& p) { 205 | marker.points.push_back(p); 206 | }); 207 | 208 | triangle_points = { p1, p4, p3, p3, p2, p1 }; 209 | std::for_each(triangle_points.begin(), triangle_points.end(), [&marker](const geometry_msgs::msg::Point& p) { 210 | marker.points.push_back(p); 211 | }); 212 | } 213 | 214 | // creating region interactive marker now 215 | visualization_msgs::msg::InteractiveMarker int_marker; 216 | int_marker.name = REGION_ID_PREFIX + std::to_string(i); 217 | int_marker.pose = tf2::toMsg(Affine3d::Identity()); 218 | 219 | // create button control 220 | visualization_msgs::msg::InteractiveMarkerControl button_control; 221 | button_control.interaction_mode = button_control.BUTTON; 222 | button_control.markers.push_back(marker); 223 | button_control.name = "button_" + int_marker.name; 224 | button_control.always_visible = true; 225 | 226 | // fill interactive marker 227 | int_marker.controls.push_back(button_control); 228 | int_marker.scale = 1; 229 | int_marker.header.frame_id = marker.header.frame_id; 230 | int_marker.description = int_marker.name; 231 | 232 | // add to server 233 | interactive_marker_server_.insert(int_marker, button_callback_); 234 | } 235 | 236 | // apply changes 237 | interactive_marker_server_.applyChanges(); 238 | } 239 | 240 | std::vector getSelectedRegions() 241 | { 242 | using namespace interactive_markers; 243 | using namespace visualization_msgs::msg; 244 | 245 | std::vector selected_indices; 246 | for (std::size_t i = 0; i < interactive_marker_server_.size(); i++) 247 | { 248 | std::string id = REGION_ID_PREFIX + std::to_string(i); 249 | InteractiveMarker int_marker; 250 | interactive_marker_server_.get(id, int_marker); 251 | bool selected = int_marker.controls.front().markers.front().action == Marker::ADD; 252 | if (selected) 253 | { 254 | selected_indices.push_back(i); 255 | } 256 | } 257 | return selected_indices; 258 | } 259 | 260 | // rclcpp 261 | std::shared_ptr node_; 262 | interactive_markers::InteractiveMarkerServer interactive_marker_server_; 263 | rclcpp::Service::SharedPtr show_selectable_regions_server_; 264 | rclcpp::Service::SharedPtr get_selected_regions_server_; 265 | 266 | // parameters 267 | double region_height_; 268 | }; 269 | 270 | int main(int argc, char** argv) 271 | { 272 | // force flush of the stdout buffer. 273 | // this ensures a correct sync of all prints 274 | // even when executed simultaneously within the launch file. 275 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 276 | 277 | rclcpp::init(argc, argv); 278 | rclcpp::NodeOptions options; 279 | options.automatically_declare_parameters_from_overrides(true); 280 | std::shared_ptr node = std::make_shared("interactive_region_selection", options); 281 | InteractiveRegionManager mngr(node); 282 | rclcpp::spin(node); 283 | return 0; 284 | } 285 | -------------------------------------------------------------------------------- /region_detection_rclcpp/src/region_detector_server.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file region_detector_server.cpp 4 | * @date Aug 4, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | 50 | #include 51 | 52 | static const std::string REGION_MARKERS_TOPIC = "detected_regions"; 53 | static const std::string DETECT_REGIONS_SERVICE = "detect_regions"; 54 | static const std::string CLOSED_REGIONS_NS = "closed_regions"; 55 | 56 | typedef std::vector > EigenPose3dVector; 57 | 58 | static geometry_msgs::msg::Pose pose3DtoPoseMsg(const std::array& p) 59 | { 60 | using namespace Eigen; 61 | geometry_msgs::msg::Pose pose_msg; 62 | Eigen::Affine3d eigen_pose = Translation3d(Vector3d(p[0], p[1], p[2])) * AngleAxisd(p[3], Vector3d::UnitX()) * 63 | AngleAxisd(p[4], Vector3d::UnitY()) * AngleAxisd(p[5], Vector3d::UnitZ()); 64 | 65 | pose_msg = tf2::toMsg(eigen_pose); 66 | return std::move(pose_msg); 67 | } 68 | 69 | visualization_msgs::msg::MarkerArray convertToAxisMarkers(const std::vector& path, 70 | const std::string& frame_id, 71 | const std::string& ns, 72 | const std::size_t& start_id, 73 | const double& axis_scale, 74 | const double& axis_length, 75 | const std::array& offset) 76 | { 77 | using namespace Eigen; 78 | 79 | visualization_msgs::msg::MarkerArray markers; 80 | 81 | auto create_line_marker = [&](const int id, 82 | const std::tuple& rgba) -> visualization_msgs::msg::Marker { 83 | visualization_msgs::msg::Marker line_marker; 84 | line_marker.action = line_marker.ADD; 85 | std::tie(line_marker.color.r, line_marker.color.g, line_marker.color.b, line_marker.color.a) = rgba; 86 | line_marker.header.frame_id = frame_id; 87 | line_marker.type = line_marker.LINE_LIST; 88 | line_marker.id = id; 89 | line_marker.lifetime = rclcpp::Duration(0); 90 | line_marker.ns = ns; 91 | std::tie(line_marker.scale.x, line_marker.scale.y, line_marker.scale.z) = std::make_tuple(axis_scale, 0.0, 0.0); 92 | line_marker.pose = pose3DtoPoseMsg(offset); 93 | return std::move(line_marker); 94 | }; 95 | 96 | // markers for each axis line 97 | int marker_id = start_id; 98 | visualization_msgs::msg::Marker x_axis_marker = create_line_marker(++marker_id, std::make_tuple(1.0, 0.0, 0.0, 1.0)); 99 | visualization_msgs::msg::Marker y_axis_marker = create_line_marker(++marker_id, std::make_tuple(0.0, 1.0, 0.0, 1.0)); 100 | visualization_msgs::msg::Marker z_axis_marker = create_line_marker(++marker_id, std::make_tuple(0.0, 0.0, 1.0, 1.0)); 101 | 102 | auto add_axis_line = [](const Isometry3d& eigen_pose, 103 | const Vector3d& dir, 104 | const geometry_msgs::msg::Point& p1, 105 | visualization_msgs::msg::Marker& marker) { 106 | geometry_msgs::msg::Point p2; 107 | Eigen::Vector3d line_endpoint; 108 | 109 | // axis endpoint 110 | line_endpoint = eigen_pose * dir; 111 | std::tie(p2.x, p2.y, p2.z) = std::make_tuple(line_endpoint.x(), line_endpoint.y(), line_endpoint.z()); 112 | 113 | // adding line 114 | marker.points.push_back(p1); 115 | marker.points.push_back(p2); 116 | }; 117 | 118 | for (auto& poses : path) 119 | { 120 | for (auto& pose : poses) 121 | { 122 | geometry_msgs::msg::Point p1; 123 | std::tie(p1.x, p1.y, p1.z) = 124 | std::make_tuple(pose.translation().x(), pose.translation().y(), pose.translation().z()); 125 | add_axis_line(pose, Vector3d::UnitX() * axis_length, p1, x_axis_marker); 126 | add_axis_line(pose, Vector3d::UnitY() * axis_length, p1, y_axis_marker); 127 | add_axis_line(pose, Vector3d::UnitZ() * axis_length, p1, z_axis_marker); 128 | } 129 | } 130 | 131 | markers.markers.push_back(x_axis_marker); 132 | markers.markers.push_back(y_axis_marker); 133 | markers.markers.push_back(z_axis_marker); 134 | return std::move(markers); 135 | } 136 | 137 | visualization_msgs::msg::MarkerArray 138 | convertToDottedLineMarker(const std::vector& path, 139 | const std::string& frame_id, 140 | const std::string& ns, 141 | const std::size_t& start_id = 0, 142 | const std::array& rgba_line = { 1.0, 1.0, 0.2, 1.0 }, 143 | const std::array& rgba_point = { 0.1, .8, 0.2, 1.0 }, 144 | const std::array& offset = { 0, 0, 0, 0, 0, 0 }, 145 | const float& line_width = 0.001, 146 | const float& point_size = 0.0015) 147 | { 148 | visualization_msgs::msg::MarkerArray markers_msgs; 149 | visualization_msgs::msg::Marker line_marker, points_marker; 150 | 151 | // configure line marker 152 | line_marker.action = line_marker.ADD; 153 | std::tie(line_marker.color.r, line_marker.color.g, line_marker.color.b, line_marker.color.a) = 154 | std::make_tuple(rgba_line[0], rgba_line[1], rgba_line[2], rgba_line[3]); 155 | line_marker.header.frame_id = frame_id; 156 | line_marker.type = line_marker.LINE_STRIP; 157 | line_marker.id = start_id; 158 | line_marker.lifetime = rclcpp::Duration(0); 159 | line_marker.ns = ns; 160 | std::tie(line_marker.scale.x, line_marker.scale.y, line_marker.scale.z) = std::make_tuple(line_width, 0.0, 0.0); 161 | line_marker.pose = pose3DtoPoseMsg(offset); 162 | 163 | // configure point marker 164 | points_marker = line_marker; 165 | points_marker.type = points_marker.POINTS; 166 | points_marker.ns = ns; 167 | std::tie(points_marker.color.r, points_marker.color.g, points_marker.color.b, points_marker.color.a) = 168 | std::make_tuple(rgba_point[0], rgba_point[1], rgba_point[2], rgba_point[3]); 169 | std::tie(points_marker.scale.x, points_marker.scale.y, points_marker.scale.z) = 170 | std::make_tuple(point_size, point_size, point_size); 171 | 172 | int id_counter = start_id; 173 | for (auto& poses : path) 174 | { 175 | line_marker.points.clear(); 176 | points_marker.points.clear(); 177 | line_marker.points.reserve(poses.size()); 178 | points_marker.points.reserve(poses.size()); 179 | for (auto& pose : poses) 180 | { 181 | geometry_msgs::msg::Point p; 182 | std::tie(p.x, p.y, p.z) = std::make_tuple(pose.translation().x(), pose.translation().y(), pose.translation().z()); 183 | line_marker.points.push_back(p); 184 | points_marker.points.push_back(p); 185 | } 186 | 187 | line_marker.id = (++id_counter); 188 | points_marker.id = (++id_counter); 189 | markers_msgs.markers.push_back(line_marker); 190 | markers_msgs.markers.push_back(points_marker); 191 | } 192 | 193 | return markers_msgs; 194 | } 195 | 196 | class RegionDetectorServer 197 | { 198 | public: 199 | RegionDetectorServer(std::shared_ptr node) 200 | : node_(node), logger_(node->get_logger()), marker_pub_timer_(nullptr) 201 | { 202 | // load parameters 203 | 204 | // creating service 205 | detect_regions_server_ = node->create_service( 206 | DETECT_REGIONS_SERVICE, 207 | std::bind(&RegionDetectorServer::detectRegionsCallback, 208 | this, 209 | std::placeholders::_1, 210 | std::placeholders::_2, 211 | std::placeholders::_3)); 212 | 213 | region_markers_pub_ = 214 | node->create_publisher(REGION_MARKERS_TOPIC, rclcpp::QoS(1)); 215 | 216 | // run this for verification of parameters 217 | loadRegionDetectionConfig(); 218 | } 219 | 220 | ~RegionDetectorServer() {} 221 | 222 | private: 223 | region_detection_core::RegionDetectionConfig loadRegionDetectionConfig() 224 | { 225 | std::string yaml_config_file = node_->get_parameter("region_detection_cfg_file").as_string(); 226 | return region_detection_core::RegionDetectionConfig::loadFromFile(yaml_config_file); 227 | } 228 | 229 | void publishRegions(const std::string& frame_id, const std::string ns, const std::vector& regions) 230 | { 231 | using namespace std::chrono_literals; 232 | 233 | if (marker_pub_timer_) 234 | { 235 | marker_pub_timer_->cancel(); 236 | } 237 | 238 | // create markers to publish 239 | visualization_msgs::msg::MarkerArray region_markers; 240 | int id = 0; 241 | for (auto& poses : regions) 242 | { 243 | id++; 244 | visualization_msgs::msg::MarkerArray m = 245 | convertToAxisMarkers({ poses }, frame_id, ns + std::to_string(id), 0, 0.001, 0.01, { 0, 0, 0, 0, 0, 0 }); 246 | region_markers.markers.insert(region_markers.markers.end(), m.markers.begin(), m.markers.end()); 247 | 248 | m = convertToDottedLineMarker({ poses }, frame_id, ns + std::to_string(id)); 249 | region_markers.markers.insert(region_markers.markers.end(), m.markers.begin(), m.markers.end()); 250 | } 251 | marker_pub_timer_ = node_->create_wall_timer( 252 | 500ms, [this, region_markers]() -> void { region_markers_pub_->publish(region_markers); }); 253 | } 254 | 255 | void detectRegionsCallback(const std::shared_ptr request_header, 256 | const std::shared_ptr request, 257 | const std::shared_ptr response) 258 | { 259 | using namespace region_detection_core; 260 | using namespace pcl; 261 | 262 | (void)request_header; 263 | 264 | // converting to input for region detection 265 | RegionDetector::DataBundleVec data_vec; 266 | const std::string img_name_prefix = "img_input_"; 267 | const std::string pcd_file_prefix = "cloud_input_"; 268 | for (std::size_t i = 0; i < request->clouds.size(); i++) 269 | { 270 | RegionDetector::DataBundle data; 271 | pcl_conversions::toPCL(request->clouds[i], data.cloud_blob); 272 | cv_bridge::CvImagePtr img = cv_bridge::toCvCopy(request->images[i], sensor_msgs::image_encodings::RGBA8); 273 | data.image = img->image; 274 | cv::imwrite(img_name_prefix + std::to_string(i) + ".jpg", data.image); 275 | pcl::io::savePCDFile(pcd_file_prefix + std::to_string(i) + ".pcd", data.cloud_blob); 276 | data.transform = tf2::transformToEigen(request->transforms[i]); 277 | data_vec.push_back(data); 278 | } 279 | 280 | // region detection 281 | RegionDetectionConfig config = loadRegionDetectionConfig(); 282 | RegionDetector region_detector(config); 283 | RegionDetector::RegionResults region_detection_results; 284 | if (!region_detector.compute(data_vec, region_detection_results)) 285 | { 286 | response->succeeded = false; 287 | response->err_msg = "Failed to find closed regions"; 288 | RCLCPP_ERROR_STREAM(logger_, response->err_msg); 289 | return; 290 | } 291 | RCLCPP_INFO(logger_, "Found %i closed regions", region_detection_results.closed_regions_poses.size()); 292 | 293 | publishRegions( 294 | request->transforms.front().header.frame_id, CLOSED_REGIONS_NS, region_detection_results.closed_regions_poses); 295 | 296 | for (const EigenPose3dVector& region : region_detection_results.closed_regions_poses) 297 | { 298 | geometry_msgs::msg::PoseArray region_poses; 299 | for (const Eigen::Affine3d& pose : region) 300 | { 301 | region_poses.poses.push_back(tf2::toMsg(pose)); 302 | } 303 | response->detected_regions.push_back(region_poses); 304 | } 305 | response->succeeded = !response->detected_regions.empty(); 306 | } 307 | 308 | // ros interfaces 309 | rclcpp::Service::SharedPtr detect_regions_server_; 310 | rclcpp::Publisher::SharedPtr region_markers_pub_; 311 | std::shared_ptr node_; 312 | rclcpp::Logger logger_; 313 | rclcpp::TimerBase::SharedPtr marker_pub_timer_; 314 | }; 315 | 316 | int main(int argc, char** argv) 317 | { 318 | // force flush of the stdout buffer. 319 | // this ensures a correct sync of all prints 320 | // even when executed simultaneously within the launch file. 321 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 322 | 323 | rclcpp::init(argc, argv); 324 | rclcpp::NodeOptions options; 325 | options.automatically_declare_parameters_from_overrides(true); 326 | std::shared_ptr node = std::make_shared("region_detector", options); 327 | RegionDetectorServer region_detector(node); 328 | rclcpp::spin(node); 329 | return 0; 330 | } 331 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.0) 2 | project(region_detection_roscpp_demo) 3 | 4 | if (NOT $ENV{ROS_VERSION} EQUAL "1") 5 | message(WARNING "ROS Version $ENV{ROS_VERSION} found, skipping project ${PROJECT_NAME}") 6 | return() 7 | endif() 8 | 9 | # Default to C99 10 | if(NOT CMAKE_C_STANDARD) 11 | set(CMAKE_C_STANDARD 99) 12 | endif() 13 | 14 | # Default to C++14 15 | if(NOT CMAKE_CXX_STANDARD) 16 | set(CMAKE_CXX_STANDARD 14) 17 | endif() 18 | 19 | ## Find catkin macros and libraries 20 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 21 | ## is used, also find other catkin packages 22 | find_package(catkin REQUIRED 23 | roscpp 24 | visualization_msgs 25 | eigen_conversions 26 | pcl_ros 27 | ) 28 | 29 | find_package(region_detection_core REQUIRED) 30 | 31 | 32 | ################################################ 33 | ## Declare ROS dynamic reconfigure parameters ## 34 | ################################################ 35 | 36 | ## To declare and build dynamic reconfigure parameters within this 37 | ## package, follow these steps: 38 | ## * In the file package.xml: 39 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "dynamic_reconfigure" to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 44 | ## and list every .cfg file to be processed 45 | 46 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 47 | # generate_dynamic_reconfigure_options( 48 | # cfg/DynReconf1.cfg 49 | # cfg/DynReconf2.cfg 50 | # ) 51 | 52 | ################################### 53 | ## catkin specific configuration ## 54 | ################################### 55 | ## The catkin_package macro generates cmake config files for your package 56 | ## Declare things to be passed to dependent projects 57 | ## INCLUDE_DIRS: uncomment this if your package contains header files 58 | ## LIBRARIES: libraries you create in this project that dependent projects also need 59 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 60 | ## DEPENDS: system dependencies of this project that dependent projects also need 61 | catkin_package( 62 | INCLUDE_DIRS 63 | LIBRARIES 64 | CATKIN_DEPENDS roscpp visualization_msgs eigen_conversions 65 | DEPENDS region_detection_core 66 | ) 67 | 68 | ########### 69 | ## Build ## 70 | ########### 71 | 72 | ## Specify additional locations of header files 73 | ## Your package locations should be listed before other locations 74 | include_directories( 75 | ${catkin_INCLUDE_DIRS} 76 | ) 77 | 78 | add_executable(detect_regions_demo src/detect_regions_demo.cpp) 79 | target_link_libraries(detect_regions_demo 80 | ${catkin_LIBRARIES} 81 | region_detection_core::region_detection_core 82 | ) 83 | 84 | ############# 85 | ## Install ## 86 | ############# 87 | install(TARGETS detect_regions_demo 88 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | ) 90 | 91 | install(DIRECTORY 92 | launch 93 | config 94 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 95 | ) 96 | 97 | install(FILES 98 | package.xml 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 100 | ) 101 | 102 | ############# 103 | ## Testing ## 104 | ############# 105 | 106 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/README.md: -------------------------------------------------------------------------------- 1 | # Region Detection RosCpp Demo 2 | ### Summary 3 | This package contains a demo application meant to demonstrate how to use the capabilities in the main core library 4 | 5 | --- 6 | ### Setup 7 | - Download the [region_detection_test_data.zip](https://github.com/swri-robotics/Region-Detection/wiki/region_detection_test_data.zip) file 8 | - Extract the **region_detection_test_data.zip** file 9 | - CD into the **region_detection_test_data** directory 10 | - Create a shortcut to the directory 11 | ```bash 12 | ln -s $(pwd) $HOME/region_detection_data 13 | ``` 14 | --- 15 | ### Run Demo 16 | - Run launch file 17 | ```bash 18 | roslaunch region_detection_roscpp_demo detect_regions_demo.launch 19 | ``` 20 | 21 | You should see Rviz with a visualization of the regions detected. 22 | 23 | - Edit the `detect_regions_demo.launch` launch file and change the config file or the dataset used to see different results 24 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/config/data_list.yaml: -------------------------------------------------------------------------------- 1 | - image_file: color.png 2 | cloud_file: point_cloud.pcd 3 | transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -------------------------------------------------------------------------------- /region_detection_roscpp_demo/config/data_multiset.yaml: -------------------------------------------------------------------------------- 1 | - image_file: left/color.png 2 | cloud_file: left/point_cloud.pcd 3 | transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 4 | - image_file: right/color.png 5 | cloud_file: right/point_cloud.pcd 6 | transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -------------------------------------------------------------------------------- /region_detection_roscpp_demo/config/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /MarkerArray1 10 | - /MarkerArray1/Namespaces1 11 | Splitter Ratio: 0.5 12 | Tree Height: 640 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Input Cloud 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Autocompute Intensity Bounds: true 59 | Autocompute Value Bounds: 60 | Max Value: 10 61 | Min Value: -10 62 | Value: true 63 | Axis: Z 64 | Channel Name: intensity 65 | Class: rviz/PointCloud2 66 | Color: 255; 255; 255 67 | Color Transformer: RGB8 68 | Decay Time: 0 69 | Enabled: true 70 | Invert Rainbow: false 71 | Max Color: 255; 255; 255 72 | Max Intensity: 4096 73 | Min Color: 0; 0; 0 74 | Min Intensity: 0 75 | Name: Input Cloud 76 | Position Transformer: XYZ 77 | Queue Size: 10 78 | Selectable: true 79 | Size (Pixels): 3 80 | Size (m): 0.009999999776482582 81 | Style: Points 82 | Topic: /regions_cloud_input 83 | Unreliable: false 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Class: rviz/MarkerArray 88 | Enabled: true 89 | Marker Topic: /regions_results 90 | Name: MarkerArray 91 | Namespaces: 92 | closed_regions_axes1: false 93 | closed_regions_lines1: true 94 | open_regions_axes1: true 95 | open_regions_lines1: true 96 | Queue Size: 100 97 | Value: true 98 | - Alpha: 1 99 | Autocompute Intensity Bounds: true 100 | Autocompute Value Bounds: 101 | Max Value: 10 102 | Min Value: -10 103 | Value: true 104 | Axis: Z 105 | Channel Name: intensity 106 | Class: rviz/PointCloud2 107 | Color: 255; 255; 255 108 | Color Transformer: Intensity 109 | Decay Time: 0 110 | Enabled: false 111 | Invert Rainbow: false 112 | Max Color: 255; 255; 255 113 | Max Intensity: 4096 114 | Min Color: 0; 0; 0 115 | Min Intensity: 0 116 | Name: Inner Region Clouds 117 | Position Transformer: XYZ 118 | Queue Size: 10 119 | Selectable: true 120 | Size (Pixels): 5 121 | Size (m): 0.009999999776482582 122 | Style: Points 123 | Topic: /cropped_clouds 124 | Unreliable: false 125 | Use Fixed Frame: true 126 | Use rainbow: true 127 | Value: false 128 | - Alpha: 1 129 | Autocompute Intensity Bounds: true 130 | Autocompute Value Bounds: 131 | Max Value: 10 132 | Min Value: -10 133 | Value: true 134 | Axis: Z 135 | Channel Name: intensity 136 | Class: rviz/PointCloud2 137 | Color: 206; 92; 0 138 | Color Transformer: FlatColor 139 | Decay Time: 0 140 | Enabled: false 141 | Invert Rainbow: false 142 | Max Color: 255; 255; 255 143 | Max Intensity: 4096 144 | Min Color: 0; 0; 0 145 | Min Intensity: 0 146 | Name: Outer Region Clouds 147 | Position Transformer: XYZ 148 | Queue Size: 10 149 | Selectable: true 150 | Size (Pixels): 4 151 | Size (m): 0.009999999776482582 152 | Style: Points 153 | Topic: /cropped_clouds_reversed 154 | Unreliable: false 155 | Use Fixed Frame: true 156 | Use rainbow: true 157 | Value: false 158 | Enabled: true 159 | Global Options: 160 | Background Color: 48; 48; 48 161 | Default Light: true 162 | Fixed Frame: world 163 | Frame Rate: 30 164 | Name: root 165 | Tools: 166 | - Class: rviz/Interact 167 | Hide Inactive Objects: true 168 | - Class: rviz/MoveCamera 169 | - Class: rviz/Select 170 | - Class: rviz/FocusCamera 171 | - Class: rviz/Measure 172 | - Class: rviz/SetInitialPose 173 | Theta std deviation: 0.2617993950843811 174 | Topic: /initialpose 175 | X std deviation: 0.5 176 | Y std deviation: 0.5 177 | - Class: rviz/SetGoal 178 | Topic: /move_base_simple/goal 179 | - Class: rviz/PublishPoint 180 | Single click: true 181 | Topic: /clicked_point 182 | Value: true 183 | Views: 184 | Current: 185 | Class: rviz/Orbit 186 | Distance: 1.4484550952911377 187 | Enable Stereo Rendering: 188 | Stereo Eye Separation: 0.05999999865889549 189 | Stereo Focal Distance: 1 190 | Swap Stereo Eyes: false 191 | Value: false 192 | Focal Point: 193 | X: 0.03899062052369118 194 | Y: 0.4167930781841278 195 | Z: 0.40866175293922424 196 | Focal Shape Fixed Size: true 197 | Focal Shape Size: 0.05000000074505806 198 | Invert Z Axis: false 199 | Name: Current View 200 | Near Clip Distance: 0.009999999776482582 201 | Pitch: 0.7847976684570312 202 | Target Frame: 203 | Value: Orbit (rviz) 204 | Yaw: 4.698577880859375 205 | Saved: ~ 206 | Window Geometry: 207 | Displays: 208 | collapsed: false 209 | Height: 1025 210 | Hide Left Dock: false 211 | Hide Right Dock: true 212 | QMainWindow State: 000000ff00000000fd0000000400000000000001f700000347fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000347000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000005afc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005400000034700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 213 | Selection: 214 | collapsed: false 215 | Time: 216 | collapsed: false 217 | Tool Properties: 218 | collapsed: false 219 | Views: 220 | collapsed: true 221 | Width: 1853 222 | X: 67 223 | Y: 27 224 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/config/region_detect.yaml: -------------------------------------------------------------------------------- 1 | opencv: 2 | #methods: ["CLAHE", "EQUALIZE_HIST", "EQUALIZE_HIST_YUV", "HSV", 3 | #"INVERT", "GRAYSCALE", "THRESHOLD", "DILATION", "EROSION", "CANNY", "THINNING","RANGE"] 4 | methods: ["GRAYSCALE", "INVERT","DILATION", "THRESHOLD", "CANNY"] 5 | debug_mode_enable: true 6 | debug_window_name: "DEBUG_REGION_DETECTION" 7 | debug_wait_key: false 8 | hsv: 9 | h: [0, 180] 10 | s: [100, 255] 11 | v: [0, 40] 12 | threshold: 13 | type: 2 14 | value: 200 15 | canny: 16 | lower_threshold: 40 17 | upper_threshold: 200 18 | aperture_size: 1 19 | dilation: 20 | elem: 0 # 0: Rect, 1: Cross, 2: Ellipse 21 | kernel_size: 1 # 2n + 1 22 | erosion: 23 | elem: 3 # 0: Rect, 1: Cross, 2: Ellipse 24 | kernel_size: 2 # 2n + 1 25 | contour: 26 | method: 1 27 | mode: 0 # See cv::RetrievalModes 28 | range: 29 | low: 150 30 | high: 200 31 | clahe: 32 | clip_limit: 4.0 33 | tile_grid_size: [4, 4] 34 | pcl2d: 35 | downsampling_radius: 4.0 # pixel units 36 | split_dist: 6.0 # pixel units 37 | closed_curve_max_dist: 6.0 # pixel units 38 | simplification_min_points: 10 # applies simplification only if the closed curve has 10 points or more 39 | simplification_alpha: 24.0 # pixel units, used in concave hull step 40 | pcl: 41 | debug_mode_enable: false 42 | max_merge_dist: 0.01 43 | closed_curve_max_dist: 0.01 44 | simplification_min_dist: 0.01 45 | split_dist: 0.1 46 | min_num_points: 10 47 | stat_removal: 48 | enable: true 49 | kmeans: 100 50 | stddev: 3.0 51 | normal_est: 52 | downsampling_radius: 0.01 53 | search_radius: 0.02 54 | kdtree_epsilon: 0.001 55 | viewpoint_xyz: [0.0, 0.0, 100.0] 56 | 57 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/config/region_detect_mult_imgs.yaml: -------------------------------------------------------------------------------- 1 | opencv: 2 | #methods: ["CLAHE", "EQUALIZE_HIST_YUV", "HSV", "INVERT",... 3 | # "GRAYSCALE", "THRESHOLD", "DILATION", "EROSION", "CANNY", "THINNING","RANGE"] 4 | methods: ["GRAYSCALE", "INVERT","THRESHOLD", "CANNY"] 5 | debug_mode_enable: true 6 | debug_window_name: "DEBUG_REGION_DETECTION" 7 | debug_wait_key: false 8 | hsv: 9 | h: [0, 180] 10 | s: [100, 255] 11 | v: [0, 100] 12 | threshold: 13 | type: 2 14 | value: 200 15 | canny: 16 | lower_threshold: 100 17 | upper_threshold: 200 18 | aperture_size: 1 19 | dilation: 20 | elem: 2 # 0: Rect, 1: Cross, 2: Ellipse 21 | kernel_size: 1 # 2n + 1 22 | erosion: 23 | elem: 2 # 0: Rect, 1: Cross, 2: Ellipse 24 | kernel_size: 4 # 2n + 1 25 | contour: 26 | method: 1 27 | mode: 0 # See cv::RetrievalModes 28 | range: 29 | low: 190 30 | high: 255 31 | clahe: 32 | clip_limit: 4.0 33 | tile_grid_size: [4, 4] 34 | pcl2d: 35 | downsampling_radius: 4.0 # pixel units 36 | split_dist: 6.0 # pixel units 37 | closed_curve_max_dist: 6.0 # pixel units 38 | simplification_min_points: 10 # applies simplification only if the closed curve has 10 points or more 39 | simplification_alpha: 24.0 # pixel units, used in concave hull step 40 | pcl: 41 | debug_mode_enable: false 42 | max_merge_dist: 0.01 43 | closed_curve_max_dist: 0.01 44 | simplification_min_dist: 0.01 45 | split_dist: 0.1 46 | min_num_points: 10 47 | stat_removal: 48 | enable: true 49 | kmeans: 100 50 | stddev: 3.0 51 | normal_est: 52 | downsampling_radius: 0.01 53 | search_radius: 0.02 54 | kdtree_epsilon: 0.001 55 | viewpoint_xyz: [0.0, 0.0, 100.0] 56 | 57 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/launch/detect_regions_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/launch/detect_regions_multiset_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | region_detection_roscpp_demo 4 | 0.0.0 5 | The region_detection_roscpp_demo package 6 | Jorge Nicho 7 | BSD 8 | Jorge Nicho 9 | 10 | catkin 11 | region_detection_core 12 | roscpp 13 | visualization_msgs 14 | eigen_conversions 15 | pcl_ros 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /region_detection_roscpp_demo/src/detect_regions_demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author Jorge Nicho 3 | * @file detect_regions_demo.cpp 4 | * @date Jun 23, 2020 5 | * @copyright Copyright (c) 2020, Southwest Research Institute 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2020, Southwest Research Institute 9 | * All rights reserved. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above copyright 17 | * notice, this list of conditions and the following disclaimer in the 18 | * documentation and/or other materials provided with the distribution. 19 | * * Neither the name of the Southwest Research Institute, nor the names 20 | * of its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 26 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 27 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | 49 | #include "pcl_ros/point_cloud.h" 50 | 51 | #include 52 | #include 53 | 54 | using namespace region_detection_core; 55 | 56 | typedef std::vector> EigenPose3dVector; 57 | 58 | static const std::string REGIONS_MARKERS_TOPIC = "regions_results"; 59 | static const std::string INPUT_CLOUD_TOPIC = "regions_cloud_input"; 60 | static const std::string CROPPED_CLOUDS_TOPIC = "cropped_clouds"; 61 | static const std::string CROPPED_CLOUDS_REVERSE_TOPIC = "cropped_clouds_reversed"; 62 | static const std::string REFERENCE_FRAME_ID = "results_frame"; 63 | 64 | static geometry_msgs::Pose pose3DtoPoseMsg(const std::array& p) 65 | { 66 | using namespace Eigen; 67 | geometry_msgs::Pose pose_msg; 68 | Eigen::Affine3d eigen_pose = Translation3d(Vector3d(p[0], p[1], p[2])) * AngleAxisd(p[3], Vector3d::UnitX()) * 69 | AngleAxisd(p[4], Vector3d::UnitY()) * AngleAxisd(p[5], Vector3d::UnitZ()); 70 | 71 | tf::poseEigenToMsg(eigen_pose, pose_msg); 72 | return std::move(pose_msg); 73 | } 74 | 75 | visualization_msgs::MarkerArray convertToAxisMarkers(const std::vector& path, 76 | const std::string& frame_id, 77 | const std::string& ns, 78 | const std::size_t& start_id, 79 | const double& axis_scale, 80 | const double& axis_length, 81 | const std::array& offset) 82 | { 83 | using namespace Eigen; 84 | 85 | visualization_msgs::MarkerArray markers; 86 | 87 | auto create_line_marker = [&](const int id, 88 | const std::tuple& rgba) -> visualization_msgs::Marker { 89 | visualization_msgs::Marker line_marker; 90 | line_marker.action = line_marker.ADD; 91 | std::tie(line_marker.color.r, line_marker.color.g, line_marker.color.b, line_marker.color.a) = rgba; 92 | line_marker.header.frame_id = frame_id; 93 | line_marker.type = line_marker.LINE_LIST; 94 | line_marker.id = id; 95 | line_marker.lifetime = ros::Duration(0); 96 | line_marker.ns = ns; 97 | std::tie(line_marker.scale.x, line_marker.scale.y, line_marker.scale.z) = std::make_tuple(axis_scale, 0.0, 0.0); 98 | line_marker.pose = pose3DtoPoseMsg(offset); 99 | return std::move(line_marker); 100 | }; 101 | 102 | // markers for each axis line 103 | int marker_id = start_id; 104 | visualization_msgs::Marker x_axis_marker = create_line_marker(++marker_id, std::make_tuple(1.0, 0.0, 0.0, 1.0)); 105 | visualization_msgs::Marker y_axis_marker = create_line_marker(++marker_id, std::make_tuple(0.0, 1.0, 0.0, 1.0)); 106 | visualization_msgs::Marker z_axis_marker = create_line_marker(++marker_id, std::make_tuple(0.0, 0.0, 1.0, 1.0)); 107 | 108 | auto add_axis_line = [](const Isometry3d& eigen_pose, 109 | const Vector3d& dir, 110 | const geometry_msgs::Point& p1, 111 | visualization_msgs::Marker& marker) { 112 | geometry_msgs::Point p2; 113 | Eigen::Vector3d line_endpoint; 114 | 115 | // axis endpoint 116 | line_endpoint = eigen_pose * dir; 117 | std::tie(p2.x, p2.y, p2.z) = std::make_tuple(line_endpoint.x(), line_endpoint.y(), line_endpoint.z()); 118 | 119 | // adding line 120 | marker.points.push_back(p1); 121 | marker.points.push_back(p2); 122 | }; 123 | 124 | for (auto& poses : path) 125 | { 126 | for (auto& pose : poses) 127 | { 128 | geometry_msgs::Point p1; 129 | std::tie(p1.x, p1.y, p1.z) = 130 | std::make_tuple(pose.translation().x(), pose.translation().y(), pose.translation().z()); 131 | add_axis_line(pose, Vector3d::UnitX() * axis_length, p1, x_axis_marker); 132 | add_axis_line(pose, Vector3d::UnitY() * axis_length, p1, y_axis_marker); 133 | add_axis_line(pose, Vector3d::UnitZ() * axis_length, p1, z_axis_marker); 134 | } 135 | } 136 | 137 | markers.markers.push_back(x_axis_marker); 138 | markers.markers.push_back(y_axis_marker); 139 | markers.markers.push_back(z_axis_marker); 140 | return std::move(markers); 141 | } 142 | 143 | visualization_msgs::MarkerArray 144 | convertToDottedLineMarker(const std::vector& path, 145 | const std::string& frame_id, 146 | const std::string& ns, 147 | const std::size_t& start_id = 0, 148 | const std::array& rgba_line = { 1.0, 1.0, 0.2, 1.0 }, 149 | const std::array& rgba_point = { 0.1, .8, 0.2, 1.0 }, 150 | const std::array& offset = { 0, 0, 0, 0, 0, 0 }, 151 | const float& line_width = 0.001, 152 | const float& point_size = 0.0015) 153 | { 154 | visualization_msgs::MarkerArray markers_msgs; 155 | visualization_msgs::Marker line_marker, points_marker; 156 | 157 | // configure line marker 158 | line_marker.action = line_marker.ADD; 159 | std::tie(line_marker.color.r, line_marker.color.g, line_marker.color.b, line_marker.color.a) = 160 | std::make_tuple(rgba_line[0], rgba_line[1], rgba_line[2], rgba_line[3]); 161 | line_marker.header.frame_id = frame_id; 162 | line_marker.type = line_marker.LINE_STRIP; 163 | line_marker.id = start_id; 164 | line_marker.lifetime = ros::Duration(0); 165 | line_marker.ns = ns; 166 | std::tie(line_marker.scale.x, line_marker.scale.y, line_marker.scale.z) = std::make_tuple(line_width, 0.0, 0.0); 167 | line_marker.pose = pose3DtoPoseMsg(offset); 168 | 169 | // configure point marker 170 | points_marker = line_marker; 171 | points_marker.type = points_marker.POINTS; 172 | points_marker.ns = ns; 173 | std::tie(points_marker.color.r, points_marker.color.g, points_marker.color.b, points_marker.color.a) = 174 | std::make_tuple(rgba_point[0], rgba_point[1], rgba_point[2], rgba_point[3]); 175 | std::tie(points_marker.scale.x, points_marker.scale.y, points_marker.scale.z) = 176 | std::make_tuple(point_size, point_size, point_size); 177 | 178 | int id_counter = start_id; 179 | for (auto& poses : path) 180 | { 181 | line_marker.points.clear(); 182 | points_marker.points.clear(); 183 | line_marker.points.reserve(poses.size()); 184 | points_marker.points.reserve(poses.size()); 185 | for (auto& pose : poses) 186 | { 187 | geometry_msgs::Point p; 188 | std::tie(p.x, p.y, p.z) = std::make_tuple(pose.translation().x(), pose.translation().y(), pose.translation().z()); 189 | line_marker.points.push_back(p); 190 | points_marker.points.push_back(p); 191 | } 192 | 193 | line_marker.id = (++id_counter); 194 | points_marker.id = (++id_counter); 195 | markers_msgs.markers.push_back(line_marker); 196 | markers_msgs.markers.push_back(points_marker); 197 | } 198 | 199 | return markers_msgs; 200 | } 201 | 202 | RegionDetector::DataBundleVec loadData() 203 | { 204 | using namespace XmlRpc; 205 | using namespace Eigen; 206 | 207 | namespace fs = boost::filesystem; 208 | RegionDetector::DataBundleVec data_vec; 209 | ros::NodeHandle ph("~"); 210 | bool success; 211 | std::string param_ns = "data"; 212 | 213 | // first data directory 214 | std::string data_dir; 215 | if (!ph.getParam("data_dir", data_dir) || !fs::exists(fs::path(data_dir))) 216 | { 217 | std::string err_msg = boost::str(boost::format("The data directory \"%s\" could not be found") % data_dir); 218 | throw std::runtime_error(err_msg); 219 | } 220 | fs::path data_dir_path(data_dir); 221 | 222 | XmlRpcValue data_entries; 223 | if (!ph.getParam(param_ns, data_entries)) 224 | { 225 | std::string err_msg = boost::str(boost::format("Failed to load data entries from \"%s\" parameter") % param_ns); 226 | throw std::runtime_error(err_msg); 227 | } 228 | 229 | if (data_entries.getType() != data_entries.TypeArray) 230 | { 231 | std::string err_msg = boost::str(boost::format("The \"%s\" parameter is not an array") % param_ns); 232 | throw std::runtime_error(err_msg); 233 | } 234 | 235 | for (int i = 0; i < data_entries.size(); i++) 236 | { 237 | RegionDetector::DataBundle bundle; 238 | XmlRpcValue entry = data_entries[i]; 239 | success = entry.hasMember("image_file") && entry.hasMember("cloud_file") && entry.hasMember("transform"); 240 | if (!success) 241 | { 242 | std::string err_msg = 243 | boost::str(boost::format("One or more fields not found in entry of \"%s\" parameter") % param_ns); 244 | throw std::runtime_error(err_msg); 245 | } 246 | 247 | fs::path image_file_path = data_dir_path / fs::path(static_cast(entry["image_file"])); 248 | fs::path cloud_file_path = data_dir_path / fs::path(static_cast(entry["cloud_file"])); 249 | std::vector file_paths = { image_file_path, cloud_file_path }; 250 | if (!std::all_of(file_paths.begin(), file_paths.end(), [](const fs::path& p) { 251 | if (!fs::exists(p)) 252 | { 253 | std::string err_msg = boost::str(boost::format("The file \"%s\" does not exists") % p.string()); 254 | ROS_ERROR_STREAM(err_msg); 255 | return false; 256 | } 257 | return true; 258 | })) 259 | { 260 | throw std::runtime_error("File not found"); 261 | } 262 | 263 | // load files now 264 | bundle.image = cv::imread(image_file_path.string(), cv::IMREAD_COLOR); 265 | pcl::io::loadPCDFile(cloud_file_path.string(), bundle.cloud_blob); 266 | std::vector transform_vals; 267 | XmlRpcValue transform_entry = entry["transform"]; 268 | for (int j = 0; j < transform_entry.size(); j++) 269 | { 270 | transform_vals.push_back(static_cast(transform_entry[j])); 271 | } 272 | bundle.transform = Translation3d(Vector3d(transform_vals[0], transform_vals[1], transform_vals[2])) * 273 | AngleAxisd(transform_vals[3], Vector3d::UnitX()) * 274 | AngleAxisd(transform_vals[4], Vector3d::UnitY()) * 275 | AngleAxisd(transform_vals[5], Vector3d::UnitZ()); 276 | 277 | data_vec.push_back(std::move(bundle)); 278 | } 279 | 280 | return data_vec; 281 | } 282 | 283 | int main(int argc, char** argv) 284 | { 285 | using PointType = pcl::PointXYZRGB; 286 | 287 | ros::init(argc, argv, "detect_regions_demo"); 288 | ros::NodeHandle nh, ph("~"); 289 | ros::AsyncSpinner spinner(2); 290 | spinner.start(); 291 | 292 | // load configuration 293 | std::string yaml_config_file; 294 | const std::string yaml_cfg_param = "region_detection_cfg_file"; 295 | if (!ph.getParam(yaml_cfg_param, yaml_config_file)) 296 | { 297 | ROS_ERROR("Failed to load \"%s\" parameter", yaml_cfg_param.c_str()); 298 | return false; 299 | } 300 | 301 | // getting configuration parameters 302 | RegionDetectionConfig cfg = RegionDetectionConfig::loadFromFile(yaml_config_file); 303 | 304 | ROS_INFO("Loaded configuration parameters"); 305 | 306 | // loading data location parameters 307 | /** 308 | * data: 309 | * - image_file: dir1/color.png 310 | * cloud_file: dir1/cloud.pcd 311 | * transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [px, py, pz, rx, ry, rz] 312 | * - image_file: dir2/color.png 313 | * cloud_file: dir2/cloud.pcd 314 | * transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [px, py, pz, rx, ry, rz] 315 | */ 316 | RegionDetector::DataBundleVec data_vec = loadData(); 317 | 318 | // computing regions now 319 | RegionDetector rd(cfg, RegionDetector::createDefaultDebugLogger("RD_Debug")); 320 | RegionDetector::RegionResults results; 321 | if (!rd.compute(data_vec, results)) 322 | { 323 | ROS_ERROR("Failed to compute regions"); 324 | } 325 | 326 | // cropping 327 | pcl::PointCloud cropped_clouds, cropped_cloud_reverse; 328 | cropped_clouds.header.frame_id = REFERENCE_FRAME_ID; 329 | cropped_cloud_reverse.header.frame_id = REFERENCE_FRAME_ID; 330 | RegionCrop crop; 331 | for (std::size_t i = 0; i < data_vec.size(); i++) 332 | { 333 | pcl::PointCloud::Ptr temp_cloud = boost::make_shared>(); 334 | pcl::fromPCLPointCloud2(data_vec[i].cloud_blob, *temp_cloud); 335 | RegionCropConfig crop_config; 336 | crop_config.view_point = Eigen::Vector3d::Zero(); 337 | crop.setConfig(crop_config); 338 | crop.setInput(temp_cloud); 339 | 340 | for (std::size_t j = 0; j < results.closed_regions_poses.size(); j++) 341 | { 342 | pcl::PointCloud::Ptr cropped_cloud = boost::make_shared>(); 343 | crop.setRegion(results.closed_regions_poses[j]); 344 | std::vector indices = crop.filter(); 345 | 346 | if (!indices.empty()) 347 | { 348 | pcl::copyPointCloud(*temp_cloud, indices, *cropped_cloud); 349 | cropped_clouds += (*cropped_cloud); 350 | } 351 | 352 | cropped_cloud->clear(); 353 | indices = crop.filter(true); 354 | if (!indices.empty()) 355 | { 356 | pcl::copyPointCloud(*temp_cloud, indices, *cropped_cloud); 357 | cropped_cloud_reverse += (*cropped_cloud); 358 | } 359 | } 360 | } 361 | 362 | // creating publishers 363 | ros::Publisher results_markers_pub = nh.advertise(REGIONS_MARKERS_TOPIC, 1); 364 | ros::Publisher input_cloud_pub = nh.advertise>(INPUT_CLOUD_TOPIC, 1); 365 | ros::Publisher cropped_cloud_pub = nh.advertise>(CROPPED_CLOUDS_TOPIC, 1); 366 | ros::Publisher cropped_cloud_reversed_pub = nh.advertise>(CROPPED_CLOUDS_REVERSE_TOPIC, 1); 367 | 368 | // create markers to publish 369 | visualization_msgs::MarkerArray results_markers; 370 | int id = 0; 371 | for (auto& poses : results.closed_regions_poses) 372 | { 373 | id++; 374 | visualization_msgs::MarkerArray m = convertToAxisMarkers({ poses }, 375 | REFERENCE_FRAME_ID, 376 | "closed_regions_axes" + std::to_string(id), 377 | 0, 378 | 0.001, 379 | 0.01, 380 | { 0, 0, 0, 0, 0, 0 }); 381 | results_markers.markers.insert(results_markers.markers.end(), m.markers.begin(), m.markers.end()); 382 | 383 | m = convertToDottedLineMarker({ poses }, REFERENCE_FRAME_ID, "closed_regions_lines" + std::to_string(id)); 384 | results_markers.markers.insert(results_markers.markers.end(), m.markers.begin(), m.markers.end()); 385 | } 386 | 387 | id = 0; 388 | for (auto& poses : results.open_regions_poses) 389 | { 390 | id++; 391 | visualization_msgs::MarkerArray m = convertToAxisMarkers( 392 | { poses }, REFERENCE_FRAME_ID, "open_regions_axes" + std::to_string(id), 0, 0.001, 0.01, { 0, 0, 0, 0, 0, 0 }); 393 | results_markers.markers.insert(results_markers.markers.end(), m.markers.begin(), m.markers.end()); 394 | 395 | m = convertToDottedLineMarker({ poses }, 396 | REFERENCE_FRAME_ID, 397 | "open_regions_lines" + std::to_string(id), 398 | 0, 399 | { 1.0, 0.2, 0.2, 1.0 }, 400 | { 0.2, 0.2, 0.2, 1.0 }); 401 | results_markers.markers.insert(results_markers.markers.end(), m.markers.begin(), m.markers.end()); 402 | } 403 | 404 | // creating point cloud to publish 405 | pcl::PointCloud input_cloud; 406 | for (auto& data : data_vec) 407 | { 408 | pcl::PointCloud::Ptr color_cloud = boost::make_shared>(); 409 | pcl::fromPCLPointCloud2(data.cloud_blob, *color_cloud); 410 | input_cloud += (*color_cloud); 411 | } 412 | input_cloud.header.frame_id = REFERENCE_FRAME_ID; 413 | 414 | ros::Duration loop_pause(0.2); 415 | while (ros::ok()) 416 | { 417 | results_markers_pub.publish(results_markers); 418 | input_cloud_pub.publish(input_cloud); 419 | cropped_cloud_pub.publish(cropped_clouds); 420 | cropped_cloud_reversed_pub.publish(cropped_cloud_reverse); 421 | loop_pause.sleep(); 422 | } 423 | 424 | ros::waitForShutdown(); 425 | return 0; 426 | } 427 | --------------------------------------------------------------------------------