├── tree_detection_ros ├── data │ └── .gitkeep ├── package.xml ├── include │ └── tree_detection_ros │ │ ├── creators.hpp │ │ ├── TreeDetectorRos.hpp │ │ └── helpers.hpp ├── launch │ └── tree_detection.launch ├── config │ └── tree_detection.yaml ├── CMakeLists.txt ├── src │ ├── creators.cpp │ ├── TreeDetectorRos.cpp │ ├── tree_detection_node.cpp │ └── helpers.cpp └── rviz │ └── default.rviz ├── .gitignore ├── doc ├── forest2.jpg ├── forest3.jpg └── forest4.jpg ├── tree_detection ├── package.xml ├── include │ └── tree_detection │ │ ├── typedefs.hpp │ │ ├── common.hpp │ │ ├── Parameters.hpp │ │ └── TreeDetector.hpp ├── CMakeLists.txt └── src │ ├── Parameters.cpp │ └── TreeDetector.cpp ├── point_cloud_preprocessing ├── package.xml ├── include │ └── point_cloud_preprocessing │ │ ├── typedefs.hpp │ │ ├── Parameters.hpp │ │ └── PointCloudPreprocessor.hpp ├── CMakeLists.txt └── src │ ├── PointCloudPreprocessor.cpp │ └── Parameters.cpp ├── ground_plane_removal ├── package.xml ├── include │ └── ground_plane_removal │ │ ├── typedefs.hpp │ │ ├── Parameters.hpp │ │ ├── GroundPlaneRemover.hpp │ │ └── ElevationMapGroundPlaneRemover.hpp ├── src │ ├── GroundPlaneRemover.cpp │ ├── Parameters.cpp │ └── ElevationMapGroundPlaneRemover.cpp └── CMakeLists.txt └── README.md /tree_detection_ros/data/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | tree_detection_ros/data/ 2 | !tree_detection_ros/data/.gitkeep 3 | -------------------------------------------------------------------------------- /doc/forest2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/tree_detection/HEAD/doc/forest2.jpg -------------------------------------------------------------------------------- /doc/forest3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/tree_detection/HEAD/doc/forest3.jpg -------------------------------------------------------------------------------- /doc/forest4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/tree_detection/HEAD/doc/forest4.jpg -------------------------------------------------------------------------------- /tree_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | tree_detection 6 | 0.5.0 7 | Tree detector from point cloud maps 8 | 9 | BSD 10 | 11 | EJ 12 | EJr 13 | 14 | 15 | catkin 16 | pcl 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | point_cloud_preprocessing 6 | 0.5.0 7 | Point Cloud Preprocessing 8 | 9 | BSD 10 | 11 | TK 12 | EJr 13 | 14 | 15 | catkin 16 | pcl 17 | 18 | 19 | -------------------------------------------------------------------------------- /ground_plane_removal/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | ground_plane_removal 6 | 0.5.0 7 | Tree detector from point cloud maps 8 | 9 | BSD 10 | 11 | EJ 12 | EJr 13 | 14 | 15 | catkin 16 | pcl 17 | grid_map_pcl 18 | 19 | 20 | -------------------------------------------------------------------------------- /tree_detection/include/tree_detection/typedefs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * typedefs.hpp 3 | * 4 | * Created on: Aug 18, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include 12 | 13 | namespace tree_detection { 14 | 15 | using PointCloud = pcl::PointCloud; 16 | using PointIndices = std::vector; 17 | using PointCloudPtrVector = std::vector; 18 | 19 | 20 | } // tree detection 21 | -------------------------------------------------------------------------------- /ground_plane_removal/include/ground_plane_removal/typedefs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * typedefs.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include 12 | 13 | namespace ground_removal { 14 | 15 | using PointCloud = pcl::PointCloud; 16 | using PointIndices = std::vector; 17 | using PointCloudPtrVector = std::vector; 18 | 19 | 20 | } // tree ground_removal 21 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/include/point_cloud_preprocessing/typedefs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * typedefs.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include 12 | 13 | namespace point_cloud_preprocessing { 14 | 15 | using PointCloud = pcl::PointCloud; 16 | using PointIndices = std::vector; 17 | using PointCloudPtrVector = std::vector; 18 | 19 | 20 | } // tree point_cloud_preprocessing 21 | -------------------------------------------------------------------------------- /tree_detection/include/tree_detection/common.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * common.hpp 3 | * 4 | * Created on: Aug 18, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | 11 | namespace tree_detection { 12 | 13 | struct ClusterDimensions 14 | { 15 | double dimX_; 16 | double dimY_; 17 | double dimZ_; 18 | }; 19 | 20 | struct Cluster 21 | { 22 | Eigen::Vector3d position_; 23 | ClusterDimensions clusterDimensions_; 24 | }; 25 | 26 | struct RPY 27 | { 28 | double roll_; 29 | double pitch_; 30 | double yaw_; 31 | }; 32 | 33 | 34 | } // tree detection 35 | -------------------------------------------------------------------------------- /tree_detection_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | tree_detection_ros 6 | 0.5.0 7 | Tree detector from point cloud maps 8 | 9 | BSD 10 | 11 | EJ 12 | EJr 13 | 14 | 15 | catkin 16 | tree_detection 17 | ground_plane_removal 18 | point_cloud_preprocessing 19 | pcl_conversions 20 | sensor_msgs 21 | visualization_msgs 22 | roscpp 23 | roslib 24 | jsk_recognition_msgs 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /tree_detection_ros/include/tree_detection_ros/creators.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * creators.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | #pragma once 8 | 9 | #include "ground_plane_removal/GroundPlaneRemover.hpp" 10 | #include "ground_plane_removal/ElevationMapGroundPlaneRemover.hpp" 11 | #include "ground_plane_removal/Parameters.hpp" 12 | 13 | 14 | namespace ground_removal { 15 | 16 | void loadParameters(const std::string &filename, ground_removal::GroundPlaneRemoverParam *p); 17 | void loadParameters(const std::string &filename, ground_removal::ElevationMapGroundPlaneRemoverParam *p); 18 | std::unique_ptr groundRemoverFactory(const std::string &strategy, 19 | const std::string &configFile, ros::NodeHandlePtr nh); 20 | 21 | } // namespace ground_removal 22 | -------------------------------------------------------------------------------- /tree_detection_ros/include/tree_detection_ros/TreeDetectorRos.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "tree_detection/TreeDetector.hpp" 5 | #include "sensor_msgs/PointCloud2.h" 6 | 7 | namespace tree_detection { 8 | 9 | class TreeDetectorRos: public TreeDetector { 10 | using BASE = TreeDetector; 11 | public: 12 | explicit TreeDetectorRos(ros::NodeHandlePtr nh); 13 | 14 | void initRos(std::string filename); 15 | void detectTrees() override; 16 | void setInputPointCloudFromRos(const sensor_msgs::PointCloud2 &cloud); 17 | void publishTreeCylinderMarkers(const ClusterVector &clusters) const; 18 | void publishTreeBoundingBoxes(const ClusterVector &clusters) const; 19 | void setCloudFrameId(const std::string &frameId); 20 | void setCloudTimestamp(const ros::Time &stamp); 21 | private: 22 | 23 | void inflateClusterDimensions(ClusterDimensions *dim) const; 24 | 25 | ros::Publisher treeBoundingBoxPubliser_; 26 | ros::Publisher treeCylinderPublisher_; 27 | ros::NodeHandlePtr nh_; 28 | std_msgs::Header lastCloudInfo_; 29 | 30 | }; 31 | 32 | } // namespace tree_detection 33 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/include/point_cloud_preprocessing/Parameters.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | 12 | namespace point_cloud_preprocessing { 13 | 14 | 15 | struct CropBoxParameters 16 | { 17 | double minX_ = -50.0; 18 | double maxX_= 50.0; 19 | double minY_= -50.0; 20 | double maxY_= 50.0; 21 | double minZ_= -10.0; 22 | double maxZ_= 10.0; 23 | }; 24 | 25 | struct VoxelGridParameters 26 | { 27 | double leafSizeX_ = 0.02; 28 | double leafSizeY_ = 0.02; 29 | double leafSizeZ_ = 0.02; 30 | }; 31 | 32 | struct PointCloudPreprocessorParam { 33 | bool isUseCropBox_ = true; 34 | CropBoxParameters cropBox_; 35 | bool isUseVoxelGrid_ = true; 36 | VoxelGridParameters voxelGrid_; 37 | }; 38 | 39 | std::ostream& operator<<(std::ostream& out, const PointCloudPreprocessorParam& p); 40 | void loadParameters(const YAML::Node &node, PointCloudPreprocessorParam *p); 41 | void loadParameters(const std::string &filename, PointCloudPreprocessorParam *p); 42 | 43 | } // namespace point_cloud_preprocessing 44 | -------------------------------------------------------------------------------- /tree_detection_ros/launch/tree_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /ground_plane_removal/include/ground_plane_removal/Parameters.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include "grid_map_pcl/PclLoaderParameters.hpp" 12 | 13 | namespace ground_removal { 14 | 15 | 16 | struct GroundPlaneCropBoxParameters 17 | { 18 | double minX_ = -50.0; 19 | double maxX_= 50.0; 20 | double minY_= -50.0; 21 | double maxY_= 50.0; 22 | double minZ_= -10.0; 23 | double maxZ_= 10.0; 24 | 25 | }; 26 | 27 | 28 | struct GroundPlaneRemoverParam { 29 | GroundPlaneCropBoxParameters cropBox_; 30 | }; 31 | 32 | struct ElevationMapGroundPlaneRemoverParam : public GroundPlaneRemoverParam { 33 | grid_map::grid_map_pcl::PclLoaderParameters pclConverter_; 34 | double minHeightAboveGround_ = 0.1; 35 | double maxHeightAboveGround_ = 8.0; 36 | double medianFilteringRadius_ = 2.0; 37 | int medianFilterDownsampleFactor_ = 1; 38 | bool isUseMedianFiltering_ = true; 39 | }; 40 | 41 | std::ostream& operator<<(std::ostream& out, const GroundPlaneCropBoxParameters& p); 42 | void loadParameters(const YAML::Node &node, GroundPlaneCropBoxParameters *p); 43 | 44 | } // namespace ground_removal 45 | -------------------------------------------------------------------------------- /ground_plane_removal/include/ground_plane_removal/GroundPlaneRemover.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GroundPlaneRemover.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "ground_plane_removal/Parameters.hpp" 10 | #include "ground_plane_removal/typedefs.hpp" 11 | 12 | 13 | 14 | namespace ground_removal{ 15 | 16 | 17 | class GroundPlaneRemover { 18 | public: 19 | GroundPlaneRemover() = default; 20 | virtual ~GroundPlaneRemover() = default; 21 | 22 | 23 | virtual void setParameters(const GroundPlaneRemoverParam &p); 24 | virtual const GroundPlaneRemoverParam &getParameters() const; 25 | void setInputCloudPtr(PointCloud::ConstPtr inputCloud); 26 | void setInputCloud(const PointCloud &inputCloud); 27 | virtual void removeGroundPlane(); 28 | PointCloud::ConstPtr getCloudWithoutGroundPlanePtr() const; 29 | const PointCloud &getCloudWithoutGroundPlane() const; 30 | 31 | protected: 32 | PointCloud::Ptr applyCropBox(PointCloud::ConstPtr input, const GroundPlaneCropBoxParameters &p) const; 33 | PointCloud::Ptr inputCloud_; 34 | PointCloud::Ptr noGroundPlaneCloud_; 35 | private: 36 | 37 | GroundPlaneRemoverParam params_; 38 | 39 | 40 | 41 | 42 | }; 43 | 44 | 45 | } // namespace ground_removal 46 | -------------------------------------------------------------------------------- /ground_plane_removal/include/ground_plane_removal/ElevationMapGroundPlaneRemover.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * elevationMapGroundPlaneRemover.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "ground_plane_removal/Parameters.hpp" 10 | #include "ground_plane_removal/typedefs.hpp" 11 | #include "ground_plane_removal/GroundPlaneRemover.hpp" 12 | #include "grid_map_pcl/GridMapPclLoader.hpp" 13 | 14 | 15 | namespace ground_removal{ 16 | 17 | 18 | class ElevationMapGroundPlaneRemover : public GroundPlaneRemover { 19 | public: 20 | ElevationMapGroundPlaneRemover() = default; 21 | explicit ElevationMapGroundPlaneRemover(ros::NodeHandle *nh); 22 | virtual ~ElevationMapGroundPlaneRemover() = default; 23 | 24 | void removeGroundPlane() override; 25 | void setParameters(const GroundPlaneRemoverParam &p); 26 | const ElevationMapGroundPlaneRemoverParam &getParameters() const; 27 | const grid_map::GridMap &getElevationMap() const; 28 | 29 | private: 30 | 31 | void snapToMapLimits(const grid_map::GridMap &map, double *x, double *y) const; 32 | 33 | ElevationMapGroundPlaneRemoverParam param_; 34 | grid_map::GridMapPclLoader pclToGridMap_; 35 | grid_map::GridMap elevationMap_; 36 | ros::NodeHandle *nh_ = nullptr; 37 | 38 | }; 39 | 40 | 41 | } // namespace ground_removal 42 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/include/point_cloud_preprocessing/PointCloudPreprocessor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * elevationMapGroundPlaneRemover.hpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include "point_cloud_preprocessing/Parameters.hpp" 10 | #include "point_cloud_preprocessing/typedefs.hpp" 11 | 12 | 13 | namespace point_cloud_preprocessing{ 14 | 15 | 16 | class PointCloudPreprocessor { 17 | public: 18 | PointCloudPreprocessor() = default; 19 | virtual ~PointCloudPreprocessor() = default; 20 | 21 | void setParameters(const PointCloudPreprocessorParam &p); 22 | const PointCloudPreprocessorParam &getParameters() const; 23 | 24 | void setInputCloudPtr(PointCloud::ConstPtr inputCloud); 25 | void setInputCloud(const PointCloud &inputCloud); 26 | 27 | PointCloud::ConstPtr getPreprocessedCloudPtr() const; 28 | const PointCloud &getPreprocessedCloud() const; 29 | 30 | void performPreprocessing(); 31 | 32 | private: 33 | PointCloud::Ptr applyCropBox(PointCloud::ConstPtr input, const CropBoxParameters &p) const; 34 | PointCloud::Ptr applyVoxelGrid(PointCloud::ConstPtr input, const VoxelGridParameters &p) const; 35 | PointCloud::Ptr inputCloud_; 36 | PointCloud::Ptr preprocessedCloud_; 37 | PointCloudPreprocessorParam params_; 38 | 39 | }; 40 | 41 | 42 | } // namespace point_cloud_preprocessing 43 | -------------------------------------------------------------------------------- /tree_detection/include/tree_detection/Parameters.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.hpp 3 | * 4 | * Created on: Aug 18, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | 12 | namespace tree_detection { 13 | 14 | struct TreeDetectionParameters { 15 | double maxDiameterTree_ = 0.6; 16 | double minHeightTree_ = 0.5; 17 | double clusterTolerance_ = 0.3; 18 | int minNumPointsTree_ = 10; 19 | int maxNumPointsTree_ = 1000; 20 | double minEigenVectorAlignment_ = 0.8; 21 | double treeMarkerRadiusInflateFactor_ = 1.0; 22 | double treeMarkerHeightInflateFactor_ = 1.0; 23 | double treeMarkerOpacity_ = 1.0; 24 | bool isPrintDiscardedTreeInstances_ = false; 25 | bool isPrintTiming_ = false; 26 | }; 27 | 28 | struct CloudCroppingParameters { 29 | double cropBoxMinX_ = -50.0; 30 | double cropBoxMaxX_ = 50.0; 31 | double cropBoxMinY_ = -50.0; 32 | double cropBoxMaxY_ = 50.0; 33 | double cropBoxMinZ_ = -10.0; 34 | double cropBoxMaxZ_ = 20.0; 35 | }; 36 | 37 | void loadParameters(const YAML::Node &node, TreeDetectionParameters *p); 38 | void loadParameters(const std::string &filename, TreeDetectionParameters *p); 39 | void loadParameters(const YAML::Node &node, CloudCroppingParameters *p); 40 | void loadParameters(const std::string &filename, CloudCroppingParameters *p); 41 | 42 | } // namespace tree_detection 43 | -------------------------------------------------------------------------------- /ground_plane_removal/src/GroundPlaneRemover.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GroundPlaneRemover.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include "ground_plane_removal/GroundPlaneRemover.hpp" 9 | #include 10 | 11 | 12 | namespace ground_removal { 13 | 14 | void GroundPlaneRemover::setParameters(const GroundPlaneRemoverParam &p) { 15 | params_ = p; 16 | } 17 | const GroundPlaneRemoverParam& GroundPlaneRemover::getParameters() const { 18 | return params_; 19 | } 20 | void GroundPlaneRemover::setInputCloudPtr(PointCloud::ConstPtr inputCloud) { 21 | setInputCloud(*inputCloud); 22 | } 23 | 24 | void GroundPlaneRemover::setInputCloud(const PointCloud &inputCloud) { 25 | inputCloud_.reset(new PointCloud); 26 | *inputCloud_ = inputCloud; //copy 27 | } 28 | void GroundPlaneRemover::removeGroundPlane() { 29 | noGroundPlaneCloud_ = applyCropBox(inputCloud_, params_.cropBox_); 30 | } 31 | 32 | PointCloud::Ptr GroundPlaneRemover::applyCropBox(PointCloud::ConstPtr input, const GroundPlaneCropBoxParameters &p) const { 33 | pcl::CropBox boxFilter; 34 | boxFilter.setMin(Eigen::Vector4f(p.minX_, p.minY_, p.minZ_, 1.0)); 35 | boxFilter.setMax(Eigen::Vector4f(p.maxX_, p.maxY_, p.maxZ_, 1.0)); 36 | boxFilter.setInputCloud(input); 37 | PointCloud::Ptr outputCloud(new PointCloud()); 38 | boxFilter.filter(*outputCloud); 39 | return outputCloud; 40 | } 41 | 42 | PointCloud::ConstPtr GroundPlaneRemover::getCloudWithoutGroundPlanePtr() const { 43 | return noGroundPlaneCloud_; 44 | } 45 | const PointCloud& GroundPlaneRemover::getCloudWithoutGroundPlane() const { 46 | return *noGroundPlaneCloud_; 47 | } 48 | 49 | } // namespace ground_removal 50 | 51 | -------------------------------------------------------------------------------- /tree_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(tree_detection) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | 7 | 8 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 9 | 10 | 11 | 12 | set(SRC_FILES 13 | src/TreeDetector.cpp 14 | src/Parameters.cpp 15 | # src/common.cpp 16 | ) 17 | 18 | find_package(catkin REQUIRED 19 | ) 20 | 21 | 22 | find_package(PCL 1.10 REQUIRED) 23 | 24 | ################################### 25 | ## catkin specific configuration ## 26 | ################################### 27 | catkin_package( 28 | INCLUDE_DIRS 29 | include 30 | LIBRARIES 31 | ${PROJECT_NAME} 32 | yaml-cpp 33 | CATKIN_DEPENDS 34 | DEPENDS 35 | PCL 36 | ) 37 | 38 | 39 | ########### 40 | ## Build ## 41 | ########### 42 | 43 | include_directories( 44 | include 45 | SYSTEM 46 | ${PCL_INCLUDE_DIRS} 47 | ${catkin_INCLUDE_DIRS} 48 | ) 49 | 50 | link_directories( #only needed for pcl 51 | ${PCL_LIBRARY_DIRS} 52 | ) 53 | 54 | add_definitions( #only needed for pcl 55 | ${PCL_DEFINITIONS} 56 | ) 57 | 58 | add_library(${PROJECT_NAME} 59 | ${SRC_FILES} 60 | ) 61 | 62 | target_link_libraries(${PROJECT_NAME} 63 | ${catkin_LIBRARIES} 64 | ${PCL_COMMON_LIBRARIES} 65 | ${PCL_IO_LIBRARIES} 66 | ) 67 | 68 | ############# 69 | ## Install ## 70 | ############# 71 | #install(DIRECTORY include/${PROJECT_NAME}/ 72 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 73 | # PATTERN ".svn" EXCLUDE 74 | #) 75 | #install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node point_cloud_stitcher_node 76 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 78 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 79 | #) 80 | # 81 | #install(DIRECTORY 82 | # launch 83 | # param 84 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 85 | #) 86 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(point_cloud_preprocessing) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | 7 | 8 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 9 | 10 | 11 | 12 | set(SRC_FILES 13 | src/PointCloudPreprocessor.cpp 14 | src/Parameters.cpp 15 | ) 16 | 17 | find_package(catkin REQUIRED 18 | COMPONENTS 19 | ${CATKIN_REQUIRED_COMPONENTS} 20 | ) 21 | 22 | 23 | find_package(PCL 1.10 REQUIRED) 24 | 25 | ################################### 26 | ## catkin specific configuration ## 27 | ################################### 28 | catkin_package( 29 | INCLUDE_DIRS 30 | include 31 | LIBRARIES 32 | ${PROJECT_NAME} 33 | yaml-cpp 34 | DEPENDS 35 | PCL 36 | ) 37 | 38 | 39 | ########### 40 | ## Build ## 41 | ########### 42 | 43 | include_directories( 44 | include 45 | SYSTEM 46 | ${PCL_INCLUDE_DIRS} 47 | ${catkin_INCLUDE_DIRS} 48 | ) 49 | 50 | link_directories( #only needed for pcl 51 | ${PCL_LIBRARY_DIRS} 52 | ) 53 | 54 | add_definitions( #only needed for pcl 55 | ${PCL_DEFINITIONS} 56 | ) 57 | 58 | add_library(${PROJECT_NAME} 59 | ${SRC_FILES} 60 | ) 61 | 62 | target_link_libraries(${PROJECT_NAME} 63 | ${catkin_LIBRARIES} 64 | ${PCL_COMMON_LIBRARIES} 65 | ${PCL_IO_LIBRARIES} 66 | ) 67 | 68 | ############# 69 | ## Install ## 70 | ############# 71 | #install(DIRECTORY include/${PROJECT_NAME}/ 72 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 73 | # PATTERN ".svn" EXCLUDE 74 | #) 75 | #install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node point_cloud_stitcher_node 76 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 78 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 79 | #) 80 | # 81 | #install(DIRECTORY 82 | # launch 83 | # param 84 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 85 | #) 86 | -------------------------------------------------------------------------------- /tree_detection/include/tree_detection/TreeDetector.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // pcl 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "tree_detection/common.hpp" 10 | #include "tree_detection/typedefs.hpp" 11 | #include "tree_detection/Parameters.hpp" 12 | 13 | namespace tree_detection { 14 | class TreeDetector { 15 | public: 16 | using ClusterVector = std::vector; 17 | 18 | TreeDetector() = default; 19 | virtual ~TreeDetector() = default; 20 | 21 | void setTreeDetectionParameters(const TreeDetectionParameters &p); 22 | void setInputPointCloud(const PointCloud &inputCloud); 23 | void setInputPointCloudPtr(PointCloud::Ptr inputCloud); 24 | virtual void detectTrees(); 25 | const ClusterVector& getDetectedTreeClusters() const; 26 | const TreeDetectionParameters &getParameters() const; 27 | PointIndices extractClusterIndices(const PointCloud::ConstPtr cloud) const; 28 | private: 29 | bool isGravityAlignmentStraightEnough(PointCloud::ConstPtr cloudCluster) const; 30 | double getClusterGravityAlignment(PointCloud::ConstPtr cloudCluster) const; 31 | bool isClusterTooLow(PointCloud::ConstPtr cloudCluster) const; 32 | bool isClusterRadiusTooBig(PointCloud::ConstPtr cloudCluster) const; 33 | PointCloudPtrVector extractValidPointClusters(const PointIndices &indices, PointCloud::ConstPtr cloud) const; 34 | PointCloud::Ptr extractClusterFromIndices(const std::vector &indices, PointCloud::ConstPtr input) const; 35 | ClusterDimensions getClusterDimensions(PointCloud::ConstPtr clusterCloud) const; 36 | ClusterVector extractClusterInformation(const PointCloudPtrVector &validClusters) const; 37 | Eigen::Vector3d getClusterMean(PointCloud::ConstPtr cloudCluster) const; 38 | 39 | TreeDetectionParameters treeDetectionParam_; 40 | PointCloud::Ptr inputCloud_; 41 | ClusterVector detectedTreeClusters_; 42 | 43 | }; 44 | 45 | } // namespace tree_detection 46 | -------------------------------------------------------------------------------- /tree_detection/src/Parameters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.cpp 3 | * 4 | * Created on: Aug 18, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include "tree_detection/Parameters.hpp" 9 | 10 | namespace tree_detection { 11 | 12 | void loadParameters(const YAML::Node &node, TreeDetectionParameters *p) { 13 | p->maxDiameterTree_ = node["max_diameter"].as(); 14 | p->minHeightTree_ = node["min_height"].as(); 15 | p->clusterTolerance_ = node["cluster_tolerance"].as(); 16 | p->minNumPointsTree_ = node["min_num_points"].as(); 17 | p->maxNumPointsTree_ = node["max_num_points"].as(); 18 | p->minEigenVectorAlignment_ = node["min_eigenvector_alignment"].as(); 19 | p->treeMarkerHeightInflateFactor_ = node["tree_marker_height_inflate_factor"].as(); 20 | p->treeMarkerRadiusInflateFactor_ = node["tree_marker_radius_inflate_factor"].as(); 21 | p->treeMarkerOpacity_ = node["tree_marker_opacity"].as(); 22 | p->isPrintDiscardedTreeInstances_ = node["is_print_discarded_clusters"].as(); 23 | p->isPrintTiming_ = node["is_print_timing"].as(); 24 | } 25 | void loadParameters(const std::string &filename, TreeDetectionParameters *p) { 26 | YAML::Node node = YAML::LoadFile(filename); 27 | 28 | if (node.IsNull()) { 29 | throw std::runtime_error("Tree cloud parameters loading failed"); 30 | } 31 | loadParameters(node["tree_detection"], p); 32 | } 33 | 34 | void loadParameters(const YAML::Node &node, CloudCroppingParameters *p) { 35 | p->cropBoxMinX_ = node["crop_box_minX"].as(); 36 | p->cropBoxMaxX_ = node["crop_box_maxX"].as(); 37 | p->cropBoxMinY_ = node["crop_box_minY"].as(); 38 | p->cropBoxMaxY_ = node["crop_box_maxY"].as(); 39 | p->cropBoxMinZ_ = node["crop_box_minZ"].as(); 40 | p->cropBoxMaxZ_ = node["crop_box_maxZ"].as(); 41 | } 42 | void loadParameters(const std::string &filename, CloudCroppingParameters *p) { 43 | YAML::Node node = YAML::LoadFile(filename); 44 | 45 | if (node.IsNull()) { 46 | throw std::runtime_error("Tree cloud parameters loading failed"); 47 | } 48 | loadParameters(node["cropping"], p); 49 | } 50 | 51 | } // namespace tree_detection 52 | -------------------------------------------------------------------------------- /tree_detection_ros/config/tree_detection.yaml: -------------------------------------------------------------------------------- 1 | point_cloud_preprocessing: 2 | is_use_crop_box: false 3 | crop_box: 4 | crop_box_minX: -100.0 5 | crop_box_maxX: 100.0 6 | crop_box_minY: -100.0 7 | crop_box_maxY: 100.0 8 | crop_box_minZ: 0.0 9 | crop_box_maxZ: 20.0 10 | is_use_voxel_grid: false 11 | voxel_grid: 12 | voxel_grid_leafSizeX: 0.02 13 | voxel_grid_leafSizeY: 0.02 14 | voxel_grid_leafSizeZ: 0.02 15 | ground_plane_removal: 16 | cropbox: 17 | crop_box_minX: -100.0 18 | crop_box_maxX: 100.0 19 | crop_box_minY: -100.0 20 | crop_box_maxY: 100.0 21 | crop_box_minZ: 0.0 22 | crop_box_maxZ: 20.0 23 | elevation_map: 24 | min_height_above_ground: 0.2 25 | max_height_above_ground: 3.5 26 | is_use_median_filter: true 27 | median_filtering_radius: 2.0 28 | median_filter_points_downsample_factor: 1 29 | num_processing_threads: 7 30 | cloud_transform: 31 | translation: 32 | x: 0.0 33 | y: 0.0 34 | z: 0.0 35 | rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence 36 | r: 0.0 37 | p: 0.0 38 | y: 0.0 39 | cluster_extraction: 40 | cluster_tolerance: 0.02 41 | min_num_points: 1 42 | max_num_points: 1000000 43 | use_max_height_as_cell_elevation: false 44 | outlier_removal: 45 | is_remove_outliers: false 46 | mean_K: 10 47 | stddev_threshold: 1.0 48 | downsampling: 49 | is_downsample_cloud: false 50 | voxel_size: 51 | x: 0.02 52 | y: 0.02 53 | z: 0.02 54 | grid_map: 55 | min_num_points_per_cell: 1 56 | max_num_points_per_cell: 100000 57 | resolution: 0.5 58 | 59 | 60 | 61 | 62 | # Tree specific parameters 63 | tree_detection: 64 | max_diameter: 4.0 65 | min_height: 0.8 66 | min_num_points: 200 67 | max_num_points: 5000000 68 | cluster_tolerance: 0.1 # [m] 69 | min_eigenvector_alignment: 0.7 # [0, 1] Alignment of principle axis along gravity. was 0.8 70 | tree_marker_radius_inflate_factor: 1.3 71 | tree_marker_height_inflate_factor: 1.3 72 | tree_marker_opacity: 1.0 73 | is_print_discarded_clusters: false 74 | is_print_timing: true 75 | 76 | 77 | 78 | 79 | tree_detection_ros: 80 | tree_bounding_boxes_topic_name: tree_bbs 81 | tree_cylinders_topic_name: tree_cylinders 82 | 83 | -------------------------------------------------------------------------------- /ground_plane_removal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ground_plane_removal) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | 7 | 8 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 9 | 10 | 11 | 12 | set(SRC_FILES 13 | src/GroundPlaneRemover.cpp 14 | src/Parameters.cpp 15 | src/ElevationMapGroundPlaneRemover.cpp 16 | ) 17 | set(CATKIN_REQUIRED_COMPONENTS 18 | grid_map_pcl 19 | ) 20 | 21 | find_package(catkin REQUIRED 22 | COMPONENTS 23 | ${CATKIN_REQUIRED_COMPONENTS} 24 | ) 25 | 26 | 27 | find_package(PCL 1.10 REQUIRED) 28 | 29 | find_package(OpenMP QUIET) 30 | if (OpenMP_FOUND) 31 | add_compile_options("${OpenMP_CXX_FLAGS}") 32 | add_definitions(-DGROUND_PLANE_REMOVAL_OPENMP_FOUND=${OpenMP_FOUND}) 33 | endif() 34 | 35 | ################################### 36 | ## catkin specific configuration ## 37 | ################################### 38 | catkin_package( 39 | INCLUDE_DIRS 40 | include 41 | LIBRARIES 42 | ${PROJECT_NAME} 43 | yaml-cpp 44 | ${OpenMP_CXX_LIBRARIES} 45 | CATKIN_DEPENDS 46 | ${CATKIN_REQUIRED_COMPONENTS} 47 | DEPENDS 48 | PCL 49 | ) 50 | 51 | 52 | ########### 53 | ## Build ## 54 | ########### 55 | 56 | include_directories( 57 | include 58 | SYSTEM 59 | ${PCL_INCLUDE_DIRS} 60 | ${catkin_INCLUDE_DIRS} 61 | ${OpenMP_CXX_INCLUDE_DIRS} 62 | ) 63 | 64 | link_directories( #only needed for pcl 65 | ${PCL_LIBRARY_DIRS} 66 | ) 67 | 68 | add_definitions( #only needed for pcl 69 | ${PCL_DEFINITIONS} 70 | ) 71 | 72 | add_library(${PROJECT_NAME} 73 | ${SRC_FILES} 74 | ) 75 | 76 | target_link_libraries(${PROJECT_NAME} 77 | ${catkin_LIBRARIES} 78 | ${PCL_COMMON_LIBRARIES} 79 | ${PCL_IO_LIBRARIES} 80 | ${OpenMP_CXX_LIBRARIES} 81 | ) 82 | 83 | ############# 84 | ## Install ## 85 | ############# 86 | #install(DIRECTORY include/${PROJECT_NAME}/ 87 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 88 | # PATTERN ".svn" EXCLUDE 89 | #) 90 | #install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node point_cloud_stitcher_node 91 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 94 | #) 95 | # 96 | #install(DIRECTORY 97 | # launch 98 | # param 99 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 100 | #) 101 | -------------------------------------------------------------------------------- /tree_detection_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(tree_detection_ros) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | 7 | 8 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 9 | 10 | set(CATKIN_PACKAGE_DEPENDENCIES 11 | tree_detection 12 | ground_plane_removal 13 | point_cloud_preprocessing 14 | pcl_conversions 15 | sensor_msgs 16 | visualization_msgs 17 | roscpp 18 | roslib 19 | jsk_recognition_msgs 20 | ) 21 | 22 | set (SRC_FILES 23 | src/TreeDetectorRos.cpp 24 | src/helpers.cpp 25 | src/creators.cpp 26 | ) 27 | 28 | 29 | find_package(catkin REQUIRED 30 | COMPONENTS 31 | ${CATKIN_PACKAGE_DEPENDENCIES} 32 | ) 33 | 34 | ################################### 35 | ## catkin specific configuration ## 36 | ################################### 37 | catkin_package( 38 | INCLUDE_DIRS 39 | include 40 | LIBRARIES 41 | ${PROJECT_NAME} 42 | yaml-cpp 43 | CATKIN_DEPENDS 44 | ${CATKIN_PACKAGE_DEPENDENCIES} 45 | DEPENDS 46 | PCL 47 | ) 48 | 49 | 50 | ########### 51 | ## Build ## 52 | ########### 53 | 54 | include_directories( 55 | include 56 | SYSTEM 57 | ${PCL_INCLUDE_DIRS} 58 | ${catkin_INCLUDE_DIRS} 59 | ) 60 | 61 | link_directories( #only needed for pcl 62 | ${PCL_LIBRARY_DIRS} 63 | ) 64 | 65 | add_definitions( #only needed for pcl 66 | ${PCL_DEFINITIONS} 67 | ) 68 | 69 | add_library(${PROJECT_NAME} 70 | ${SRC_FILES} 71 | ) 72 | 73 | target_link_libraries(${PROJECT_NAME} 74 | ${catkin_LIBRARIES} 75 | ${PCL_COMMON_LIBRARIES} 76 | ${PCL_IO_LIBRARIES} 77 | ) 78 | 79 | add_executable(tree_detection_node 80 | src/tree_detection_node.cpp 81 | ) 82 | 83 | target_link_libraries(tree_detection_node 84 | ${PROJECT_NAME} 85 | ${catkin_LIBRARIES} 86 | ) 87 | 88 | ############# 89 | ## Install ## 90 | ############# 91 | #install(DIRECTORY include/${PROJECT_NAME}/ 92 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 93 | # PATTERN ".svn" EXCLUDE 94 | #) 95 | #install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node point_cloud_stitcher_node 96 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 98 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 99 | #) 100 | # 101 | #install(DIRECTORY 102 | # launch 103 | # param 104 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | #) 106 | -------------------------------------------------------------------------------- /tree_detection_ros/src/creators.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * creators.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include "tree_detection_ros/creators.hpp" 9 | 10 | 11 | namespace ground_removal { 12 | 13 | void loadParameters(const std::string &filename, ground_removal::GroundPlaneRemoverParam *p) { 14 | 15 | YAML::Node node = YAML::LoadFile(filename); 16 | loadParameters(node["ground_plane_removal"]["cropbox"], &p->cropBox_); 17 | 18 | } 19 | 20 | void loadParameters(const std::string &filename, ground_removal::ElevationMapGroundPlaneRemoverParam *p) { 21 | 22 | YAML::Node node = YAML::LoadFile(filename); 23 | auto groundRemoval = node["ground_plane_removal"]["elevation_map"]; 24 | grid_map::grid_map_pcl::PclLoaderParameters pclLoaderParam; 25 | pclLoaderParam.loadParameters(groundRemoval); 26 | p->pclConverter_ = pclLoaderParam; 27 | 28 | p->medianFilteringRadius_ = groundRemoval["median_filtering_radius"].as(); 29 | p->medianFilterDownsampleFactor_ = groundRemoval["median_filter_points_downsample_factor"].as(); 30 | p->minHeightAboveGround_ = groundRemoval["min_height_above_ground"].as(); 31 | p->maxHeightAboveGround_ = groundRemoval["max_height_above_ground"].as(); 32 | p->isUseMedianFiltering_ = groundRemoval["is_use_median_filter"].as(); 33 | } 34 | 35 | 36 | 37 | std::unique_ptr groundRemoverFactory(const std::string &strategy, 38 | const std::string &configFile, ros::NodeHandlePtr nh) { 39 | 40 | std::unique_ptr ret; 41 | 42 | if (strategy == "cropbox") { 43 | ground_removal::GroundPlaneRemoverParam groundPlaneRemovalParam; 44 | loadParameters(configFile, &groundPlaneRemovalParam); 45 | auto groundRemover = std::make_unique(); 46 | groundRemover->setParameters(groundPlaneRemovalParam); 47 | ret = std::move(groundRemover); 48 | } else if (strategy == "elevation_map") { 49 | ground_removal::ElevationMapGroundPlaneRemoverParam groundPlaneRemovalParam; 50 | loadParameters(configFile, &groundPlaneRemovalParam); 51 | auto groundRemover = std::make_unique(nh.get()); 52 | groundRemover->setParameters(groundPlaneRemovalParam); 53 | ret = std::move(groundRemover); 54 | } else { 55 | throw std::runtime_error("Unknown groun plane removal strategy!!!"); 56 | } 57 | 58 | return ret; 59 | } 60 | 61 | } // namespace ground_removal 62 | -------------------------------------------------------------------------------- /ground_plane_removal/src/Parameters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | #include "ground_plane_removal/Parameters.hpp" 8 | #include 9 | 10 | namespace ground_removal { 11 | 12 | void loadParameters(const YAML::Node &node, GroundPlaneCropBoxParameters *p) { 13 | p->minX_ = node["crop_box_minX"].as(); 14 | p->maxX_ = node["crop_box_maxX"].as(); 15 | p->minY_ = node["crop_box_minY"].as(); 16 | p->maxY_ = node["crop_box_maxY"].as(); 17 | p->minZ_ = node["crop_box_minZ"].as(); 18 | p->maxZ_ = node["crop_box_maxZ"].as(); 19 | } 20 | 21 | std::ostream& operator<<(std::ostream& out, const GroundPlaneCropBoxParameters& p) { 22 | out << "┌────────────────────────────────────────────────────┐\n"; 23 | out << "│ GroundPlaneCropBoxParameters │\n"; 24 | out << "├─────────────────────────────────┬──────────────────┤\n"; 25 | out << "│ " << std::left << std::setw(31) << " minX" 26 | << " │ " << std::right << std::setw(16) << std::fixed 27 | << std::setprecision(4) << p.minX_ << " │\n"; 28 | out << "├─────────────────────────────────┼──────────────────┤\n"; 29 | out << "│ " << std::left << std::setw(31) << "max X" 30 | << " │ " << std::right << std::setw(16) << std::fixed 31 | << std::setprecision(4) << p.minX_ << " │\n"; 32 | out << "├─────────────────────────────────┼──────────────────┤\n"; 33 | out << "│ " << std::left << std::setw(31) << "min Y" 34 | << " │ " << std::right << std::setw(16) << std::fixed 35 | << std::setprecision(4) << p.minY_ << " │\n"; 36 | out << "├─────────────────────────────────┼──────────────────┤\n"; 37 | out << "│ " << std::left << std::setw(31) << "max Y" 38 | << " │ " << std::right << std::setw(16) << std::fixed 39 | << std::setprecision(4) << p.maxY_ << " │\n"; 40 | out << "├─────────────────────────────────┼──────────────────┤\n"; 41 | out << "│ " << std::left << std::setw(31) << "min Z" 42 | << " │ " << std::right << std::setw(16) << std::fixed 43 | << std::setprecision(4) << p.minZ_ << " │\n"; 44 | out << "├─────────────────────────────────┼──────────────────┤\n"; 45 | out << "│ " << std::left << std::setw(31) << "max Z" 46 | << " │ " << std::right << std::setw(16) << std::fixed 47 | << std::setprecision(4) << p.maxZ_ << " │\n"; 48 | out << "├─────────────────────────────────┼──────────────────┤\n"; 49 | 50 | 51 | return out; 52 | } 53 | 54 | } // namespace ground_removal 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/src/PointCloudPreprocessor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ElevationMapGroundPlaneRemover.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include "point_cloud_preprocessing/PointCloudPreprocessor.hpp" 12 | 13 | namespace point_cloud_preprocessing { 14 | 15 | void PointCloudPreprocessor::setParameters(const PointCloudPreprocessorParam &p) { 16 | params_ = p; 17 | } 18 | const PointCloudPreprocessorParam& PointCloudPreprocessor::getParameters() const { 19 | return params_; 20 | } 21 | void PointCloudPreprocessor::setInputCloudPtr(PointCloud::ConstPtr inputCloud) { 22 | setInputCloud(*inputCloud); 23 | } 24 | 25 | void PointCloudPreprocessor::setInputCloud(const PointCloud &inputCloud) { 26 | inputCloud_.reset(new PointCloud); 27 | *inputCloud_ = inputCloud; //copy 28 | } 29 | void PointCloudPreprocessor::performPreprocessing() { 30 | preprocessedCloud_.reset(new PointCloud); 31 | *preprocessedCloud_ = *inputCloud_; 32 | if (params_.isUseCropBox_) { 33 | preprocessedCloud_ = applyCropBox(preprocessedCloud_, params_.cropBox_); 34 | } 35 | if (params_.isUseVoxelGrid_) { 36 | preprocessedCloud_ = applyVoxelGrid(preprocessedCloud_, params_.voxelGrid_); 37 | } 38 | } 39 | 40 | PointCloud::Ptr PointCloudPreprocessor::applyCropBox(PointCloud::ConstPtr input, const CropBoxParameters &p) const { 41 | pcl::CropBox boxFilter; 42 | boxFilter.setMin(Eigen::Vector4f(p.minX_, p.minY_, p.minZ_, 1.0)); 43 | boxFilter.setMax(Eigen::Vector4f(p.maxX_, p.maxY_, p.maxZ_, 1.0)); 44 | boxFilter.setInputCloud(input); 45 | PointCloud::Ptr outputCloud(new PointCloud()); 46 | boxFilter.filter(*outputCloud); 47 | return outputCloud; 48 | } 49 | 50 | PointCloud::Ptr PointCloudPreprocessor::applyVoxelGrid(PointCloud::ConstPtr input, const VoxelGridParameters &p) const { 51 | pcl::VoxelGrid vox; 52 | vox.setInputCloud(input); 53 | vox.setLeafSize(p.leafSizeX_, p.leafSizeY_, p.leafSizeZ_); 54 | PointCloud::Ptr outputCloud(new PointCloud()); 55 | vox.filter(*outputCloud); 56 | return outputCloud; 57 | } 58 | 59 | PointCloud::ConstPtr PointCloudPreprocessor::getPreprocessedCloudPtr() const { 60 | return preprocessedCloud_; 61 | } 62 | const PointCloud& PointCloudPreprocessor::getPreprocessedCloud() const { 63 | return *preprocessedCloud_; 64 | } 65 | 66 | } // namespace point_cloud_preprocessing 67 | -------------------------------------------------------------------------------- /tree_detection_ros/include/tree_detection_ros/helpers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * helpers.hpp 3 | * 4 | * Created on: Aug 18, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #pragma once 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "tree_detection/common.hpp" 14 | #include "tree_detection/typedefs.hpp" 15 | #include "sensor_msgs/PointCloud2.h" 16 | #include "ground_plane_removal/Parameters.hpp" 17 | #include 18 | 19 | namespace tree_detection { 20 | 21 | class Color: public std_msgs::ColorRGBA { 22 | public: 23 | Color(); 24 | Color(double red, double green, double blue); 25 | Color(double red, double green, double blue, double alpha); 26 | 27 | static const Color White() { 28 | return Color(1.0, 1.0, 1.0); 29 | } 30 | static const Color Black() { 31 | return Color(0.0, 0.0, 0.0); 32 | } 33 | static const Color Gray() { 34 | return Color(0.5, 0.5, 0.5); 35 | } 36 | static const Color Red() { 37 | return Color(1.0, 0.0, 0.0); 38 | } 39 | static const Color Green() { 40 | return Color(0.0, 1.0, 0.0); 41 | } 42 | static const Color Blue() { 43 | return Color(0.0, 0.0, 1.0); 44 | } 45 | static const Color Yellow() { 46 | return Color(1.0, 1.0, 0.0); 47 | } 48 | static const Color Orange() { 49 | return Color(1.0, 0.5, 0.0); 50 | } 51 | static const Color Purple() { 52 | return Color(0.5, 0.0, 1.0); 53 | } 54 | static const Color Chartreuse() { 55 | return Color(0.5, 1.0, 0.0); 56 | } 57 | static const Color Teal() { 58 | return Color(0.0, 1.0, 1.0); 59 | } 60 | static const Color Pink() { 61 | return Color(1.0, 0.0, 0.5); 62 | } 63 | static const Color Magenta() { 64 | return Color(0.78, 0.0, 0.9); 65 | } 66 | }; 67 | 68 | visualization_msgs::Marker getTreeCylinderMarker(const Eigen::Vector3d &treeCoordinates, 69 | const ClusterDimensions &clusterDimension, int markerId, const ros::Time ×tamp, const std::string &frameId, 70 | const Color &color); 71 | 72 | jsk_recognition_msgs::BoundingBox getTreeBBMarker(const Eigen::Vector3d &treeCoordinates, 73 | const ClusterDimensions &clusterDimension, int markerId, const ros::Time ×tamp, 74 | const std::string &frameId); 75 | 76 | PointCloud::Ptr fromRos(const sensor_msgs::PointCloud2 &cloud); 77 | sensor_msgs::PointCloud2 toRos(const PointCloud &cloud); 78 | PointCloud::Ptr loadPointcloudFromPcd(const std::string &filename); 79 | 80 | inline void printTimeElapsed(const std::chrono::steady_clock::time_point& start, const std::string& prefix) { 81 | const auto stop = std::chrono::steady_clock::now(); 82 | const auto duration = std::chrono::duration_cast(stop - start).count() / 1e6; 83 | std::cout << prefix << duration << " sec \n"; 84 | } 85 | 86 | } // namespace tree_detection 87 | -------------------------------------------------------------------------------- /tree_detection_ros/src/TreeDetectorRos.cpp: -------------------------------------------------------------------------------- 1 | #include "tree_detection_ros/TreeDetectorRos.hpp" 2 | #include "tree_detection_ros/helpers.hpp" 3 | 4 | namespace tree_detection { 5 | 6 | TreeDetectorRos::TreeDetectorRos(ros::NodeHandlePtr nh) : 7 | nh_(nh) { 8 | } 9 | 10 | void TreeDetectorRos::initRos(std::string filename) { 11 | YAML::Node node = YAML::LoadFile(filename); 12 | auto treeDetectionRos = node["tree_detection_ros"]; 13 | 14 | treeBoundingBoxPubliser_ = nh_->advertise(treeDetectionRos["tree_bounding_boxes_topic_name"].as(), 1, true); 15 | treeCylinderPublisher_ = nh_->advertise(treeDetectionRos["tree_cylinders_topic_name"].as(), 1, true); 16 | } 17 | 18 | void TreeDetectorRos::detectTrees() { 19 | 20 | BASE::detectTrees(); 21 | const auto clusters = getDetectedTreeClusters(); 22 | publishTreeBoundingBoxes(clusters); 23 | publishTreeCylinderMarkers(clusters); 24 | } 25 | 26 | void TreeDetectorRos::setInputPointCloudFromRos(const sensor_msgs::PointCloud2 &cloud) { 27 | PointCloud::Ptr input = fromRos(cloud); 28 | setInputPointCloudPtr(input); 29 | lastCloudInfo_ = cloud.header; 30 | } 31 | 32 | void TreeDetectorRos::setCloudFrameId(const std::string &frameId) { 33 | lastCloudInfo_.frame_id = frameId; 34 | } 35 | void TreeDetectorRos::setCloudTimestamp(const ros::Time &stamp) { 36 | lastCloudInfo_.stamp = stamp; 37 | } 38 | 39 | void TreeDetectorRos::publishTreeCylinderMarkers(const ClusterVector &clusters) const { 40 | visualization_msgs::MarkerArray markers; 41 | int id = 0; 42 | for (const auto &c : clusters) { 43 | Cluster transformedCluster = c; 44 | inflateClusterDimensions(&transformedCluster.clusterDimensions_); 45 | auto marker = getTreeCylinderMarker(transformedCluster.position_, transformedCluster.clusterDimensions_, id++, 46 | lastCloudInfo_.stamp, lastCloudInfo_.frame_id, Color::Green()); 47 | markers.markers.push_back(marker); 48 | } 49 | treeCylinderPublisher_.publish(markers); 50 | } 51 | 52 | void TreeDetectorRos::publishTreeBoundingBoxes(const ClusterVector &clusters) const { 53 | jsk_recognition_msgs::BoundingBoxArray bbxs; 54 | int id = 0; 55 | 56 | for (const auto &c : clusters) { 57 | Cluster transformedCluster = c; 58 | inflateClusterDimensions(&transformedCluster.clusterDimensions_); 59 | auto bb = getTreeBBMarker(transformedCluster.position_, transformedCluster.clusterDimensions_, id++, 60 | lastCloudInfo_.stamp, lastCloudInfo_.frame_id); 61 | bbxs.boxes.push_back(bb); 62 | } 63 | bbxs.header.frame_id = lastCloudInfo_.frame_id; 64 | bbxs.header.stamp = lastCloudInfo_.stamp; 65 | treeBoundingBoxPubliser_.publish(bbxs); 66 | } 67 | 68 | void TreeDetectorRos::inflateClusterDimensions(ClusterDimensions *dim) const { 69 | const auto &p = getParameters(); 70 | dim->dimX_ *= p.treeMarkerRadiusInflateFactor_; 71 | dim->dimY_ *= p.treeMarkerRadiusInflateFactor_; 72 | dim->dimZ_ *= p.treeMarkerHeightInflateFactor_; 73 | } 74 | 75 | } // namespace tree_detection 76 | -------------------------------------------------------------------------------- /tree_detection_ros/src/tree_detection_node.cpp: -------------------------------------------------------------------------------- 1 | #include "tree_detection_ros/TreeDetectorRos.hpp" 2 | #include "tree_detection_ros/helpers.hpp" 3 | #include "tree_detection_ros/creators.hpp" 4 | #include "point_cloud_preprocessing/Parameters.hpp" 5 | #include "point_cloud_preprocessing/PointCloudPreprocessor.hpp" 6 | 7 | 8 | int main(int argc, char **argv) { 9 | using namespace tree_detection; 10 | ros::init(argc, argv, "tree_detection_node"); 11 | ros::NodeHandlePtr nh(new ros::NodeHandle("~")); 12 | const std::string pcdFilename = nh->param("pcd_filepath", ""); 13 | std::cout << "Loading pointcloud from: " << pcdFilename << std::endl; 14 | PointCloud::Ptr loadedCloud = loadPointcloudFromPcd(pcdFilename); 15 | const std::string frameId = "map"; 16 | loadedCloud->header.frame_id = frameId; 17 | ros::Publisher cloudPub = nh->advertise("input_cloud", 1, true); 18 | sensor_msgs::PointCloud2 cloudMsg = toRos(*loadedCloud); 19 | cloudMsg.header.frame_id = frameId; 20 | cloudMsg.header.stamp = ros::Time::now(); 21 | cloudPub.publish(cloudMsg); 22 | 23 | const std::string configFilePath = nh->param("config_filepath", ""); 24 | std::cout << "Loading parameters from: " << configFilePath << std::endl; 25 | 26 | //here create all the shizzle 27 | TreeDetectorRos treeDetector(nh); 28 | treeDetector.initRos(configFilePath); 29 | TreeDetectionParameters treeDetectParam; 30 | loadParameters(configFilePath, &treeDetectParam); 31 | treeDetector.setTreeDetectionParameters(treeDetectParam); 32 | const std::string groundRemovalStrategy = nh->param("ground_removal_strategy", ""); 33 | std::shared_ptr groundRemover = ground_removal::groundRemoverFactory(groundRemovalStrategy, 34 | configFilePath,nh); 35 | std::unique_ptr pointCloudPreprocessor(new point_cloud_preprocessing::PointCloudPreprocessor()); 36 | point_cloud_preprocessing::PointCloudPreprocessorParam pointCloudPreprocessorParam; 37 | point_cloud_preprocessing::loadParameters(configFilePath, &pointCloudPreprocessorParam); 38 | pointCloudPreprocessor->setParameters(pointCloudPreprocessorParam); 39 | 40 | //here preprocess point cloud 41 | pointCloudPreprocessor->setInputCloud(*loadedCloud); 42 | pointCloudPreprocessor->performPreprocessing(); 43 | PointCloud::ConstPtr preprocessedCloud = pointCloudPreprocessor->getPreprocessedCloudPtr(); 44 | 45 | //here crop the ground plane 46 | const auto startTime = std::chrono::steady_clock::now(); 47 | groundRemover->setInputCloud(*preprocessedCloud); 48 | groundRemover->removeGroundPlane(); 49 | printTimeElapsed(startTime,"Time elapsed for ground removal: "); 50 | 51 | //publish the cloud without ground plane 52 | ros::Publisher noGroundCloudPub = nh->advertise("no_ground_cloud", 1, true); 53 | PointCloud::ConstPtr noGroundPlaneCloud = groundRemover->getCloudWithoutGroundPlanePtr(); 54 | sensor_msgs::PointCloud2 noGroundCloudMsg = toRos(*noGroundPlaneCloud); 55 | noGroundCloudMsg.header.frame_id = frameId; 56 | noGroundCloudMsg.header.stamp = ros::Time::now(); 57 | noGroundCloudPub.publish(noGroundCloudMsg); 58 | 59 | //here detect the trees 60 | treeDetector.setInputPointCloud(*noGroundPlaneCloud); 61 | treeDetector.setCloudFrameId(frameId); 62 | treeDetector.setCloudTimestamp(ros::Time::now()); 63 | treeDetector.detectTrees(); 64 | printTimeElapsed(startTime,"Total time: "); 65 | 66 | 67 | 68 | 69 | ros::spin(); 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /tree_detection_ros/src/helpers.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * helpers.cpp 3 | * 4 | * Created on: Aug 18, 2021 5 | * Author: jelavice 6 | */ 7 | #include "tree_detection_ros/helpers.hpp" 8 | #include 9 | 10 | namespace tree_detection { 11 | 12 | Color::Color() : 13 | std_msgs::ColorRGBA() { 14 | } 15 | Color::Color(double red, double green, double blue) : 16 | Color(red, green, blue, 1.0) { 17 | } 18 | Color::Color(double red, double green, double blue, double alpha) : 19 | Color() { 20 | r = red; 21 | g = green; 22 | b = blue; 23 | a = alpha; 24 | } 25 | 26 | visualization_msgs::Marker getTreeCylinderMarker(const Eigen::Vector3d &treeCoordinates, 27 | const ClusterDimensions &clusterDimension, int markerId, const ros::Time ×tamp, const std::string &frameId, 28 | const Color &color) { 29 | 30 | visualization_msgs::Marker treeMarker; 31 | 32 | // Set marker properties 33 | //this is a debug marker for visualization 34 | treeMarker.header.frame_id = frameId; 35 | treeMarker.header.stamp = timestamp; // ros::Time(); 36 | treeMarker.id = markerId; 37 | treeMarker.type = visualization_msgs::Marker::CYLINDER; 38 | treeMarker.action = visualization_msgs::Marker::ADD; 39 | treeMarker.pose.position.x = treeCoordinates.x(); 40 | treeMarker.pose.position.y = treeCoordinates.y(); 41 | treeMarker.pose.position.z = treeCoordinates.z(); 42 | //todo this just assumes that it will be always pinting upright 43 | treeMarker.pose.orientation.x = 0.0; 44 | treeMarker.pose.orientation.y = 0.0; 45 | treeMarker.pose.orientation.z = 0.0; 46 | treeMarker.pose.orientation.w = 1.0; 47 | //get cloud dimensions write a function 48 | treeMarker.scale.x = clusterDimension.dimX_; 49 | treeMarker.scale.y = clusterDimension.dimY_; 50 | treeMarker.scale.z = clusterDimension.dimZ_; 51 | treeMarker.color.a = color.a; 52 | treeMarker.color.r = color.r; 53 | treeMarker.color.g = color.g; 54 | treeMarker.color.b = color.b; 55 | 56 | return treeMarker; 57 | } 58 | 59 | jsk_recognition_msgs::BoundingBox getTreeBBMarker(const Eigen::Vector3d &treeCoordinates, 60 | const ClusterDimensions &clusterDimension, int markerId, const ros::Time ×tamp, const std::string &frameId) { 61 | 62 | jsk_recognition_msgs::BoundingBox treeMarker; 63 | 64 | // Set marker properties 65 | //this is a debug marker for visualization 66 | treeMarker.header.frame_id = frameId; 67 | treeMarker.header.stamp = timestamp; // ros::Time(); 68 | treeMarker.pose.position.x = treeCoordinates.x(); 69 | treeMarker.pose.position.y = treeCoordinates.y(); 70 | treeMarker.pose.position.z = treeCoordinates.z(); 71 | //this just assumes that it will be always pinting upright 72 | treeMarker.pose.orientation.x = 0.0; 73 | treeMarker.pose.orientation.y = 0.0; 74 | treeMarker.pose.orientation.z = 0.0; 75 | treeMarker.pose.orientation.w = 1.0; 76 | //get cloud dimensions write a function 77 | treeMarker.dimensions.x = clusterDimension.dimX_ ; 78 | treeMarker.dimensions.y = clusterDimension.dimY_ ; 79 | treeMarker.dimensions.z = clusterDimension.dimZ_ ; 80 | 81 | return treeMarker; 82 | } 83 | 84 | PointCloud::Ptr fromRos(const sensor_msgs::PointCloud2 &cloud) { 85 | const PointCloud::Ptr pclTypeCloud(new PointCloud); 86 | pcl::fromROSMsg(cloud, *pclTypeCloud); 87 | return pclTypeCloud; 88 | } 89 | 90 | sensor_msgs::PointCloud2 toRos(const PointCloud &cloud){ 91 | sensor_msgs::PointCloud2 cloudOut; 92 | pcl::toROSMsg(cloud, cloudOut); 93 | return cloudOut; 94 | } 95 | 96 | PointCloud::Ptr loadPointcloudFromPcd(const std::string &filename) { 97 | PointCloud::Ptr cloud(new PointCloud); 98 | pcl::PCLPointCloud2 cloudBlob; 99 | pcl::io::loadPCDFile(filename, cloudBlob); 100 | pcl::fromPCLPointCloud2(cloudBlob, *cloud); 101 | return cloud; 102 | } 103 | 104 | } // namespace tree_detection 105 | 106 | -------------------------------------------------------------------------------- /point_cloud_preprocessing/src/Parameters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Parameters.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | #include "point_cloud_preprocessing/Parameters.hpp" 8 | #include 9 | 10 | namespace point_cloud_preprocessing { 11 | 12 | void loadParameters(const std::string &filename, PointCloudPreprocessorParam *p) { 13 | YAML::Node node = YAML::LoadFile(filename); 14 | 15 | if (node.IsNull()) { 16 | throw std::runtime_error("Point cloud preprocessing parameters loading failed"); 17 | } 18 | loadParameters(node["point_cloud_preprocessing"], p); 19 | } 20 | 21 | void loadParameters(const YAML::Node &node, PointCloudPreprocessorParam *p) { 22 | p->isUseCropBox_ = node["is_use_crop_box"].as(); 23 | p->cropBox_.minX_ = node["crop_box"]["crop_box_minX"].as(); 24 | p->cropBox_.maxX_ = node["crop_box"]["crop_box_maxX"].as(); 25 | p->cropBox_.minY_ = node["crop_box"]["crop_box_minY"].as(); 26 | p->cropBox_.maxY_ = node["crop_box"]["crop_box_maxY"].as(); 27 | p->cropBox_.minZ_ = node["crop_box"]["crop_box_minZ"].as(); 28 | p->cropBox_.maxZ_ = node["crop_box"]["crop_box_maxZ"].as(); 29 | 30 | p->isUseVoxelGrid_ = node["is_use_voxel_grid"].as(); 31 | p->voxelGrid_.leafSizeX_ = node["voxel_grid"]["voxel_grid_leafSizeX"].as(); 32 | p->voxelGrid_.leafSizeY_ = node["voxel_grid"]["voxel_grid_leafSizeY"].as(); 33 | p->voxelGrid_.leafSizeZ_ = node["voxel_grid"]["voxel_grid_leafSizeZ"].as(); 34 | } 35 | 36 | std::ostream& operator<<(std::ostream& out, const PointCloudPreprocessorParam& p) { 37 | out << "┌────────────────────────────────────────────────────┐\n"; 38 | out << "│ Point Cloud Preprocessor Param │\n"; 39 | out << "├─────────────────────────────────┬──────────────────┤\n"; 40 | out << "│ " << std::left << std::setw(31) << "is use crop box" 41 | << " │ " << std::right << std::setw(16) << std::fixed 42 | << std::setprecision(4) << p.isUseCropBox_ << " │\n"; 43 | out << "├─────────────────────────────────┼──────────────────┤\n"; 44 | out << "│ " << std::left << std::setw(31) << "crop box minX" 45 | << " │ " << std::right << std::setw(16) << std::fixed 46 | << std::setprecision(4) << p.cropBox_.minX_ << " │\n"; 47 | out << "├─────────────────────────────────┼──────────────────┤\n"; 48 | out << "│ " << std::left << std::setw(31) << "crop box max X" 49 | << " │ " << std::right << std::setw(16) << std::fixed 50 | << std::setprecision(4) << p.cropBox_.minX_ << " │\n"; 51 | out << "├─────────────────────────────────┼──────────────────┤\n"; 52 | out << "│ " << std::left << std::setw(31) << "crop box min Y" 53 | << " │ " << std::right << std::setw(16) << std::fixed 54 | << std::setprecision(4) << p.cropBox_.minY_ << " │\n"; 55 | out << "├─────────────────────────────────┼──────────────────┤\n"; 56 | out << "│ " << std::left << std::setw(31) << "crop box max Y" 57 | << " │ " << std::right << std::setw(16) << std::fixed 58 | << std::setprecision(4) << p.cropBox_.maxY_ << " │\n"; 59 | out << "├─────────────────────────────────┼──────────────────┤\n"; 60 | out << "│ " << std::left << std::setw(31) << "crop box min Z" 61 | << " │ " << std::right << std::setw(16) << std::fixed 62 | << std::setprecision(4) << p.cropBox_.minZ_ << " │\n"; 63 | out << "├─────────────────────────────────┼──────────────────┤\n"; 64 | out << "│ " << std::left << std::setw(31) << "crop box max Z" 65 | << " │ " << std::right << std::setw(16) << std::fixed 66 | << std::setprecision(4) << p.cropBox_.maxZ_ << " │\n"; 67 | out << "├─────────────────────────────────┼──────────────────┤\n"; 68 | out << "│ " << std::left << std::setw(31) << "is use voxel grid" 69 | << " │ " << std::right << std::setw(16) << std::fixed 70 | << std::setprecision(4) << p.isUseVoxelGrid_ << " │\n"; 71 | out << "├─────────────────────────────────┼──────────────────┤\n"; 72 | out << "│ " << std::left << std::setw(31) << "voxel grid leaf size X " 73 | << " │ " << std::right << std::setw(16) << std::fixed 74 | << std::setprecision(4) << p.voxelGrid_.leafSizeX_ << " │\n"; 75 | out << "├─────────────────────────────────┼──────────────────┤\n"; 76 | 77 | out << "│ " << std::left << std::setw(31) << "voxel grid leaf size Y" 78 | << " │ " << std::right << std::setw(16) << std::fixed 79 | << std::setprecision(4) << p.voxelGrid_.leafSizeY_ << " │\n"; 80 | out << "├─────────────────────────────────┼──────────────────┤\n"; 81 | 82 | out << "│ " << std::left << std::setw(31) << "voxel grid leaf size Z" 83 | << " │ " << std::right << std::setw(16) << std::fixed 84 | << std::setprecision(4) << p.voxelGrid_.leafSizeZ_ << " │\n"; 85 | out << "├─────────────────────────────────┼──────────────────┤\n"; 86 | 87 | return out; 88 | } 89 | 90 | } // namespace point_cloud_preprocessing 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /ground_plane_removal/src/ElevationMapGroundPlaneRemover.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ElevationMapGroundPlaneRemover.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: jelavice 6 | */ 7 | 8 | #include "ground_plane_removal/ElevationMapGroundPlaneRemover.hpp" 9 | #include "grid_map_ros/grid_map_ros.hpp" 10 | #include "grid_map_msgs/GridMap.h" 11 | 12 | #ifdef GROUND_PLANE_REMOVAL_OPENMP_FOUND 13 | #include 14 | #endif 15 | 16 | namespace ground_removal { 17 | 18 | namespace { 19 | const std::string elevationLayer = "elevation"; 20 | ros::Publisher gridMapPub; 21 | 22 | template 23 | T clamp(const T val, const T lower, const T upper) { 24 | return std::max(lower, std::min(val, upper)); 25 | } 26 | 27 | void bindToRange(const grid_map::Matrix &data, grid_map::Index *id) { 28 | const int i = clamp(id->x(), 0, (int) data.rows()); 29 | const int j = clamp(id->y(), 0, (int) data.cols()); 30 | id->x() = i; 31 | id->y() = j; 32 | } 33 | 34 | void computePointsForLocalTerrainFit(double R, const grid_map::GridMap &gridMap, std::vector *xs, 35 | std::vector *ys, int downSamplingFactor = 1) { 36 | downSamplingFactor = downSamplingFactor > 0 ? downSamplingFactor : 1; // safety check 37 | const int minNumPoints = 8; //todo magic 38 | xs->clear(); 39 | ys->clear(); 40 | const double res = gridMap.getResolution(); 41 | const int approxNumCellsInCircle = std::ceil(R * R * M_PI / (res * res) * 1.2 / downSamplingFactor); 42 | xs->reserve(approxNumCellsInCircle); 43 | ys->reserve(approxNumCellsInCircle); 44 | int i = 0; 45 | for (grid_map::SpiralIterator it(gridMap, grid_map::Position(0.0, 0.0), R); !it.isPastEnd(); ++it, ++i) { 46 | if (i > minNumPoints && !(i % downSamplingFactor == 0)) { //ensure minimal minNumPoints points 47 | continue; 48 | } 49 | const grid_map::Index circleCellId(*it); 50 | grid_map::Position circleCellPos; 51 | gridMap.getPosition(circleCellId, circleCellPos); 52 | xs->push_back(circleCellPos.x()); 53 | ys->push_back(circleCellPos.y()); 54 | } 55 | //ensure that the center is always in 56 | xs->push_back(0.0); 57 | ys->push_back(0.0); 58 | } 59 | 60 | void medianFiltering(grid_map::GridMap *mapPtr, double estimationRadius, int downsampleFactorNumData, int nThreads) { 61 | 62 | auto &map = *mapPtr; 63 | const grid_map::Matrix &gridMapData = mapPtr->get(elevationLayer); 64 | grid_map::Matrix gridMapDataCopy = mapPtr->get(elevationLayer); 65 | unsigned int linearGridMapSize = mapPtr->getSize().prod(); 66 | std::vector xs, ys; 67 | computePointsForLocalTerrainFit(estimationRadius, map, &xs, &ys, downsampleFactorNumData); 68 | const int maxNpoints = xs.size(); 69 | 70 | #ifdef GROUND_PLANE_REMOVAL_OPENMP_FOUND 71 | omp_set_num_threads(nThreads); 72 | #pragma omp parallel for schedule(static) 73 | #endif 74 | for (unsigned int linearIndex = 0; linearIndex < linearGridMapSize; ++linearIndex) { 75 | std::vector goodHs; 76 | goodHs.reserve(maxNpoints); 77 | const grid_map::Index currentCellId(grid_map::getIndexFromLinearIndex(linearIndex, mapPtr->getSize())); 78 | grid_map::Position currentCellPosition; 79 | map.getPosition(currentCellId, currentCellPosition); 80 | // fill data 81 | for (int i = 0; i < maxNpoints; ++i) { 82 | grid_map::Position currentPointPos(currentCellPosition.x() + xs[i], currentCellPosition.y() + ys[i]); 83 | grid_map::Index currentPointId; 84 | map.getIndex(currentPointPos, currentPointId); 85 | bindToRange(gridMapData, ¤tPointId); 86 | const double h = gridMapData(currentPointId.x(), currentPointId.y()); 87 | bool isAddPoint = std::isfinite(h); 88 | if (isAddPoint) { 89 | goodHs.push_back(h); 90 | } 91 | } 92 | std::nth_element(goodHs.begin(), goodHs.begin() + goodHs.size() / 2, goodHs.end()); 93 | gridMapDataCopy(currentCellId.x(), currentCellId.y()) = goodHs[goodHs.size() / 2]; 94 | } // end iterating over linear index 95 | map.get(elevationLayer) = gridMapDataCopy; 96 | } 97 | 98 | } // namespace 99 | 100 | ElevationMapGroundPlaneRemover::ElevationMapGroundPlaneRemover(ros::NodeHandle *nh) : 101 | nh_(nh) { 102 | } 103 | 104 | void ElevationMapGroundPlaneRemover::setParameters(const GroundPlaneRemoverParam &p) { 105 | auto param = static_cast(&p); 106 | param_ = *param; 107 | pclToGridMap_.setParameters(param_.pclConverter_.get()); 108 | } 109 | const ElevationMapGroundPlaneRemoverParam& ElevationMapGroundPlaneRemover::getParameters() const { 110 | return param_; 111 | } 112 | 113 | const grid_map::GridMap& ElevationMapGroundPlaneRemover::getElevationMap() const { 114 | return elevationMap_; 115 | } 116 | 117 | void ElevationMapGroundPlaneRemover::removeGroundPlane() { 118 | pclToGridMap_.setInputCloud(inputCloud_); 119 | pclToGridMap_.initializeGridMapGeometryFromInputCloud(); 120 | pclToGridMap_.addLayerFromInputCloud(elevationLayer); 121 | auto gridMap = pclToGridMap_.getGridMap(); 122 | if (param_.isUseMedianFiltering_) { 123 | medianFiltering(&gridMap, param_.medianFilteringRadius_, param_.medianFilterDownsampleFactor_, 124 | param_.pclConverter_.get().numThreads_); 125 | } 126 | 127 | elevationMap_ = gridMap; 128 | noGroundPlaneCloud_.reset(new PointCloud); 129 | noGroundPlaneCloud_->points.reserve(inputCloud_->size()); 130 | // const auto &data = gridMap.get(elevationLayer); 131 | for (int i = 0; i < inputCloud_->size(); ++i) { 132 | double x = inputCloud_->points[i].x; 133 | double y = inputCloud_->points[i].y; 134 | snapToMapLimits(gridMap, &x, &y); 135 | const double z = inputCloud_->points[i].z; 136 | const double h = gridMap.atPosition(elevationLayer, grid_map::Position(x, y)); 137 | const bool isSkip = (z < h + param_.minHeightAboveGround_) || ( z > h + param_.maxHeightAboveGround_ ); 138 | if (isSkip) { 139 | continue; 140 | } 141 | noGroundPlaneCloud_->points.push_back(inputCloud_->points[i]); 142 | } 143 | noGroundPlaneCloud_->width = noGroundPlaneCloud_->points.size(); 144 | noGroundPlaneCloud_->height = 1; 145 | noGroundPlaneCloud_->is_dense = true; 146 | 147 | if (nh_ != nullptr) { 148 | gridMapPub = nh_->advertise("elevation_map", 1, true); 149 | grid_map_msgs::GridMap msg; 150 | grid_map::GridMapRosConverter::toMessage(gridMap, msg); 151 | msg.info.header.frame_id = inputCloud_->header.frame_id; 152 | gridMapPub.publish(msg); 153 | } 154 | } 155 | 156 | void ElevationMapGroundPlaneRemover::snapToMapLimits(const grid_map::GridMap &gm, double *x, double *y) const { 157 | if (gm.isInside(grid_map::Position(*x, *y))) { 158 | return; 159 | } 160 | const auto snapped = gm.getClosestPositionInMap(grid_map::Position(*x, *y)); 161 | const auto p0 = gm.getPosition(); 162 | // todo raise an issue on grid_map github about this 163 | 164 | const double hackingFactor = 0.99; 165 | *x = (snapped.x() - p0.x()) * hackingFactor + p0.x(); 166 | *y = (snapped.y() - p0.y()) * hackingFactor + p0.y(); 167 | } 168 | 169 | } // namespace ground_removal 170 | -------------------------------------------------------------------------------- /tree_detection_ros/rviz/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21/Autocompute Value Bounds1 10 | - /MarkerArray1 11 | - /MarkerArray1/Namespaces1 12 | - /BoundingBoxArray1 13 | - /PointCloud22/Autocompute Value Bounds1 14 | - /GridMap1 15 | Splitter Ratio: 0.5 16 | Tree Height: 839 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: PointCloud2 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 1 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: false 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 10 59 | Reference Frame: 60 | Value: false 61 | - Alpha: 1 62 | Autocompute Intensity Bounds: true 63 | Autocompute Value Bounds: 64 | Max Value: 5.043700695037842 65 | Min Value: -3.076345443725586 66 | Value: true 67 | Axis: Z 68 | Channel Name: intensity 69 | Class: rviz/PointCloud2 70 | Color: 245; 121; 0 71 | Color Transformer: AxisColor 72 | Decay Time: 0 73 | Enabled: false 74 | Invert Rainbow: false 75 | Max Color: 255; 255; 255 76 | Min Color: 0; 0; 0 77 | Name: PointCloud2 78 | Position Transformer: XYZ 79 | Queue Size: 10 80 | Selectable: true 81 | Size (Pixels): 3 82 | Size (m): 0.05000000074505806 83 | Style: Flat Squares 84 | Topic: /tree_detection_node/input_cloud 85 | Unreliable: false 86 | Use Fixed Frame: true 87 | Use rainbow: false 88 | Value: false 89 | - Class: rviz/MarkerArray 90 | Enabled: false 91 | Marker Topic: /tree_detection_node/tree_cylinders 92 | Name: MarkerArray 93 | Namespaces: 94 | {} 95 | Queue Size: 100 96 | Value: false 97 | - Alpha: 1 98 | Class: rviz/Axes 99 | Enabled: false 100 | Length: 1 101 | Name: Axes 102 | Radius: 0.10000000149011612 103 | Reference Frame: 104 | Value: false 105 | - Class: jsk_rviz_plugin/BoundingBoxArray 106 | Enabled: true 107 | Name: BoundingBoxArray 108 | Queue Size: 10 109 | Topic: /tree_detection_node/tree_bbs 110 | Unreliable: false 111 | Value: true 112 | alpha: 1 113 | color: 4; 4; 2 114 | coloring: Flat color 115 | line width: 0.15000000596046448 116 | only edge: true 117 | show coords: false 118 | - Alpha: 1 119 | Autocompute Intensity Bounds: true 120 | Autocompute Value Bounds: 121 | Max Value: 16.1895809173584 122 | Min Value: -1.2812024354934692 123 | Value: true 124 | Axis: Z 125 | Channel Name: intensity 126 | Class: rviz/PointCloud2 127 | Color: 255; 255; 255 128 | Color Transformer: AxisColor 129 | Decay Time: 0 130 | Enabled: true 131 | Invert Rainbow: false 132 | Max Color: 255; 255; 255 133 | Min Color: 0; 0; 0 134 | Name: PointCloud2 135 | Position Transformer: XYZ 136 | Queue Size: 10 137 | Selectable: true 138 | Size (Pixels): 2 139 | Size (m): 0.03999999910593033 140 | Style: Points 141 | Topic: /tree_detection_node/no_ground_cloud 142 | Unreliable: false 143 | Use Fixed Frame: true 144 | Use rainbow: true 145 | Value: true 146 | - Alpha: 1 147 | Autocompute Intensity Bounds: true 148 | Class: grid_map_rviz_plugin/GridMap 149 | Color: 200; 200; 200 150 | Color Layer: elevation 151 | Color Transformer: GridMapLayer 152 | ColorMap: default 153 | Enabled: false 154 | Grid Cell Decimation: 1 155 | Grid Line Thickness: 0.10000000149011612 156 | Height Layer: elevation 157 | Height Transformer: GridMapLayer 158 | History Length: 1 159 | Invert ColorMap: false 160 | Max Color: 255; 255; 255 161 | Max Intensity: 10 162 | Min Color: 0; 0; 0 163 | Min Intensity: 0 164 | Name: GridMap 165 | Show Grid Lines: true 166 | Topic: /tree_detection_node/elevation_map 167 | Unreliable: false 168 | Use ColorMap: true 169 | Value: false 170 | Enabled: true 171 | Global Options: 172 | Background Color: 255; 255; 255 173 | Default Light: true 174 | Fixed Frame: map 175 | Frame Rate: 30 176 | Name: root 177 | Tools: 178 | - Class: rviz/Interact 179 | Hide Inactive Objects: true 180 | - Class: rviz/MoveCamera 181 | - Class: rviz/Select 182 | - Class: rviz/FocusCamera 183 | - Class: rviz/Measure 184 | - Class: rviz/SetInitialPose 185 | Theta std deviation: 0.2617993950843811 186 | Topic: /initialpose 187 | X std deviation: 0.5 188 | Y std deviation: 0.5 189 | - Class: rviz/SetGoal 190 | Topic: /move_base_simple/goal 191 | - Class: rviz/PublishPoint 192 | Single click: true 193 | Topic: /clicked_point 194 | Value: true 195 | Views: 196 | Current: 197 | Class: rviz/Orbit 198 | Distance: 132.78628540039062 199 | Enable Stereo Rendering: 200 | Stereo Eye Separation: 0.05999999865889549 201 | Stereo Focal Distance: 1 202 | Swap Stereo Eyes: false 203 | Value: false 204 | Field of View: 0.7853981852531433 205 | Focal Point: 206 | X: 8.717408180236816 207 | Y: -18.83536148071289 208 | Z: -19.237504959106445 209 | Focal Shape Fixed Size: true 210 | Focal Shape Size: 0.05000000074505806 211 | Invert Z Axis: false 212 | Name: Current View 213 | Near Clip Distance: 0.009999999776482582 214 | Pitch: 0.9547967910766602 215 | Target Frame: 216 | Yaw: 5.011143207550049 217 | Saved: ~ 218 | Window Geometry: 219 | Displays: 220 | collapsed: false 221 | Height: 1136 222 | Hide Left Dock: false 223 | Hide Right Dock: true 224 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003db000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 225 | Selection: 226 | collapsed: false 227 | Time: 228 | collapsed: false 229 | Tool Properties: 230 | collapsed: false 231 | Views: 232 | collapsed: true 233 | Width: 1848 234 | X: 1992 235 | Y: 27 236 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # tree_detection 2 | 3 | ## Overview 4 | 5 | This package provides tree detection on pointcloud data. In particular, it is designed for detecting tree trunks which could later be used for tree grabbing and harvesting. It does not provide the full tree segmentation with the crown. The package was developed in the scope of [1] (see below). 6 | 7 | The functionality was developed for an autonomous harvester where the goal is to detect trees in a local map. Nonetheless, it can also be used on large maps to compute tree coordinates or get an estimate of tree density. 8 | 9 | Released under [BSD 3-Clause license](LICENSE). 10 | 11 | **Author:** Edo Jelavic 12 | 13 | **Maintainer:** Edo Jelavic, [jelavice@ethz.ch](jelavice@ethz.ch) 14 | 15 | Examples of tree detection on point clouds: 16 | 17 | | forest 2 (ground removed)| forest 3 | forest 4 | 18 | |:--------:|------------------|--------------| 19 | |[![forest2](doc/forest2.jpg)](doc/forest2.jog)|[![forest3](doc/forest3.jpg)](doc/forest3.jpg)|[![forest4](doc/forest4.jpg)](doc/forest4.jpg)| 20 | 21 | ## Publications 22 | The code for tree detection has been developed as a part of research on autonomous precision harvesting. If you are using this tree detector in academic context, please consider adding the following citation to your publication: 23 | 24 | [1] Jelavic, E., Jud, D., Egli, P. and Hutter, M., 2021. Towards Autonomous Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester. 25 | 26 | @article{jelavic2021towards, 27 | title={Towards Autonomous Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester}, 28 | author={Jelavic, Edo and Jud, Dominic and Egli, Pascal and Hutter, Marco}, 29 | journal={Field Robotics}, 30 | year={2021}, 31 | publisher={Wiley} 32 | } 33 | 34 | ## Packages in this repo 35 | This repository consists of following packages: 36 | 37 | * ***point_cloud_preprocessig*** is a package used to preprocess the point cloud before removing the ground plane 38 | * ***ground_plane_removal*** is a lightweight package for ground plane removal. 39 | * ***tree_detection*** implements the core algorithm for tree detection. 40 | * ***tree_detection_ros*** adds visualization and I/O handling for easier usage. This package is tightly integrated with ROS. 41 | 42 | 43 | ## Algorithm description 44 | 45 | The algorithm first attempts to remove the gound plane. This can be done using two strategies, either a simple box filter (for which the user needs to provide the cropping region) or using an elevation map. In case the elevation map strategy is used, the algorithm first computes the elevation map and then uses it to remove the ground plane. The elevaiton map computation is implemented in a different package (see the link [here](https://github.com/ANYbotics/grid_map/tree/master/grid_map_pcl)) and a more detailed description can be found in [1]. Before proceeding with the pointcloud processing the median filter is applied to the elevation map. 46 | 47 | Once the ground plane is removed, the algorithm proceeds with Euclidean cluster extraction and performs some checks to make sure that the extracted clusters aree indeed trees. The more detailed description can be found in [1]. 48 | 49 | ## Installation 50 | 51 | Clone the following dependencies: 52 | ```bash 53 | # in your source folder `src` 54 | git clone https://github.com/ANYbotics/grid_map.git 55 | ``` 56 | Installing the grid map from apt might not work (you'll get compiler errors). Recommended way is to build the grid map from source. In case you built the grid map from source, but you're still getting errors, most likely the Cmake is finding the system install version of the grid map. Try cleaning your workspace and rebuilding `grid_map_pcl` from source first, as described [here](https://github.com/leggedrobotics/tree_detection/issues/7). 57 | 58 | Install the ROS and library dependencies with: 59 | ```bash 60 | sudo apt install -y ros-noetic-pcl-ros ros-noetic-pcl-conversions ros-noetic-jsk-recognition-msgs ros-noetic-tf2-geometry-msgs 61 | # OR, use rosdep in your source folder `src` 62 | sudo rosdep install -yr --from-paths . 63 | ``` 64 | In case you want to use the detection boxes for visualization you will have to install the [jsk_rviz_plugins](http://wiki.ros.org/jsk_rviz_plugins). 65 | ```bash 66 | sudo apt install ros-noetic-jsk-visualization 67 | ``` 68 | Recommended to build in release mode for performance (`catkin config -DCMAKE_BUILD_TYPE=Release`) 69 | 70 | Build with: 71 | ```bash 72 | catkin build tree_detection_ros 73 | ``` 74 | In case you want to visualize the elevation map created by the ground plane remover, you should compile the grid_map_rviz_plugin as well: 75 | ```bash 76 | catkin build grid_map_rviz_plugin 77 | ``` 78 | 79 | 80 | ## Usage & Examples 81 | 82 | The example datasets can be downloaded [here](https://drive.google.com/drive/folders/1m_sRtMN5n6-ShQvnbCfedKIVpoupao5u?usp=sharing). The folder contains five forest patches. One dataset has almost no clutter (forest3.pcd) and two have a lot of branch clutter (forest1.pcd and forest2.pcd). 83 | 84 | This package can also work with forests on inclined surfaces and we provide two examples (forest4.pcd and forest5.pcd). The point clouds forest4.pcd and forest5.pcd were taken from the [The Montmorency dataset](https://norlab.ulaval.ca/research/montmorencydataset/) and downsampled. Note that for these datasets, we recommend to use a slightly different tuning. See `tree_detection_dataset_4_and_5.yaml`. 85 | 86 | Modify the `pcd_filepath` inside the `tree_detection.launch` to point the location where the point clouds `.pcd` files are stored. 87 | If you want to use a different configuraiton file, you will have to modify the `config_filepath` as well. The ground plane removal strategy can be selected by setting the value of `ground_removal_strategy` parameter. 88 | 89 | Run with: 90 | ```bash 91 | roslaunch tree_detection_ros tree_detection.launch 92 | ``` 93 | The node publishes the input pointcloud, the pointcloud with the ground removed and the elevation map used to remove the ground plane. The detected trees will be marked with green cylinders and bounding boxes. The visualization should appar in the Rviz window. 94 | 95 | Note that the computation can last for up to 7-9 minutes for larger datasets (e.g. forest4.pcd and forest5.pcd). This can be shortened with different tuning, downsampling or voxelizing the point clouds. 96 | 97 | If the choice is made to use the point cloud preprocessing package, special care needs to be taken that the voxel grid leaf size is chosen big enough. (Otherwise, there is an integer overflow in the pcl library and a warning is popping up in the terminal.) In addition, if the ground plane is removed using the elevation map approach, it is important, that the crop box of the preprocessing is not removing partially the point cloud representing the ground plane, else the elevation map will not correctly be generated. This could result to the fact that trees are not detected in the point cloud. 98 | 99 | In general, the preprocessing should only be applied to point clouds, which either have points far away from the origin and/or which are dense. Both phemomena decrease the processing speed of the tree detection significally! The first phenomenum can be handled using the crop box in the preprocessing package. The second one can be tackled using the voxel grid filter in the same package. 100 | 101 | 102 | ## Parameters 103 | 104 | * `ground_plane_removal/cropbox` - Limits (XYZ) for the ground plane removal. The points contained within the box specified by the user will be kept and the rest will be removed. The tree detection will proceed on the remaining points. 105 | * `ground_plane_removal/elevation_map` - Parameters for elevation map extraction from the pointcloud are described [here](https://github.com/ANYbotics/grid_map/tree/master/grid_map_pcl). We add a couple of new parameters which are listed below. 106 | * `ground_plane_removal/min_height_above_ground` - minimal height above the extracted elevation map to keep the points. Any points below this threshold will be discarded. Note that this number can also be negative. 107 | * `ground_plane_removal/max_height_above_ground` - maximal height above the elevation map to keep the points. Any points above this threshold will be discarded. 108 | * `ground_plane_removal/is_use_median_filter` - whether to apply the median filter on the elevation map. 109 | * `ground_plane_removal/median_filtering_radius` - median filtering radius for the elevation map. For each cell, the filter will take the data from the neighbouring cells withing specified radius and apply the median filter. The number of neighbouring points depends on the cell size and this parameter. 110 | * `ground_plane_removal/median_filter_points_downsample_factor` - If one wishes to have sparser coverage, you can apply downsampling. The algorithm will calculate all the points within the specified radius and divide their number by this factor. E.g., (if there are 50 neighbouring points within the radius and the downsample factor is set to 5, 10 points will be used for median filtering). 111 | 112 | 113 | -------------------------------------------------------------------------------- /tree_detection/src/TreeDetector.cpp: -------------------------------------------------------------------------------- 1 | #include "tree_detection/TreeDetector.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace tree_detection { 10 | 11 | void TreeDetector::setTreeDetectionParameters(const TreeDetectionParameters &p) { 12 | treeDetectionParam_ = p; 13 | } 14 | 15 | const TreeDetectionParameters& TreeDetector::getParameters() const { 16 | return treeDetectionParam_; 17 | } 18 | 19 | void TreeDetector::setInputPointCloudPtr(PointCloud::Ptr inputCloud) { 20 | inputCloud_.reset(); 21 | inputCloud_ = inputCloud; 22 | } 23 | 24 | void TreeDetector::setInputPointCloud(const PointCloud &inputCloud) { 25 | inputCloud_.reset(new PointCloud()); 26 | *inputCloud_ = inputCloud; 27 | } 28 | void TreeDetector::detectTrees() { 29 | const auto startTime = std::chrono::steady_clock::now(); 30 | detectedTreeClusters_.clear(); 31 | 32 | //safety check 33 | if (inputCloud_->empty()) { 34 | std::cout << "[TreeDetector] inputCloud is empty. Returning. \n"; 35 | return; 36 | } 37 | 38 | PointIndices setsOfClusterIndices = extractClusterIndices(inputCloud_); 39 | 40 | auto validClusters = extractValidPointClusters(setsOfClusterIndices, inputCloud_); 41 | 42 | if (validClusters.empty()) { 43 | std::cout << "[TreeDetector] No valid clusters found\n"; 44 | return; 45 | } 46 | 47 | auto clusterInfo = extractClusterInformation(validClusters); 48 | detectedTreeClusters_ = clusterInfo; 49 | namespace ch = std::chrono; 50 | const auto endTime = std::chrono::steady_clock::now(); 51 | double numSec = ch::duration_cast(endTime - startTime).count() / 1e6; 52 | if (treeDetectionParam_.isPrintTiming_) { 53 | std::cout << "Tree detection took: " << numSec << " seconds \n"; 54 | } 55 | std::cout << "[TreeDetector]: " << detectedTreeClusters_.size() << " trees were detected \n"; 56 | 57 | } 58 | const TreeDetector::ClusterVector& TreeDetector::getDetectedTreeClusters() const { 59 | return detectedTreeClusters_; 60 | } 61 | 62 | PointIndices TreeDetector::extractClusterIndices(const PointCloud::ConstPtr cloud) const { 63 | 64 | pcl::search::KdTree::Ptr kdTree(new pcl::search::KdTree); 65 | kdTree->setInputCloud(cloud); 66 | 67 | PointIndices clusterIndices; 68 | pcl::EuclideanClusterExtraction euclideanClusterExtraction; 69 | euclideanClusterExtraction.setClusterTolerance(treeDetectionParam_.clusterTolerance_); 70 | euclideanClusterExtraction.setMinClusterSize(treeDetectionParam_.minNumPointsTree_); 71 | euclideanClusterExtraction.setMaxClusterSize(treeDetectionParam_.maxNumPointsTree_); 72 | euclideanClusterExtraction.setSearchMethod(kdTree); 73 | euclideanClusterExtraction.setInputCloud(cloud); 74 | euclideanClusterExtraction.extract(clusterIndices); 75 | 76 | // std::cout << "Min cluster size: " << euclideanClusterExtraction.getMinClusterSize() << "\n"; 77 | // std::cout << "Max cluster size: " << euclideanClusterExtraction.getMaxClusterSize() << "\n"; 78 | // std::cout << "Cluster tolerance: " << euclideanClusterExtraction.getClusterTolerance() << "\n"; 79 | std::cout << "[TreeDetector]: " << clusterIndices.size() << " clusters from cloud extracted." << "\n"; 80 | 81 | return clusterIndices; 82 | } 83 | 84 | PointCloudPtrVector TreeDetector::extractValidPointClusters(const PointIndices &setOfClusterIndices, 85 | PointCloud::ConstPtr cloud) const { 86 | 87 | //these clusters are hopefully trees 88 | PointCloudPtrVector validClusters; 89 | for (std::vector::const_iterator it = setOfClusterIndices.begin(); 90 | it != setOfClusterIndices.end(); ++it) { 91 | 92 | PointCloud::Ptr cloudCluster = extractClusterFromIndices(it->indices, cloud); 93 | 94 | const bool isDiscardCluster = !isGravityAlignmentStraightEnough(cloudCluster) || isClusterTooLow(cloudCluster) 95 | || isClusterRadiusTooBig(cloudCluster); 96 | 97 | if (isDiscardCluster) { 98 | continue; 99 | } 100 | 101 | validClusters.push_back(PointCloud::Ptr(new PointCloud())); 102 | validClusters.back() = cloudCluster; 103 | } 104 | 105 | return validClusters; 106 | } 107 | 108 | PointCloud::Ptr TreeDetector::extractClusterFromIndices(const std::vector &indices, 109 | PointCloud::ConstPtr cloud) const { 110 | PointCloud::Ptr cloudCluster(new PointCloud); 111 | for (auto index : indices) { 112 | cloudCluster->points.push_back(cloud->points[index]); 113 | } 114 | cloudCluster->width = cloudCluster->points.size(); 115 | cloudCluster->height = 1; 116 | cloudCluster->is_dense = true; 117 | return cloudCluster; 118 | } 119 | 120 | bool TreeDetector::isClusterTooLow(PointCloud::ConstPtr cloudCluster) const { 121 | auto clusterDimension = getClusterDimensions(cloudCluster); 122 | 123 | //discard if too low 124 | if (clusterDimension.dimZ_ < treeDetectionParam_.minHeightTree_) { 125 | if (treeDetectionParam_.isPrintDiscardedTreeInstances_) { 126 | std::cout << "Discard cluster candidate with height " << clusterDimension.dimZ_ << "m. \n"; 127 | } 128 | return true; 129 | } 130 | return false; 131 | } 132 | 133 | bool TreeDetector::isClusterRadiusTooBig(PointCloud::ConstPtr cloudCluster) const { 134 | 135 | auto clusterDimension = getClusterDimensions(cloudCluster); 136 | 137 | //discard if diameter wrong 138 | if (clusterDimension.dimX_ > treeDetectionParam_.maxDiameterTree_ 139 | || clusterDimension.dimY_ > treeDetectionParam_.maxDiameterTree_) { 140 | if (treeDetectionParam_.isPrintDiscardedTreeInstances_) { 141 | std::cout << "Discard cluster with width in x axis " << clusterDimension.dimX_ << "m. \n"; 142 | std::cout << "Width in y axis " << clusterDimension.dimY_ << "m. \n"; 143 | } 144 | return true; 145 | } 146 | 147 | return false; 148 | } 149 | 150 | ClusterDimensions TreeDetector::getClusterDimensions(PointCloud::ConstPtr clusterCloud) const { 151 | pcl::PointXYZ minPt, maxPt; 152 | pcl::getMinMax3D(*clusterCloud, minPt, maxPt); 153 | 154 | ClusterDimensions clusterDimensions; 155 | clusterDimensions.dimX_ = maxPt.x - minPt.x; 156 | clusterDimensions.dimY_ = maxPt.y - minPt.y; 157 | clusterDimensions.dimZ_ = maxPt.z - minPt.z; 158 | 159 | // std::cout << "Max x: " << maxPt.x << std::endl; 160 | // std::cout << "Max y: " << maxPt.y << std::endl; 161 | // std::cout << "Max z: " << maxPt.z << std::endl; 162 | // std::cout << "Min x: " << minPt.x << std::endl; 163 | // std::cout << "Min y: " << minPt.y << std::endl; 164 | // std::cout << "Min z: " << minPt.z << std::endl; 165 | 166 | return clusterDimensions; 167 | } 168 | 169 | bool TreeDetector::isGravityAlignmentStraightEnough(PointCloud::ConstPtr cloudCluster) const { 170 | double gravityAlignement = getClusterGravityAlignment(cloudCluster); 171 | // ROS_DEBUG_STREAM("Gravity Alignment: " << gravityAlignement); 172 | if (gravityAlignement > treeDetectionParam_.minEigenVectorAlignment_) { 173 | return true; 174 | } else { 175 | if (treeDetectionParam_.isPrintDiscardedTreeInstances_) { 176 | std::cout << "Discarding because the the gravity alignment " << gravityAlignement << " is less than: " 177 | << treeDetectionParam_.minEigenVectorAlignment_ << "\n"; 178 | } 179 | return false; 180 | } 181 | 182 | } 183 | 184 | double TreeDetector::getClusterGravityAlignment(PointCloud::ConstPtr cloudCluster) const { 185 | 186 | pcl::PCA cloudPCA = new pcl::PCA; 187 | cloudPCA.setInputCloud(cloudCluster); 188 | 189 | Eigen::Matrix3f eigenVectors = cloudPCA.getEigenVectors(); 190 | Eigen::Vector3f gravityAligned = Eigen::Vector3f::UnitZ(); 191 | Eigen::Vector3f principleAxis = eigenVectors.col(0); 192 | double principleAxisAlignment = std::abs(principleAxis.dot(gravityAligned)); 193 | std::pair returnValue; 194 | 195 | return principleAxisAlignment; 196 | 197 | } 198 | 199 | Eigen::Vector3d TreeDetector::getClusterMean(PointCloud::ConstPtr cloudCluster) const { 200 | 201 | // pcl::PCA cloudPCA = new pcl::PCA; 202 | // cloudPCA.setInputCloud(cloudCluster); 203 | // Eigen::Vector4f mean = cloudPCA.getMean(); 204 | const int N = cloudCluster->size(); 205 | double meanX = 0, meanY = 0, meanZ = 0; 206 | for (int i = 0; i < N; ++i) { 207 | meanX += cloudCluster->points.at(i).x; 208 | meanY += cloudCluster->points.at(i).y; 209 | meanZ += cloudCluster->points.at(i).z; 210 | } 211 | 212 | return Eigen::Vector3d(meanX / N, meanY / N, meanZ / N); 213 | 214 | } 215 | 216 | TreeDetector::ClusterVector TreeDetector::extractClusterInformation(const PointCloudPtrVector &validClusters) const { 217 | 218 | auto clusterCloudToCluster = [this](const PointCloud::Ptr cloud) { 219 | Cluster cluster; 220 | cluster.position_ = getClusterMean(cloud); 221 | cluster.clusterDimensions_ = getClusterDimensions(cloud); 222 | return cluster; 223 | }; 224 | 225 | std::vector clusters; 226 | clusters.reserve(validClusters.size()); 227 | for (int i = 0; i < validClusters.size(); ++i) { 228 | Cluster c = clusterCloudToCluster(validClusters.at(i)); 229 | clusters.push_back(c); 230 | } 231 | 232 | return clusters; 233 | } 234 | 235 | } // namespace tree_detection 236 | --------------------------------------------------------------------------------