├── .gitignore ├── LICENSE.txt ├── README.md ├── laser_segmentation ├── CMakeLists.txt ├── LICENSE.txt ├── include │ └── laser_segmentation │ │ ├── point_type.h │ │ └── segmenter.h ├── launch │ └── segmentation.launch ├── nodelet_plugins.xml ├── package.xml └── src │ ├── laser_segmentation_node.cpp │ ├── laser_segmentation_nodelet.cpp │ └── segmenter.cpp └── object_detection ├── CMakeLists.txt ├── LICENSE.txt ├── include └── object_detection │ └── detector.h ├── launch ├── object_detection.launch ├── visualize_on_simulated_data.launch └── visualize_on_simulated_data.rviz ├── nodelet_plugins.xml ├── package.xml └── src ├── detector.cpp ├── object_detection_node.cpp └── object_detection_nodelet.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.directory 2 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Jan Razlaw 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Laser Segmentation 2 | 3 | The ROS packages provide: 4 | - the [segmentation method](/laser_segmentation) for LiDAR point clouds 5 | - and the [detection method](/object_detection) based on the generated segments 6 | 7 | used in our paper ["Detection and Tracking of Small Objects in Sparse 3D Laser Range Data"](https://arxiv.org/abs/1903.05889). 8 | The accompanying [video](https://www.youtube.com/watch?v=rl49G1rY5Pk) visualizes the setup and data. 9 | 10 | # Limitations 11 | 12 | The implementation in it's current form supports: 13 | - sensor_msgs::LaserScan messages from Hokuyo single-scan-ring laser range scanners, 14 | - and point clouds from the Velodyne Puck. 15 | 16 | It can easily be extended to support any LiDAR providing 360 degrees scan rings. 17 | 18 | The assumption in our setup was that the sensor is scanning the environment mostly horizontally - e.g. mounted at the bottom of an unmanned aerial vehicle. 19 | This assumption allowed to specify a minimal and maximal width of an object and segment all scan rings individually into point groups of the specified size. 20 | 21 | The input point cloud has to consist of points providing a field named distance. 22 | This field has to contain the distance from the sensor to the measurement as a floating point number. 23 | The [Usage](#usage) section links to a bag file with exemplary data. 24 | 25 | # Installation 26 | 27 | The object_detection package depends on the [multi_hypothesis_tracking_msgs](https://github.com/AIS-Bonn/multi_hypothesis_tracking_msgs) ROS package. 28 | Clone and build all three packages as usual. 29 | 30 | Tested with Ubuntu 18.04 and ROS Melodic. 31 | 32 | # Usage 33 | 34 | To see a demo: 35 | 1) Download the [example bag file](https://www.dropbox.com/s/cwuc7k2p5csrlw6/simulated_humans_scanned_by_moving_velodyne_puck.bag?dl=0). 36 | 2) Update the path to the bag file in the [visualize_on_simulated_data.launch](/object_detection/launch/visualize_on_simulated_data.launch) file. 37 | 3) Launch the launch file - 38 | `roslaunch object_detection visualize_on_simulated_data.launch` 39 | 40 | The bag file contains simulated data from a moving Velodyne Puck. 41 | The simulated environment is populated with 50 simulated humans moving with random speeds to random points on a plane. 42 | 43 | The packages are part of a pipeline to segment, detect, and track multiple objects. 44 | To see the full demo: 45 | - also clone and build the [multi_hypothesis_tracking](https://github.com/AIS-Bonn/multi_hypothesis_tracking) package, 46 | - and set the _use_multi_hypothesis_tracker_ parameter at the top of [visualize_on_simulated_data.launch](/object_detection/launch/visualize_on_simulated_data.launch) to true. 47 | 48 | # Citation 49 | 50 | ``` 51 | @inproceedings{razlaw2019detection, 52 | title={Detection and Tracking of Small Objects in Sparse 3D Laser Range Data}, 53 | author={Razlaw, Jan and Quenzel, Jan and Behnke, Sven}, 54 | booktitle={2019 International Conference on Robotics and Automation (ICRA)}, 55 | pages={2967--2973}, 56 | year={2019}, 57 | organization={IEEE} 58 | } 59 | ``` 60 | 61 | # License 62 | 63 | The packages are licensed under BSD-3. 64 | -------------------------------------------------------------------------------- /laser_segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_segmentation) 3 | 4 | add_definitions(-std=c++11) 5 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 6 | 7 | OPTION(USE_OMP "Use OpenMP" OFF) 8 | if(USE_OMP) 9 | FIND_PACKAGE(OpenMP) 10 | if(OPENMP_FOUND) 11 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 13 | endif() 14 | endif() 15 | 16 | ## Find catkin macros and libraries 17 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 18 | ## is used, also find other catkin packages 19 | find_package(catkin REQUIRED COMPONENTS 20 | roscpp 21 | rospy 22 | std_msgs 23 | pcl_ros 24 | nodelet 25 | tf 26 | eigen_conversions 27 | tf_conversions 28 | laser_geometry 29 | sensor_msgs 30 | ) 31 | 32 | catkin_package( 33 | INCLUDE_DIRS include 34 | ) 35 | 36 | ########### 37 | ## Build ## 38 | ########### 39 | 40 | ## Specify additional locations of header files 41 | ## Your package locations should be listed before other locations 42 | include_directories( 43 | include 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | 48 | ## Declare a C++ executable 49 | add_executable(laser_segmentation_node 50 | src/laser_segmentation_node.cpp 51 | src/segmenter.cpp 52 | ) 53 | 54 | ## Add cmake target dependencies of the executable 55 | add_dependencies(laser_segmentation_node 56 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 57 | ${catkin_EXPORTED_TARGETS} 58 | ) 59 | 60 | ## Specify libraries to link a library or executable target against 61 | target_link_libraries(laser_segmentation_node 62 | ${catkin_LIBRARIES} 63 | ) 64 | 65 | install(TARGETS laser_segmentation_node 66 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 67 | 68 | ## Declare a C++ library 69 | add_library(laser_segmentation_nodelet 70 | src/laser_segmentation_nodelet.cpp 71 | src/segmenter.cpp 72 | ) 73 | 74 | add_dependencies(laser_segmentation_nodelet 75 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 76 | ${catkin_EXPORTED_TARGETS} 77 | ) 78 | 79 | target_link_libraries(laser_segmentation_nodelet 80 | ${catkin_LIBRARIES} 81 | ) 82 | 83 | install(TARGETS laser_segmentation_nodelet 84 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | ) 88 | 89 | ## Install project namespaced headers 90 | install(DIRECTORY include/${PROJECT_NAME}/ 91 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 92 | ) 93 | -------------------------------------------------------------------------------- /laser_segmentation/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Jan Razlaw 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /laser_segmentation/include/laser_segmentation/point_type.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- 2 | * 3 | * WARNING: If you use this header, always include it first. 4 | * It defines PCL_NO_PRECOMPILE which has to be defined in order 5 | * to use templated pcl functions. If you use the latter, make 6 | * sure to include their .hpp files as well. 7 | */ 8 | 9 | #ifndef __LASER_SEGMENTATION_POINT_TYPES_H 10 | #define __LASER_SEGMENTATION_POINT_TYPES_H 11 | 12 | #define PCL_NO_PRECOMPILE 13 | 14 | #include 15 | #include 16 | 17 | namespace laser_segmentation 18 | { 19 | /** Euclidean Velodyne coordinate, including intensity and ring number. */ 20 | struct PointXYZIR 21 | { 22 | PCL_ADD_POINT4D; // quad-word XYZ 23 | float intensity; ///< laser intensity reading 24 | uint16_t ring; ///< laser ring number 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 26 | } EIGEN_ALIGN16; 27 | 28 | /** Euclidean Velodyne coordinate, including intensity, distance and ring number. */ 29 | struct PointXYZIDR 30 | { 31 | PCL_ADD_POINT4D; // quad-word XYZ 32 | float intensity; ///< laser intensity reading 33 | float distance; ///< distance of point to sensor 34 | uint16_t ring; ///< laser ring number 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 36 | } EIGEN_ALIGN16; 37 | 38 | /** Euclidean Velodyne coordinate, including object segmentation flag and ring number. */ 39 | struct PointXYZSegmentation 40 | { 41 | PCL_ADD_POINT4D; ///< quad-word XYZ 42 | uint8_t segmentation; ///< segmentation flag 43 | uint16_t ring; ///< laser ring number 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 45 | } EIGEN_ALIGN16; 46 | 47 | }; // namespace laser_segmentation 48 | 49 | POINT_CLOUD_REGISTER_POINT_STRUCT(laser_segmentation::PointXYZIR, 50 | (float, x, x) 51 | (float, y, y) 52 | (float, z, z) 53 | (float, intensity, intensity) 54 | (uint16_t, ring, ring)) 55 | 56 | POINT_CLOUD_REGISTER_POINT_STRUCT(laser_segmentation::PointXYZIDR, 57 | (float, x, x) 58 | (float, y, y) 59 | (float, z, z) 60 | (float, intensity, intensity) 61 | (float, distance, distance) 62 | (uint16_t, ring, ring)) 63 | 64 | POINT_CLOUD_REGISTER_POINT_STRUCT(laser_segmentation::PointXYZSegmentation, 65 | (float, x, x) 66 | (float, y, y) 67 | (float, z, z) 68 | (uint8_t, segmentation, segmentation) 69 | (uint16_t, ring, ring)) 70 | 71 | #endif // __LASER_SEGMENTATION_POINT_TYPES_H 72 | 73 | -------------------------------------------------------------------------------- /laser_segmentation/include/laser_segmentation/segmenter.h: -------------------------------------------------------------------------------- 1 | /* -*- mode: C++ -*- */ 2 | /** @file 3 | * 4 | * This class segments objects of a specified width in laser point clouds 5 | * 6 | * @author Jan Razlaw 7 | */ 8 | 9 | #ifndef _SEGMENTER_H_ 10 | #define _SEGMENTER_H_ 1 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | 43 | namespace laser_segmentation 44 | { 45 | 46 | /** 47 | * Class to segment objects of a specified width in laser point clouds. 48 | */ 49 | class Segmenter 50 | { 51 | public: 52 | typedef laser_segmentation::PointXYZIDR InputPoint; 53 | // TODO: rename to segmented_cloud or output_cloud 54 | typedef laser_segmentation::PointXYZSegmentation Point4Detection; 55 | 56 | typedef pcl::PointCloud InputPointCloud; 57 | typedef pcl::PointCloud Cloud4Detection; 58 | 59 | typedef typename boost::circular_buffer::iterator buffer_iterator; 60 | typedef typename boost::circular_buffer::const_iterator buffer_const_iterator; 61 | 62 | typedef boost::circular_buffer BufferInputPoints; 63 | typedef std::shared_ptr BufferInputPointsPtr; 64 | 65 | /** Point and additional information from the filters. */ 66 | struct MedianFiltered { 67 | MedianFiltered(InputPoint p):point(p){}; 68 | MedianFiltered(){}; 69 | InputPoint point; ///< 3D point + intensity + distance + ring 70 | float dist_noise_kernel; ///< result of noise filter on distances 71 | float dist_object_kernel; ///< result of object filter on distances 72 | float intens_noise_kernel; ///< result of noise filter on intensities 73 | float intens_object_kernel; ///< result of object filter on intensities 74 | }; EIGEN_ALIGN16; 75 | 76 | typedef boost::circular_buffer BufferMedians; 77 | typedef std::shared_ptr BufferMediansPtr; 78 | typedef typename BufferMedians::iterator median_iterator; 79 | typedef typename BufferMedians::const_iterator median_const_iterator; 80 | 81 | /** 82 | * @brief Constructor. 83 | * 84 | * Parameters are needed to use this class within a ros node or nodelet. 85 | */ 86 | Segmenter(ros::NodeHandle node, ros::NodeHandle private_nh); 87 | /** @brief Destructor. */ 88 | ~Segmenter(){ m_time_file.close(); }; 89 | 90 | /** @brief Set up circular buffers and iterators. Number depends on sensor.*/ 91 | void initBuffer(int number_of_rings); 92 | /** @brief Clear circular buffers and iterators. */ 93 | void resetBuffer(); 94 | 95 | /** 96 | * @brief Process one scan from a Hokuyo laser range scanner. 97 | * 98 | * Transform one 2D scan line from the sensor's frame to a 3D point cloud 99 | * in the base_link. 100 | * Feeds those points to the circular buffer and starts the segmentation. 101 | * 102 | * @param[in] input_scan one scan line from the Hokuyo. 103 | */ 104 | void hokuyoCallback(const sensor_msgs::LaserScanConstPtr& input_scan); 105 | 106 | /** 107 | * @brief Process one scan from a Velodyne laser range scanner. 108 | * 109 | * Feeds points from scan rings to the corresponding circular buffers and 110 | * starts the segmentation. 111 | * 112 | * @param[in] input_cloud one scan from the Velodyne. 113 | */ 114 | void velodyneCallback(const InputPointCloud::ConstPtr &input_cloud); 115 | 116 | /** 117 | * @brief Segment the current points in the circular buffers. 118 | * 119 | * Applies the noise and object filters to the points in the circular buffers 120 | * and computes the segmentation probabilities depending on the results. 121 | * Publishes the resulting cloud in the end. 122 | * 123 | * @param[in] header header of the currently processed point's cloud, needed for publishing. 124 | */ 125 | void processScan(pcl::PCLHeader header); 126 | 127 | /** 128 | * @brief Apply the noise and object filters to the unfiltered points in the buffers. 129 | * 130 | * For each unfiltered point in the buffers, compute the kernel sizes wrt. the distance 131 | * and apply the noise and the object median filters. 132 | * 133 | * @param[in,out] buffer_median_filtered circular buffer containing the filtered and unfiltered points. 134 | * @param[in,out] iter iterator pointing to the first unfiltered point that should be filtered next. 135 | */ 136 | void filterRing(std::shared_ptr > buffer_median_filtered, 137 | median_iterator& iter); 138 | 139 | /** 140 | * @brief Apply the noise and object filters to the current point. 141 | * 142 | * Fills a vector with the distance values of the neighbors of the current point. 143 | * Vector has the size of the object median filter kernel. 144 | * Computes noise median filter result first, as the vector is partly sorted during the process. 145 | * Computes the object median filter result afterwards. 146 | * 147 | * @param[in] noise_filter_kernel_radius half kernel size of the noise filter. 148 | * @param[in] object_filter_kernel_radius half kernel size of the object filter. 149 | * @param[in] buffer pointer to circular buffer. 150 | * @param[in] current_element iterator pointing to current point. 151 | * @param[in] getValue function to get either the distance value or the intensity value of the point. 152 | * @param[in] isValid function to check if value is valid. 153 | * @param[in] max_dist_for_median_computation threshold that can optionally be used to discard neighbors if their distance difference to the current point is too high. 154 | * @param[out] noise_filter_result result of the noise median filter for the current point. 155 | * @param[out] object_filter_result result of the object median filter for the current point. 156 | */ 157 | void calcMedianFromBuffer(const int noise_filter_kernel_radius, 158 | const int object_filter_kernel_radius, 159 | const BufferMediansPtr& buffer, 160 | const median_const_iterator& current_element, 161 | std::function getValue, 162 | std::function isValid, 163 | const float invalid_value, 164 | float& noise_filter_result, 165 | float& object_filter_result) const; 166 | 167 | /** 168 | * @brief Converts delta to a segmentation probability. 169 | * 170 | * @param[in] delta difference between the results of the object and noise filter. 171 | * @param[in] do_weighting if true, delta is weighted using #m_max_intensity_range and #m_weight_for_small_intensities. 172 | * 173 | * @return segmentation probability 174 | */ 175 | float computeSegmentationProbability(float delta, bool do_weighting=false); 176 | 177 | /** 178 | * @brief Computes the segmentation probabilities for the points in the buffers. 179 | * 180 | * @param[in,out] median_it iterator pointing to the currently processed element. 181 | * @param[in] steps number of entries in the buffer to segment. 182 | * @param[out] cloud4detection segmented point cloud (from one scan). 183 | * @see segmentRing() 184 | */ 185 | void segmentRing(median_iterator& median_it, 186 | int steps, 187 | Cloud4Detection::Ptr& cloud4detection); 188 | 189 | /** 190 | * @brief Computes the segmentation probabilities for the points in the buffers. 191 | * 192 | * @param[in,out] median_it iterator pointing to the currently processed element. 193 | * @param[in] end iterator to the last point in the buffer that was filtered. 194 | * @param[out] cloud4detection segmented point cloud (from one scan). 195 | */ 196 | void segmentRing(median_iterator& median_it, 197 | median_iterator& end, 198 | Cloud4Detection::Ptr& cloud4detection); 199 | 200 | private: 201 | const int PUCK_NUM_RINGS; 202 | const int HOKUYO_NUM_RINGS; 203 | 204 | bool m_input_is_velodyne; 205 | 206 | ros::Subscriber m_velodyne_sub; 207 | ros::Subscriber m_hokuyo_sub; 208 | ros::Publisher m_pub_detection_cloud; 209 | 210 | tf::TransformListener m_tf_listener; 211 | 212 | int m_circular_buffer_capacity; 213 | float m_angle_between_scanpoints; 214 | 215 | int m_max_kernel_size; 216 | 217 | float m_max_prob_by_distance; 218 | float m_max_intensity_range; 219 | 220 | float m_certainty_threshold; 221 | float m_dist_weight; 222 | float m_intensity_weight; 223 | float m_weight_for_small_intensities; 224 | 225 | float m_min_object_width; 226 | float m_max_object_width; 227 | 228 | float m_delta_min; 229 | float m_delta_low; 230 | float m_delta_high; 231 | float m_delta_max; 232 | 233 | std::vector m_median_filtered_circ_buffer_vector; 234 | 235 | std::vector> m_median_iters_by_ring; 236 | std::vector> m_segmentation_iters_by_ring; 237 | 238 | bool m_buffer_initialized; 239 | laser_geometry::LaserProjection m_scan_projector; 240 | 241 | float m_max_range; 242 | float m_almost_max_range; 243 | bool m_is_organized_cloud; 244 | 245 | bool m_first_cloud; 246 | 247 | bool m_use_all_neighbors_for_median; 248 | 249 | float m_self_filter_radius; 250 | 251 | bool m_measure_time; 252 | std::chrono::microseconds m_summed_time_for_callbacks; 253 | int m_number_of_callbacks; 254 | std::ofstream m_time_file; 255 | }; 256 | 257 | } // namespace laser_segmentation 258 | 259 | #endif // _SEGMENTER_H_ 260 | -------------------------------------------------------------------------------- /laser_segmentation/launch/segmentation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /laser_segmentation/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Segment objects by width in laser range data 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /laser_segmentation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | laser_segmentation 3 | Segment objects by width in laser range data 4 | 0.0.1 5 | BSD 6 | Jan Razlaw 7 | 8 | catkin 9 | roscpp 10 | rospy 11 | std_msgs 12 | pcl_ros 13 | nodelet 14 | tf 15 | eigen_conversions 16 | tf_conversions 17 | laser_geometry 18 | sensor_msgs 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /laser_segmentation/src/laser_segmentation_node.cpp: -------------------------------------------------------------------------------- 1 | /** \file 2 | * 3 | * This ROS node segments objects of a specified width in laser point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | /** 12 | * Main node entry point. 13 | */ 14 | int main(int argc, char **argv) 15 | { 16 | ros::init(argc, argv, "laser_segmentation_node"); 17 | ros::NodeHandle node; 18 | ros::NodeHandle priv_nh("~"); 19 | 20 | // create conversion class, which subscribes to raw data 21 | laser_segmentation::Segmenter segmenter(node, priv_nh); 22 | 23 | // handle callbacks until shut down 24 | ros::spin(); 25 | 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /laser_segmentation/src/laser_segmentation_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /** @file 2 | * 3 | * This ROS node segments objects of a specified width in laser point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace laser_segmentation 15 | { 16 | /** 17 | * @brief Laser segmentation in a nodelet 18 | * @see Segmenter 19 | */ 20 | class LaserSegmenterNodelet: public nodelet::Nodelet 21 | { 22 | public: 23 | 24 | LaserSegmenterNodelet() {} 25 | ~LaserSegmenterNodelet() {} 26 | 27 | private: 28 | 29 | virtual void onInit(); 30 | boost::shared_ptr segmenter_; 31 | }; 32 | 33 | /** 34 | * @brief Nodelet initialization. 35 | */ 36 | void LaserSegmenterNodelet::onInit() 37 | { 38 | segmenter_.reset(new Segmenter(getNodeHandle(), getPrivateNodeHandle())); 39 | } 40 | 41 | } // namespace laser_segmentation 42 | 43 | 44 | // Register this plugin with pluginlib. Names must match nodelet_plugin.xml. 45 | PLUGINLIB_EXPORT_CLASS(laser_segmentation::LaserSegmenterNodelet, nodelet::Nodelet); 46 | -------------------------------------------------------------------------------- /laser_segmentation/src/segmenter.cpp: -------------------------------------------------------------------------------- 1 | /** @file 2 | * 3 | * This class segments objects of a specified width in laser point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #include 9 | 10 | namespace laser_segmentation 11 | { 12 | Segmenter::Segmenter(ros::NodeHandle node, ros::NodeHandle private_nh) 13 | : PUCK_NUM_RINGS(16) 14 | , HOKUYO_NUM_RINGS(1) 15 | , m_input_is_velodyne(true) 16 | , m_circular_buffer_capacity(6000) 17 | , m_angle_between_scanpoints(0.2f) // 0.1 for 5Hz 0.2 for 10Hz 0.4 for 20Hz 18 | , m_max_kernel_size(100) 19 | , m_max_intensity_range(100.f) 20 | , m_certainty_threshold(0.f) 21 | , m_dist_weight(0.75f) 22 | , m_intensity_weight(0.25f) 23 | , m_weight_for_small_intensities(10.f) 24 | , m_min_object_width(0.2f) 25 | , m_max_object_width(1.2f) 26 | , m_delta_min(2.5f) 27 | , m_delta_low(5.f) 28 | , m_delta_high(200.f) 29 | , m_delta_max(200.f) 30 | , m_buffer_initialized(false) 31 | , m_max_range(130.f) 32 | , m_almost_max_range(m_max_range - 5.f) 33 | , m_is_organized_cloud(false) 34 | , m_first_cloud(true) 35 | , m_use_all_neighbors_for_median(false) 36 | , m_self_filter_radius(1.f) 37 | , m_measure_time(false) 38 | , m_number_of_callbacks(0) 39 | { 40 | ROS_INFO("laser_segmentation::Segmenter: Init..."); 41 | 42 | m_pub_detection_cloud = node.advertise("/laser_segments4detection", 1); 43 | 44 | private_nh.getParam("certainty_threshold", m_certainty_threshold); 45 | 46 | private_nh.getParam("dist_weight", m_dist_weight); 47 | private_nh.getParam("intensity_weight", m_intensity_weight); 48 | 49 | private_nh.getParam("min_object_width", m_min_object_width); 50 | private_nh.getParam("max_object_width", m_max_object_width); 51 | 52 | private_nh.getParam("delta_min", m_delta_min); 53 | private_nh.getParam("delta_low", m_delta_low); 54 | private_nh.getParam("delta_high", m_delta_high); 55 | private_nh.getParam("delta_max", m_delta_max); 56 | 57 | private_nh.getParam("circular_buffer_capacity", m_circular_buffer_capacity); 58 | private_nh.getParam("max_kernel_size", m_max_kernel_size); 59 | private_nh.getParam("angle_between_scanpoints", m_angle_between_scanpoints); 60 | private_nh.getParam("use_all_neighbors_for_median", m_use_all_neighbors_for_median); 61 | private_nh.getParam("self_filter_radius", m_self_filter_radius); 62 | private_nh.getParam("measure_time", m_measure_time); 63 | if(m_measure_time) 64 | { 65 | std::string path_to_results_file = "/tmp/times_laser_segmentation"; 66 | m_time_file.open(path_to_results_file); 67 | } 68 | 69 | m_summed_time_for_callbacks = std::chrono::microseconds::zero(); 70 | 71 | std::string input_topic = "/velodyne_points"; 72 | private_nh.getParam("input_topic", input_topic); 73 | private_nh.getParam("input_is_velodyne", m_input_is_velodyne); 74 | if(m_input_is_velodyne) 75 | m_velodyne_sub = node.subscribe(input_topic, 1, &Segmenter::velodyneCallback, this); 76 | else 77 | m_hokuyo_sub = node.subscribe(input_topic, 1, &Segmenter::hokuyoCallback, this); 78 | } 79 | 80 | void Segmenter::initBuffer(int number_of_rings) 81 | { 82 | for(int i = 0; i < number_of_rings; i++) 83 | { 84 | BufferMediansPtr median_filtered_circ_buffer(new BufferMedians(m_circular_buffer_capacity)); 85 | m_median_filtered_circ_buffer_vector.push_back(median_filtered_circ_buffer); 86 | } 87 | 88 | m_median_iters_by_ring.resize(number_of_rings); 89 | m_segmentation_iters_by_ring.resize(number_of_rings); 90 | 91 | m_buffer_initialized = true; 92 | } 93 | 94 | void Segmenter::resetBuffer() 95 | { 96 | for(int ring = 0; ring < (int)m_median_filtered_circ_buffer_vector.size(); ring++) 97 | { 98 | m_median_filtered_circ_buffer_vector.at(ring)->clear(); 99 | m_median_iters_by_ring.at(ring).reset(); 100 | m_segmentation_iters_by_ring.at(ring).reset(); 101 | } 102 | } 103 | 104 | void Segmenter::hokuyoCallback(const sensor_msgs::LaserScanConstPtr& input_scan) 105 | { 106 | if(m_pub_detection_cloud.getNumSubscribers() == 0) 107 | { 108 | ROS_DEBUG_STREAM("Segmenter::hokuyoCallback: No subscriber to laser_segmentation. Resetting buffer."); 109 | resetBuffer(); 110 | return; 111 | } 112 | 113 | if(!m_buffer_initialized) 114 | initBuffer(HOKUYO_NUM_RINGS); 115 | 116 | sensor_msgs::PointCloud2 cloud; 117 | InputPointCloud::Ptr cloud_transformed(new InputPointCloud()); 118 | 119 | std::string frame_id = "base_link"; 120 | 121 | if (!m_tf_listener.waitForTransform(frame_id, input_scan->header.frame_id, input_scan->header.stamp + ros::Duration().fromSec((input_scan->ranges.size()) * input_scan->time_increment), ros::Duration(0.1))) 122 | { 123 | ROS_ERROR_THROTTLE(10.0, "Segmenter::hokuyoCallback: Could not wait for transform."); 124 | return; 125 | } 126 | 127 | // transform 2D scan line to 3D point cloud 128 | try 129 | { 130 | m_scan_projector.transformLaserScanToPointCloud(frame_id, *input_scan, cloud, m_tf_listener, 35.f, 131 | (laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Distance)); 132 | 133 | // fix fields.count member 134 | for (unsigned int i = 0; i < cloud.fields.size(); i++) 135 | cloud.fields[i].count = 1; 136 | 137 | sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); 138 | sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); 139 | sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); 140 | sensor_msgs::PointCloud2Iterator iter_intensity(cloud, "intensity"); 141 | sensor_msgs::PointCloud2Iterator iter_distance(cloud, "distances"); 142 | 143 | cloud_transformed->points.reserve(cloud.height * cloud.width); 144 | for(; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_distance) 145 | { 146 | if(*iter_distance >= m_almost_max_range || std::isnan(*iter_x)) 147 | continue; 148 | 149 | InputPoint point; 150 | point.x = *iter_x; 151 | point.y = *iter_y; 152 | point.z = *iter_z; 153 | point.intensity = *iter_intensity; 154 | point.distance = *iter_distance; 155 | point.ring = 0; 156 | 157 | m_median_filtered_circ_buffer_vector[point.ring]->push_back(point); 158 | } 159 | } 160 | catch (tf::TransformException& exc) 161 | { 162 | ROS_ERROR_THROTTLE(10.0, "Segmenter::hokuyoCallback: No transform found."); 163 | ROS_ERROR_THROTTLE(10.0, "message: '%s'", exc.what()); 164 | } 165 | 166 | processScan(pcl_conversions::toPCL(cloud.header)); 167 | } 168 | 169 | void Segmenter::velodyneCallback(const InputPointCloud::ConstPtr &input_cloud) 170 | { 171 | if(m_pub_detection_cloud.getNumSubscribers() == 0) 172 | { 173 | ROS_DEBUG_STREAM("Segmenter::velodyneCallback: No subscriber to laser_segmentation. Resetting buffer"); 174 | resetBuffer(); 175 | return; 176 | } 177 | 178 | auto callback_start_time = std::chrono::high_resolution_clock::now(); 179 | 180 | if(!m_first_cloud && m_is_organized_cloud != (input_cloud->height > 1)) 181 | ROS_FATAL("\nCloud switched between being organized and not being organized!!!\n"); 182 | 183 | m_is_organized_cloud = input_cloud->height > 1; 184 | 185 | if(!m_buffer_initialized) 186 | initBuffer(PUCK_NUM_RINGS); 187 | 188 | for(const auto& point : input_cloud->points) 189 | { 190 | // skip this point if we do not use max range values and point is either at max range or is not valid at all 191 | if(!m_is_organized_cloud && (point.distance >= m_almost_max_range || std::isnan(point.x))) 192 | continue; 193 | 194 | m_median_filtered_circ_buffer_vector[point.ring]->push_back(point); 195 | // if point is not valid, set distance and intensity to invalid values 196 | if(std::isnan(m_median_filtered_circ_buffer_vector[point.ring]->back().point.x)) 197 | { 198 | m_median_filtered_circ_buffer_vector[point.ring]->back().point.distance = m_max_range; 199 | m_median_filtered_circ_buffer_vector[point.ring]->back().point.intensity = -1.f; 200 | } 201 | } 202 | 203 | processScan(input_cloud->header); 204 | 205 | if(!input_cloud->empty()) 206 | m_first_cloud = false; 207 | 208 | if(m_measure_time && !input_cloud->empty()) 209 | { 210 | std::chrono::microseconds time_for_one_callback = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - callback_start_time); 211 | ROS_DEBUG_STREAM("Time for segmenting one cloud with " << (int)input_cloud->size() << " points : " << time_for_one_callback.count() << " microseconds."); 212 | m_time_file << (double)time_for_one_callback.count()/1000.0 << std::endl; 213 | m_summed_time_for_callbacks += time_for_one_callback; 214 | m_number_of_callbacks++; 215 | ROS_DEBUG_STREAM("Mean time for segmenting one cloud: " << m_summed_time_for_callbacks.count() / m_number_of_callbacks << " microseconds."); 216 | } 217 | } 218 | 219 | void Segmenter::processScan(pcl::PCLHeader header) 220 | { 221 | Cloud4Detection::Ptr cloud4detection (new Cloud4Detection); 222 | cloud4detection->header = header; 223 | 224 | #pragma omp parallel for schedule(dynamic) num_threads(4) 225 | // counting backwards because for m600 setup scan rings with higher index are scanning the ground when m600 is standing -> these rings take more time during filtering because of shorter distances -> more efficient to filter these first when omp is on 226 | for(auto ring = (int)m_median_filtered_circ_buffer_vector.size() - 1; ring >= 0; --ring) 227 | { 228 | // initialize member iterators 229 | if(!m_median_filtered_circ_buffer_vector.at(ring)->empty() && !m_median_iters_by_ring[ring]) 230 | m_median_iters_by_ring[ring] = m_median_filtered_circ_buffer_vector.at(ring)->begin(); 231 | 232 | if(!m_median_filtered_circ_buffer_vector.at(ring)->empty() && !m_segmentation_iters_by_ring[ring]) 233 | m_segmentation_iters_by_ring[ring] = m_median_filtered_circ_buffer_vector.at(ring)->begin(); 234 | 235 | // apply median filters to ring 236 | if(m_median_iters_by_ring[ring]) 237 | filterRing(m_median_filtered_circ_buffer_vector.at(ring), *m_median_iters_by_ring.at(ring)); 238 | } 239 | 240 | if(m_is_organized_cloud) 241 | { 242 | // find min distance of segmentation iterators to median iterators per ring 243 | int min_iterator_distance = std::numeric_limits::max(); 244 | for(auto ring = 0; ring < (int)m_median_filtered_circ_buffer_vector.size(); ++ring) 245 | { 246 | int iterator_distance_for_ring = static_cast(std::distance(*m_segmentation_iters_by_ring.at(ring), *m_median_iters_by_ring.at(ring))); 247 | min_iterator_distance = std::min(min_iterator_distance, iterator_distance_for_ring); 248 | } 249 | 250 | // compute segments for each ring 251 | for(auto ring = 0; ring < (int)m_median_filtered_circ_buffer_vector.size(); ++ring) 252 | if(m_segmentation_iters_by_ring[ring]) 253 | segmentRing(*m_segmentation_iters_by_ring.at(ring), 254 | min_iterator_distance, 255 | cloud4detection); 256 | 257 | 258 | cloud4detection->height = (int)m_median_filtered_circ_buffer_vector.size(); 259 | cloud4detection->width = min_iterator_distance; 260 | cloud4detection->is_dense = false; 261 | } 262 | else 263 | { 264 | // compute segments for each ring 265 | for(auto ring = 0; ring < (int)m_median_filtered_circ_buffer_vector.size(); ++ring) 266 | if(m_segmentation_iters_by_ring[ring]) 267 | segmentRing(*m_segmentation_iters_by_ring.at(ring), 268 | *m_median_iters_by_ring.at(ring), 269 | cloud4detection); 270 | 271 | } 272 | 273 | if(m_pub_detection_cloud.getNumSubscribers() > 0) 274 | m_pub_detection_cloud.publish(cloud4detection); 275 | 276 | ROS_DEBUG_STREAM("cloud4detection cloud with " << (int)cloud4detection->size() << " points"); 277 | } 278 | 279 | void Segmenter::calcMedianFromBuffer(const int noise_filter_kernel_radius, 280 | const int object_filter_kernel_radius, 281 | const BufferMediansPtr& buffer, 282 | const median_const_iterator& current_element, 283 | std::function getValue, 284 | std::function isValid, 285 | const float invalid_value, 286 | float& noise_filter_result, 287 | float& object_filter_result) const 288 | { 289 | assert(std::distance(buffer->begin(), buffer->end()) > object_filter_kernel_radius * 2 + 1); 290 | 291 | median_const_iterator noise_kernel_start = current_element - noise_filter_kernel_radius; 292 | median_const_iterator noise_kernel_end = current_element + noise_filter_kernel_radius; 293 | median_const_iterator object_kernel_start = current_element - object_filter_kernel_radius; 294 | median_const_iterator object_kernel_end = current_element + object_filter_kernel_radius; 295 | long int noise_kernel_start_offset = -1; 296 | long int noise_kernel_end_offset = -1; 297 | 298 | // get distances of neighbors 299 | std::vector neighborhood_values; 300 | neighborhood_values.reserve(object_filter_kernel_radius * 2 + 1); 301 | 302 | // use all values in the specified neighborhood for the median computation 303 | if(!m_is_organized_cloud || m_use_all_neighbors_for_median) 304 | { 305 | median_const_iterator it_tmp = object_kernel_start; 306 | noise_kernel_start_offset = std::distance(object_kernel_start, noise_kernel_start); 307 | noise_kernel_end_offset = std::distance(object_kernel_start, noise_kernel_end); 308 | // use advance to cast const 309 | while(it_tmp <= object_kernel_end) 310 | neighborhood_values.push_back(getValue((*it_tmp++).point)); 311 | } 312 | // else use only valid points 313 | else 314 | { 315 | // we want to make sure that the same number of points left and right of the current are represented in the kernel 316 | std::vector start_values; 317 | start_values.reserve(object_filter_kernel_radius); 318 | int start_nan_counter = 0; 319 | 320 | std::vector end_values; 321 | end_values.reserve(object_filter_kernel_radius); 322 | int end_nan_counter = 0; 323 | 324 | // fill vectors starting from current element moving to begin for start_values and equivalently for end_values 325 | median_const_iterator start_iterator = object_kernel_start; 326 | while(std::distance(start_iterator, current_element) > 0) 327 | { 328 | if(isValid(getValue(start_iterator->point))) 329 | start_values.push_back(getValue(start_iterator->point)); 330 | else 331 | start_nan_counter++; 332 | 333 | start_iterator++; 334 | } 335 | 336 | median_const_iterator end_iterator = std::next(current_element); 337 | while(std::distance(end_iterator, object_kernel_end) >= 0) 338 | { 339 | if(isValid(getValue(end_iterator->point))) 340 | end_values.push_back(getValue(end_iterator->point)); 341 | else 342 | end_nan_counter++; 343 | 344 | end_iterator++; 345 | } 346 | 347 | // subtract max count from object_filter_kernel_radius 348 | int max_nan_counter = std::max(start_nan_counter, end_nan_counter); 349 | int adapted_object_filter_kernel_radius = object_filter_kernel_radius - max_nan_counter; 350 | 351 | // if new kernel_radius is smaller or eqaul to noise_filter_kernel_radius fill results with invalid values and return 352 | if(adapted_object_filter_kernel_radius <= noise_filter_kernel_radius) 353 | { 354 | noise_filter_result = invalid_value; 355 | object_filter_result = invalid_value; 356 | return; 357 | } 358 | // else fuse vectors and current element to neighborhood_values 359 | else 360 | { 361 | auto start_values_begin = start_values.end(); 362 | std::advance(start_values_begin, -adapted_object_filter_kernel_radius); 363 | neighborhood_values.insert(neighborhood_values.end(), start_values_begin, start_values.end()); 364 | neighborhood_values.push_back(getValue(current_element->point)); 365 | auto end_values_end = end_values.begin() + adapted_object_filter_kernel_radius; 366 | neighborhood_values.insert(neighborhood_values.end(), end_values.begin(), end_values_end); 367 | 368 | noise_kernel_start_offset = (int)std::distance(object_kernel_start, noise_kernel_start) - max_nan_counter; 369 | noise_kernel_end_offset = noise_kernel_start_offset + noise_filter_kernel_radius * 2; 370 | } 371 | } 372 | 373 | // get median of neighborhood distances within range of noise kernel 374 | long int noise_kernel_middle_offset = (noise_kernel_end_offset + noise_kernel_start_offset) / 2; 375 | std::nth_element(neighborhood_values.begin() + noise_kernel_start_offset, 376 | neighborhood_values.begin() + noise_kernel_middle_offset, 377 | neighborhood_values.begin() + noise_kernel_end_offset + 1); 378 | 379 | noise_filter_result = neighborhood_values[noise_kernel_middle_offset]; 380 | 381 | // get median of neighborhood distances within range of object kernel 382 | std::nth_element(neighborhood_values.begin(), neighborhood_values.begin() + neighborhood_values.size() / 2, neighborhood_values.end()); 383 | 384 | object_filter_result = neighborhood_values[neighborhood_values.size() / 2]; 385 | } 386 | 387 | void Segmenter::filterRing(std::shared_ptr > buffer_median_filtered, 388 | median_iterator& iter) 389 | { 390 | if(buffer_median_filtered->empty()) 391 | return; 392 | 393 | while(std::distance(iter, buffer_median_filtered->end()) > 1) 394 | { 395 | if(iter->point.distance > m_almost_max_range || iter->point.distance <= m_self_filter_radius) 396 | { 397 | iter->dist_noise_kernel = m_max_range; 398 | iter->dist_object_kernel = m_max_range; 399 | iter->intens_noise_kernel = -1.f; 400 | iter->intens_object_kernel = -1.f; 401 | ++iter; 402 | continue; 403 | } 404 | 405 | // compute how many points are approximately on the objects with the minimal specified width 406 | float alpha = static_cast(std::atan((m_min_object_width/2.f)/iter->point.distance) * (180.0 / M_PI)); 407 | float points_on_minimal_object = alpha * 2.f / m_angle_between_scanpoints; 408 | // the kernel size at which the target object gets filtered out is when there are more 409 | // non-object points than object points in the median filter kernel. To stay slightly below 410 | // this border we floor the result of the computation above and treat this result as the kernel 411 | // radius (half of the kernel size). This way only noise and objects smaller than the target 412 | // objects get filtered out. 413 | int noise_filter_kernel_radius = (int)std::floor(points_on_minimal_object); 414 | 415 | noise_filter_kernel_radius = std::max(noise_filter_kernel_radius, 0); 416 | noise_filter_kernel_radius = std::min(noise_filter_kernel_radius, m_max_kernel_size / 2 - 1); 417 | 418 | // do as for noise kernel but instead of floor do ceil 419 | alpha = static_cast(std::atan((m_max_object_width/2.f)/iter->point.distance) * (180.0 / M_PI)); 420 | float points_on_maximal_object = alpha * 2.f / m_angle_between_scanpoints; 421 | 422 | int object_filter_kernel_radius = (int)std::ceil(points_on_maximal_object); 423 | object_filter_kernel_radius = std::max(object_filter_kernel_radius, 1); 424 | object_filter_kernel_radius = std::min(object_filter_kernel_radius, m_max_kernel_size / 2); 425 | 426 | // if current element has enough neighbors to both sides, compute medians 427 | if(std::distance(buffer_median_filtered->begin(), iter) >= object_filter_kernel_radius && 428 | std::distance(iter, buffer_median_filtered->end()) > object_filter_kernel_radius) 429 | { 430 | if(m_dist_weight != 0.f) 431 | { 432 | calcMedianFromBuffer(noise_filter_kernel_radius, 433 | object_filter_kernel_radius, 434 | buffer_median_filtered, 435 | median_const_iterator(iter), 436 | [&](const InputPoint &fn) -> float { return fn.distance; }, 437 | [&](const float value) -> float { return value < m_almost_max_range; }, 438 | m_max_range, 439 | iter->dist_noise_kernel, 440 | iter->dist_object_kernel); 441 | } 442 | else 443 | iter->dist_noise_kernel = iter->dist_object_kernel = m_max_range; 444 | 445 | if(m_intensity_weight != 0.f) 446 | { 447 | calcMedianFromBuffer(noise_filter_kernel_radius, 448 | object_filter_kernel_radius, 449 | buffer_median_filtered, 450 | median_const_iterator(iter), 451 | [&](const InputPoint &fn) -> float { return fn.intensity; }, 452 | [&](const float value) -> float { return value >= 0.f; }, 453 | -1.f, 454 | iter->intens_noise_kernel, 455 | iter->intens_object_kernel); 456 | } 457 | else 458 | iter->intens_noise_kernel = iter->intens_object_kernel = -1.f; 459 | } 460 | 461 | // if there are not enough neighboring points left in the circular to filter -> break 462 | if(std::distance(iter, buffer_median_filtered->end()) <= object_filter_kernel_radius) 463 | break; 464 | 465 | ++iter; 466 | } 467 | } 468 | 469 | float Segmenter::computeSegmentationProbability(float delta, bool do_weighting) 470 | { 471 | if(do_weighting) 472 | { 473 | // cap absolute difference to 0 - m_max_intensity_range 474 | // and do some kind of weighting, bigger weight -> bigger weight for smaller intensity differences 475 | delta = std::max(0.f, delta); 476 | delta = std::min(delta, m_max_intensity_range/m_weight_for_small_intensities); 477 | delta *= m_weight_for_small_intensities; 478 | } 479 | 480 | if(delta >= m_delta_min && delta < m_delta_low) 481 | return (m_delta_min - delta) / (m_delta_min - m_delta_low); 482 | 483 | if(delta >= m_delta_low && delta < m_delta_high) 484 | return 1.f; 485 | 486 | if(delta >= m_delta_high && delta < m_delta_max) 487 | return (m_delta_max - delta) / (m_delta_max - m_delta_high); 488 | 489 | // if delta out of min-max-bounds 490 | return 0.f; 491 | } 492 | 493 | void Segmenter::segmentRing(median_iterator& median_it, 494 | int steps, 495 | Cloud4Detection::Ptr& cloud4detection) 496 | { 497 | median_iterator end = median_it; 498 | std::advance(end, steps); 499 | segmentRing(median_it, 500 | end, 501 | cloud4detection); 502 | } 503 | 504 | void Segmenter::segmentRing(median_iterator& median_it, 505 | median_iterator& end, 506 | Cloud4Detection::Ptr& cloud4detection) 507 | { 508 | for(; median_it != end; ++median_it) 509 | { 510 | // compute differences and resulting certainty value 511 | float certainty_value = 0.f; 512 | float distance_delta = 0.f; 513 | if(m_dist_weight > 0.f) 514 | { 515 | distance_delta = (*median_it).dist_object_kernel - (*median_it).dist_noise_kernel; 516 | certainty_value += computeSegmentationProbability(distance_delta) * m_dist_weight; 517 | } 518 | 519 | float intensity_delta = 0.f; 520 | if(certainty_value > 0.f && m_intensity_weight > 0.f) 521 | { 522 | // use absolute distance here because we are just searching for objects that are different to their background 523 | intensity_delta = fabsf((*median_it).intens_noise_kernel - (*median_it).intens_object_kernel); 524 | certainty_value += computeSegmentationProbability(intensity_delta, true) * m_intensity_weight; 525 | } 526 | 527 | certainty_value = std::min(certainty_value, 1.0f); 528 | certainty_value = std::max(certainty_value, 0.0f); 529 | 530 | const auto& current_point = (*median_it).point; 531 | 532 | if(m_pub_detection_cloud.getNumSubscribers() > 0) 533 | { 534 | Point4Detection point4detection; 535 | if(current_point.distance > m_self_filter_radius) 536 | { 537 | point4detection.x = current_point.x; 538 | point4detection.y = current_point.y; 539 | point4detection.z = current_point.z; 540 | point4detection.segmentation = (certainty_value < m_certainty_threshold) ? 0 : 1; 541 | } 542 | else 543 | { 544 | point4detection.x = std::nanf(""); 545 | point4detection.y = std::nanf(""); 546 | point4detection.z = std::nanf(""); 547 | point4detection.segmentation = 0; 548 | } 549 | point4detection.ring = current_point.ring; 550 | if(m_is_organized_cloud || std::isfinite(point4detection.x)) 551 | cloud4detection->push_back(point4detection); 552 | 553 | if(current_point.distance > m_almost_max_range && point4detection.segmentation > 0) 554 | ROS_ERROR("\nPoint with max range is a segment! This shouldn't happen.\n"); 555 | } 556 | } 557 | } 558 | 559 | } // namespace laser_segmentation 560 | -------------------------------------------------------------------------------- /object_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(object_detection) 3 | 4 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | tf 9 | pcl_ros 10 | tf_conversions 11 | pcl_conversions 12 | eigen_conversions 13 | laser_segmentation 14 | nodelet 15 | multi_hypothesis_tracking_msgs 16 | geometry_msgs 17 | visualization_msgs 18 | ) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | ) 23 | 24 | include_directories( 25 | include 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11") 30 | 31 | add_library(detector 32 | src/detector.cpp 33 | ) 34 | 35 | add_dependencies(detector 36 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 37 | ${catkin_EXPORTED_TARGETS} 38 | ) 39 | 40 | target_link_libraries(detector 41 | ${catkin_LIBRARIES} 42 | ) 43 | 44 | ## Declare a C++ executable 45 | add_executable(object_detection_node 46 | src/object_detection_node.cpp 47 | ) 48 | 49 | ## Add cmake target dependencies of the executable 50 | add_dependencies(object_detection_node 51 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 52 | ${catkin_EXPORTED_TARGETS} 53 | ) 54 | 55 | ## Specify libraries to link a library or executable target against 56 | target_link_libraries(object_detection_node 57 | detector 58 | ${catkin_LIBRARIES} 59 | ) 60 | 61 | install(TARGETS object_detection_node 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 63 | 64 | ## Declare a C++ library 65 | add_library(object_detection_nodelet 66 | src/object_detection_nodelet.cpp 67 | ) 68 | 69 | add_dependencies(object_detection_nodelet 70 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 71 | ${catkin_EXPORTED_TARGETS} 72 | ) 73 | 74 | target_link_libraries(object_detection_nodelet 75 | detector 76 | ${catkin_LIBRARIES} 77 | ) 78 | 79 | install(TARGETS object_detection_nodelet 80 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 81 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 83 | ) 84 | 85 | ## Install project namespaced headers 86 | install(DIRECTORY include/${PROJECT_NAME}/ 87 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 88 | ) 89 | -------------------------------------------------------------------------------- /object_detection/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Jan Razlaw 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /object_detection/include/object_detection/detector.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * 3 | * This class detects potential objects in point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #ifndef DETECTOR_H 9 | #define DETECTOR_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | namespace object_detection 44 | { 45 | 46 | /** 47 | * Class to detect objects in 3D point clouds. 48 | */ 49 | 50 | class Detector 51 | { 52 | public: 53 | 54 | typedef laser_segmentation::PointXYZSegmentation InputPoint; 55 | typedef pcl::PointCloud InputPointCloud; 56 | 57 | /** 58 | * @brief Constructor. 59 | * 60 | * Parameters are needed to use this class within a ros node or nodelet. 61 | */ 62 | Detector(ros::NodeHandle node, ros::NodeHandle private_nh); 63 | virtual ~Detector(); 64 | 65 | private: 66 | 67 | /** 68 | * @brief Tries to get the transform from from to to. 69 | * 70 | * @param[in] to frame to transform to. 71 | * @param[in] from frame to transform from. 72 | * @param[in] when time stamp of transformation. 73 | * @param[out] transform resulting transform. 74 | * 75 | * @return true if transform successfully found, false otherwise 76 | */ 77 | bool tryGetTransform(const std::string& to, 78 | const std::string& from, 79 | const ros::Time& when, 80 | Eigen::Affine3f& transform); 81 | 82 | /** 83 | * @brief Transforms the cloud into the target_frame. 84 | * 85 | * @param[in,out] cloud the cloud that is transformed. 86 | * @param[in] target_frame the frame the cloud should be transformed to. 87 | * 88 | * @return true if transform was successful, false otherwise 89 | */ 90 | bool transformCloud(InputPointCloud::Ptr& cloud, std::string target_frame); 91 | 92 | /** 93 | * @brief Performs euclidean clustering on the points in cloud. 94 | * 95 | * Each cluster is represented by point indices that are stored in the entries 96 | * of the cluster_indices vector. 97 | * 98 | * @param[in] cloud input cloud. 99 | * @param[out] cluster_indices vector of clusters - each cluster represented 100 | * by several point indices. 101 | * @param[in] cluster_tolerance distance threshold for euclidean clustering. 102 | * @param[in] min_cluster_size minimal number of points a cluster has to have. 103 | * @param[in] max_cluster_size maximal number of points a cluster is allowed to have. 104 | * 105 | * @return true if clustering was successful, false otherwise 106 | */ 107 | bool euclideanClustering(pcl::PointCloud::Ptr cloud, 108 | std::vector& cluster_indices, 109 | float cluster_tolerance, 110 | int min_cluster_size, 111 | int max_cluster_size); 112 | 113 | /** 114 | * @brief Computes the mean point and the size of a cluster. 115 | * 116 | * @param[in] cloud input cloud. 117 | * @param[in] point_indices indices of the cluster's points. 118 | * @param[out] mean mean point of the cluster's points. 119 | * @param[out] min minimum in x, y and z direction of all points. 120 | * @param[out] max maximum in x, y and z direction of all points. 121 | */ 122 | void getClusterProperties(const InputPointCloud::Ptr cloud, 123 | const pcl::PointIndices& point_indices, 124 | Eigen::Vector3f& mean, 125 | Eigen::Array3f& min, 126 | Eigen::Array3f& max, 127 | bool& peripheral); 128 | 129 | /** 130 | * @brief Save currently tracked hypotheses. 131 | * 132 | * @param[in] hypotheses hypotheses from tracking. 133 | */ 134 | void handleTracks(const multi_hypothesis_tracking_msgs::HypothesesFull::ConstPtr& hypotheses); 135 | 136 | /** 137 | * @brief Detect all objects in a cloud that fit the description. 138 | * 139 | * First filter out all background points according to segmentation value in each point. 140 | * Then cluster the remaining points by distance. 141 | * Then filter clusters by description. 142 | * 143 | * @param[in] segmentation input cloud with segmentation values for each point. 144 | */ 145 | void handleCloud(const InputPointCloud::ConstPtr& segmentation); 146 | 147 | /** 148 | * @brief Publishes empty messages in case no detection was made. 149 | * 150 | * This is needed for a tracking that should know that there are no detections for 151 | * a specific time. 152 | * 153 | * @param[in] header header for empty messages. 154 | */ 155 | void publishEmpty(const pcl::PCLHeader& header); 156 | 157 | tf::TransformListener m_tf; 158 | 159 | ros::Subscriber m_sub_cloud; 160 | ros::Subscriber m_sub_tracks; 161 | ros::Publisher m_pub_detections; 162 | ros::Publisher m_pub_vis_marker; 163 | ros::Publisher m_pub_clustered_cloud; 164 | 165 | float m_min_certainty_thresh; 166 | 167 | float m_cluster_tolerance; 168 | int m_min_cluster_size; 169 | int m_max_cluster_size; 170 | 171 | float m_min_object_height; 172 | float m_max_object_height; 173 | float m_max_object_width; 174 | float m_max_object_altitude; 175 | 176 | std::string m_fixed_frame; 177 | std::string m_sensor_frame; 178 | 179 | multi_hypothesis_tracking_msgs::HypothesesFull m_current_hypotheses; 180 | std::mutex m_hypotheses_mutex; 181 | 182 | float m_amplify_threshold; 183 | 184 | int m_region_growing_radius; 185 | float m_region_growing_distance_threshold; 186 | 187 | bool m_measure_time; 188 | std::chrono::microseconds m_summed_time_for_callbacks; 189 | int m_number_of_callbacks; 190 | std::ofstream m_time_file; 191 | }; 192 | 193 | } 194 | 195 | #endif 196 | -------------------------------------------------------------------------------- /object_detection/launch/object_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /object_detection/launch/visualize_on_simulated_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /object_detection/launch/visualize_on_simulated_data.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5402542352676392 9 | Tree Height: 1002 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Segmented Cloud 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 52 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/TF 55 | Enabled: true 56 | Frame Timeout: 15 57 | Frames: 58 | All Enabled: true 59 | base_footprint: 60 | Value: true 61 | base_link: 62 | Value: true 63 | velodyne: 64 | Value: true 65 | velodyne_base_link: 66 | Value: true 67 | world: 68 | Value: true 69 | Marker Scale: 1 70 | Name: TF 71 | Show Arrows: true 72 | Show Axes: true 73 | Show Names: true 74 | Tree: 75 | world: 76 | base_footprint: 77 | base_link: 78 | velodyne_base_link: 79 | velodyne: 80 | {} 81 | Update Interval: 0 82 | Value: true 83 | - Alpha: 1 84 | Autocompute Intensity Bounds: true 85 | Autocompute Value Bounds: 86 | Max Value: -50.08469009399414 87 | Min Value: -76.30184173583984 88 | Value: true 89 | Axis: Z 90 | Channel Name: intensity 91 | Class: rviz/PointCloud2 92 | Color: 255; 255; 255 93 | Color Transformer: Intensity 94 | Decay Time: 0 95 | Enabled: false 96 | Invert Rainbow: false 97 | Max Color: 0; 0; 0 98 | Max Intensity: 999999 99 | Min Color: 255; 255; 255 100 | Min Intensity: 6.602756934315448e-38 101 | Name: Input Point Cloud 102 | Position Transformer: XYZ 103 | Queue Size: 10 104 | Selectable: true 105 | Size (Pixels): 5 106 | Size (m): 0.05000000074505806 107 | Style: Points 108 | Topic: /velodyne_points 109 | Unreliable: false 110 | Use Fixed Frame: true 111 | Use rainbow: true 112 | Value: false 113 | - Alpha: 1 114 | Autocompute Intensity Bounds: true 115 | Autocompute Value Bounds: 116 | Max Value: 10 117 | Min Value: -10 118 | Value: true 119 | Axis: Z 120 | Channel Name: segmentation 121 | Class: rviz/PointCloud2 122 | Color: 255; 255; 255 123 | Color Transformer: Intensity 124 | Decay Time: 0 125 | Enabled: true 126 | Invert Rainbow: false 127 | Max Color: 255; 255; 255 128 | Max Intensity: 1 129 | Min Color: 0; 0; 0 130 | Min Intensity: 0 131 | Name: Segmented Cloud 132 | Position Transformer: XYZ 133 | Queue Size: 10 134 | Selectable: true 135 | Size (Pixels): 5 136 | Size (m): 0.009999999776482582 137 | Style: Points 138 | Topic: /laser_segments4detection 139 | Unreliable: false 140 | Use Fixed Frame: true 141 | Use rainbow: true 142 | Value: true 143 | - Class: rviz/MarkerArray 144 | Enabled: true 145 | Marker Topic: /object_detection_markers 146 | Name: Object Detections 147 | Namespaces: 148 | {} 149 | Queue Size: 100 150 | Value: true 151 | - Class: rviz/Marker 152 | Enabled: true 153 | Marker Topic: /multi_hypothesis_tracking_node_for_sparse_lidar/hypotheses_paths 154 | Name: Hypotheses Paths 155 | Namespaces: 156 | mot_hypotheses_paths: true 157 | Queue Size: 100 158 | Value: true 159 | - Class: rviz/MarkerArray 160 | Enabled: true 161 | Marker Topic: /multi_hypothesis_tracking_node_for_sparse_lidar/hypotheses_bounding_boxes 162 | Name: Hypotheses Boxes 163 | Namespaces: 164 | {} 165 | Queue Size: 100 166 | Value: true 167 | Enabled: true 168 | Global Options: 169 | Background Color: 235; 235; 235 170 | Default Light: true 171 | Fixed Frame: world 172 | Frame Rate: 30 173 | Name: root 174 | Tools: 175 | - Class: rviz/Interact 176 | Hide Inactive Objects: true 177 | - Class: rviz/MoveCamera 178 | - Class: rviz/Select 179 | - Class: rviz/FocusCamera 180 | - Class: rviz/Measure 181 | - Class: rviz/SetInitialPose 182 | Theta std deviation: 0.2617993950843811 183 | Topic: /initialpose 184 | X std deviation: 0.5 185 | Y std deviation: 0.5 186 | - Class: rviz/SetGoal 187 | Topic: /move_base_simple/goal 188 | - Class: rviz/PublishPoint 189 | Single click: true 190 | Topic: /clicked_point 191 | Value: true 192 | Views: 193 | Current: 194 | Class: rviz/Orbit 195 | Distance: 61.57426452636719 196 | Enable Stereo Rendering: 197 | Stereo Eye Separation: 0.05999999865889549 198 | Stereo Focal Distance: 1 199 | Swap Stereo Eyes: false 200 | Value: false 201 | Focal Point: 202 | X: 2.631563186645508 203 | Y: -9.199975967407227 204 | Z: 1.787619709968567 205 | Focal Shape Fixed Size: false 206 | Focal Shape Size: 0.05000000074505806 207 | Invert Z Axis: false 208 | Name: Current View 209 | Near Clip Distance: 0.009999999776482582 210 | Pitch: 0.8747978210449219 211 | Target Frame: os_sensor 212 | Value: Orbit (rviz) 213 | Yaw: 5.208599090576172 214 | Saved: ~ 215 | Window Geometry: 216 | "&Displays": 217 | collapsed: false 218 | "&Time": 219 | collapsed: false 220 | Height: 1373 221 | Hide Left Dock: false 222 | Hide Right Dock: false 223 | QMainWindow State: 000000ff00000000fd0000000400000000000001e000000491fc020000000efb0000001200530065006c0065006300740069006f006e000000004c000001f20000007601000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000005a000000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c00000491000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000024e000001b30000000000000000fb0000000a0049006d00610067006501000002800000012e0000000000000000fb0000000a0049006d0061006700650100000345000001980000000000000000fb0000000a0049006d00610067006500000003bd0000013e0000000000000000fb0000000a0049006d00610067006500000003b10000014a0000000000000000fb0000000a0049006d006100670065000000039d0000015e000000000000000000000001000001b2000004affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004c000004af000000dc01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000052fc0100000002fb0000000800540069006d0065010000000000000a000000034a01000003fb0000000800540069006d006501000000000000045000000000000000000000081f0000049100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Selection: 225 | collapsed: false 226 | Tool Properties: 227 | collapsed: false 228 | Views: 229 | collapsed: false 230 | Width: 2560 231 | X: 0 232 | Y: 0 233 | -------------------------------------------------------------------------------- /object_detection/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Detect potential objects in point clouds 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /object_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | object_detection 3 | Detect potential objects in point clouds 4 | 0.0.1 5 | BSD 6 | 7 | Jan Razlaw 8 | 9 | catkin 10 | laser_segmentation 11 | pcl_ros 12 | nodelet 13 | multi_hypothesis_tracking_msgs 14 | geometry_msgs 15 | visualization_msgs 16 | pcl_conversions 17 | tf_conversions 18 | eigen_conversions 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /object_detection/src/detector.cpp: -------------------------------------------------------------------------------- 1 | /** @file 2 | * 3 | * This class detects potential objects in point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #include 9 | 10 | namespace object_detection 11 | { 12 | 13 | Detector::Detector(ros::NodeHandle node, ros::NodeHandle private_nh) 14 | : m_min_certainty_thresh(0.5f) 15 | , m_cluster_tolerance(1.f) 16 | , m_min_cluster_size(4) 17 | , m_max_cluster_size(25000) 18 | , m_min_object_height(0.5f) 19 | , m_max_object_height(1.8f) 20 | , m_max_object_width(2.f) 21 | , m_max_object_altitude(2.f) 22 | , m_fixed_frame("world") 23 | , m_sensor_frame("velodyne") 24 | , m_amplify_threshold(1.f) 25 | , m_region_growing_radius(1) 26 | , m_region_growing_distance_threshold(1.f) 27 | , m_measure_time(false) 28 | , m_number_of_callbacks(0) 29 | { 30 | ROS_INFO("Object_detector: Init..."); 31 | 32 | private_nh.getParam("certainty_thresh", m_min_certainty_thresh); 33 | 34 | private_nh.getParam("cluster_tolerance", m_cluster_tolerance); 35 | private_nh.getParam("min_cluster_size", m_min_cluster_size); 36 | private_nh.getParam("max_cluster_size", m_max_cluster_size); 37 | 38 | private_nh.getParam("min_object_height", m_min_object_height); 39 | private_nh.getParam("max_object_height", m_max_object_height); 40 | private_nh.getParam("max_object_width", m_max_object_width); 41 | private_nh.getParam("max_object_altitude", m_max_object_altitude); 42 | 43 | private_nh.getParam("fixed_frame", m_fixed_frame); 44 | private_nh.getParam("sensor_frame", m_sensor_frame); 45 | private_nh.getParam("amplify_threshold", m_amplify_threshold); 46 | private_nh.getParam("region_growing_radius", m_region_growing_radius); 47 | private_nh.getParam("region_growing_distance_threshold", m_region_growing_distance_threshold); 48 | private_nh.getParam("measure_time", m_measure_time); 49 | if(m_measure_time) 50 | { 51 | std::string path_to_results_file = "/tmp/times_object_detection"; 52 | m_time_file.open(path_to_results_file); 53 | } 54 | m_summed_time_for_callbacks = std::chrono::microseconds::zero(); 55 | 56 | m_pub_detections = node.advertise("object_detections", 1); 57 | m_pub_vis_marker = node.advertise("object_detection_markers", 1); 58 | m_pub_clustered_cloud = node.advertise("object_detection_cloud", 1); 59 | 60 | std::string tracks_topic = "/multi_object_tracking/hypotheses_full"; 61 | private_nh.getParam("tracks_topic", tracks_topic); 62 | m_sub_tracks = node.subscribe(tracks_topic, 1, &Detector::handleTracks, this); 63 | 64 | std::string segments_topic = "/laser_segmenter_objects"; 65 | private_nh.getParam("segments_topic", segments_topic); 66 | m_sub_cloud = node.subscribe(segments_topic, 1, &Detector::handleCloud, this); 67 | } 68 | 69 | Detector::~Detector() 70 | { 71 | m_time_file.close(); 72 | } 73 | 74 | bool Detector::tryGetTransform(const std::string& to, 75 | const std::string& from, 76 | const ros::Time& when, 77 | Eigen::Affine3f& transform) 78 | { 79 | if(!m_tf.waitForTransform(to, from, when, ros::Duration(0.5))) 80 | { 81 | ROS_ERROR_STREAM("Could not wait for transform from " << from << " to " << to); 82 | return false; 83 | } 84 | 85 | tf::StampedTransform transformTF; 86 | try 87 | { 88 | m_tf.lookupTransform(to, from, when, transformTF); 89 | } 90 | catch(tf::TransformException& e) 91 | { 92 | ROS_ERROR("Could not lookup transform to frame: '%s'", e.what()); 93 | return false; 94 | } 95 | 96 | Eigen::Affine3d transform_double; 97 | tf::transformTFToEigen(transformTF, transform_double); 98 | transform = transform_double.cast(); 99 | return true; 100 | } 101 | 102 | bool Detector::transformCloud(InputPointCloud::Ptr& cloud, std::string target_frame) 103 | { 104 | if(cloud->header.frame_id == target_frame) 105 | return true; 106 | 107 | ros::Time stamp = pcl_conversions::fromPCL(cloud->header.stamp); 108 | 109 | Eigen::Affine3f transform; 110 | if(!tryGetTransform(target_frame, cloud->header.frame_id, stamp, transform)) 111 | return false; 112 | 113 | InputPointCloud::Ptr cloud_transformed(new InputPointCloud); 114 | pcl::transformPointCloud(*cloud, *cloud_transformed, transform); 115 | cloud_transformed->header = cloud->header; 116 | cloud_transformed->header.frame_id = target_frame; 117 | 118 | cloud.swap(cloud_transformed); 119 | 120 | return true; 121 | } 122 | 123 | bool Detector::euclideanClustering(pcl::PointCloud::Ptr cloud, 124 | std::vector& cluster_indices, 125 | float cluster_tolerance, 126 | int min_cluster_size, 127 | int max_cluster_size) 128 | { 129 | try 130 | { 131 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); 132 | tree->setInputCloud(cloud); 133 | 134 | pcl::EuclideanClusterExtraction ec; 135 | ec.setClusterTolerance(cluster_tolerance); // in m 136 | ec.setMinClusterSize(min_cluster_size); 137 | ec.setMaxClusterSize(max_cluster_size); 138 | ec.setSearchMethod(tree); 139 | ec.setInputCloud(cloud); 140 | ec.extract(cluster_indices); 141 | } 142 | catch(...) 143 | { 144 | ROS_ERROR("Object_detection: Clustering failed."); 145 | return false; 146 | } 147 | 148 | ROS_DEBUG_STREAM("Object_detector: Number of found clusters is " << cluster_indices.size() << ". "); 149 | return true; 150 | } 151 | 152 | void Detector::getClusterProperties(const InputPointCloud::Ptr cloud, 153 | const pcl::PointIndices& point_indices, 154 | Eigen::Vector3f& mean, 155 | Eigen::Array3f& min, 156 | Eigen::Array3f& max, 157 | bool& peripheral) 158 | { 159 | mean = Eigen::Vector3f::Zero(); 160 | min = Eigen::Array3f::Constant(std::numeric_limits::max()); 161 | max = Eigen::Array3f::Constant(-std::numeric_limits::max()); 162 | 163 | for(const auto& idx : point_indices.indices) 164 | { 165 | if(!peripheral && ((*cloud)[idx].ring == 0 || (*cloud)[idx].ring == 15)) 166 | peripheral = true; 167 | 168 | auto pos = (*cloud)[idx].getVector3fMap(); 169 | mean += pos; 170 | 171 | for(std::size_t i = 0; i < 3; ++i) 172 | { 173 | min[i] = std::min(min[i], pos[i]); 174 | max[i] = std::max(max[i], pos[i]); 175 | } 176 | } 177 | 178 | mean /= point_indices.indices.size(); 179 | } 180 | 181 | void Detector::handleTracks(const multi_hypothesis_tracking_msgs::HypothesesFull::ConstPtr& hypotheses) 182 | { 183 | std::lock_guard guard(m_hypotheses_mutex); 184 | m_current_hypotheses = *hypotheses; 185 | } 186 | 187 | float getXYAngle(const Eigen::Vector3f& a, const Eigen::Vector3f& b) 188 | { 189 | return std::atan2(a.x()*b.y()-a.y()*b.x(), a.x()*b.x()+a.y()*b.y()); 190 | } 191 | 192 | void Detector::handleCloud(const InputPointCloud::ConstPtr& input_cloud) 193 | { 194 | // start when subscribers are available 195 | if(m_pub_vis_marker.getNumSubscribers() == 0 && 196 | m_pub_detections.getNumSubscribers() == 0 && 197 | m_pub_clustered_cloud.getNumSubscribers() == 0) 198 | { 199 | ROS_DEBUG_STREAM("Object_detector: No subscriber."); 200 | return; 201 | } 202 | 203 | auto callback_start_time = std::chrono::high_resolution_clock::now(); 204 | 205 | if(input_cloud->empty()) 206 | { 207 | ROS_DEBUG("Object_detection: Received empty cloud."); 208 | publishEmpty(input_cloud->header); 209 | return; 210 | } 211 | 212 | InputPointCloud::Ptr segments_cloud(new InputPointCloud); 213 | std::vector cluster_indices; 214 | 215 | std_msgs::Header header = pcl_conversions::fromPCL(input_cloud->header); 216 | // if cloud organized 217 | bool cloud_is_organized = input_cloud->height > 1; 218 | 219 | if(cloud_is_organized) 220 | { 221 | // get transform to sensor frame - needed for overlap estimation 222 | Eigen::Affine3f transform; 223 | if(!tryGetTransform(m_sensor_frame, header.frame_id , header.stamp, transform)) 224 | return; 225 | 226 | pcl::copyPointCloud(*input_cloud, *segments_cloud); 227 | segments_cloud->is_dense = false; // needed to keep cloud organized through pcl::transformPointCloud later on 228 | 229 | // test if start and end of scan overlap 230 | float angle_between_ring_start_and_end = 0.f; 231 | for(int i = 0; i < 16; i++) 232 | { 233 | Eigen::Vector3f first_point_in_ring = segments_cloud->points[i * segments_cloud->width].getVector3fMap(); 234 | Eigen::Vector3f last_point_in_ring = segments_cloud->points[(i + 1) * segments_cloud->width - 1].getVector3fMap(); 235 | 236 | if(std::isfinite(first_point_in_ring.x()) && std::isfinite(last_point_in_ring.x())) 237 | { 238 | // transform points to sensor frame to be able to apply getXYAngle function correctly 239 | first_point_in_ring = transform * first_point_in_ring; 240 | last_point_in_ring = transform * last_point_in_ring; 241 | 242 | angle_between_ring_start_and_end = getXYAngle(first_point_in_ring, last_point_in_ring) * static_cast(180.0 / M_PI); 243 | break; 244 | } 245 | 246 | ROS_ERROR("Couldn't estimate the overlap of the scan ends."); 247 | } 248 | 249 | // region growing to extract segments 250 | uint8_t cluster_id = 2; 251 | for(int col = 0; col < (int)segments_cloud->width; col++) 252 | { 253 | for(int row = 0; row < (int)segments_cloud->height; row++) 254 | { 255 | int point_index = col + segments_cloud->width * row; 256 | // if segment and not visited yet, start region growing 257 | if(segments_cloud->points[point_index].segmentation == 1) 258 | { 259 | pcl::PointIndices current_cluster_indices; 260 | std::queue points_to_check; 261 | points_to_check.push(point_index); 262 | segments_cloud->points[point_index].segmentation = cluster_id; 263 | current_cluster_indices.indices.push_back(point_index); 264 | 265 | while(!points_to_check.empty()) 266 | { 267 | int current_index = points_to_check.front(); 268 | int current_col = current_index % (int)segments_cloud->width; 269 | int current_row = current_index / (int)segments_cloud->width; 270 | points_to_check.pop(); 271 | 272 | // compute indices of neighbors that need to be checked wrt. radius 273 | int window_col_min = std::max(0, current_col - m_region_growing_radius); 274 | int window_col_max = std::min((int)segments_cloud->width - 1, current_col + m_region_growing_radius); 275 | int window_row_min = std::max(0, current_row - m_region_growing_radius); 276 | int window_row_max = std::min((int)segments_cloud->height - 1, current_row + m_region_growing_radius); 277 | for(int search_col = window_col_min; search_col <= window_col_max; search_col++) 278 | { 279 | for(int search_row = window_row_min; search_row <= window_row_max; search_row++) 280 | { 281 | int dist_to_current = abs(search_col - current_col) + abs(search_row - current_row); 282 | if(dist_to_current > m_region_growing_radius) 283 | continue; 284 | 285 | int neighbor_index = search_col + segments_cloud->width * search_row; 286 | if(segments_cloud->points[neighbor_index].segmentation == 1) 287 | { 288 | // if one object occludes another their points could be neighbors in the ordered cloud 289 | // comparing their distances to the sensor is a simple way to keep them apart 290 | Eigen::Vector3f current_point = segments_cloud->points[neighbor_index].getVector3fMap(); 291 | Eigen::Vector3f neighbor = segments_cloud->points[current_index].getVector3fMap(); 292 | current_point = transform * current_point; 293 | neighbor = transform * neighbor; 294 | 295 | float distance = fabsf(current_point.norm() - neighbor.norm()); 296 | if(distance > m_region_growing_distance_threshold) 297 | continue; 298 | 299 | points_to_check.push(neighbor_index); 300 | segments_cloud->points[neighbor_index].segmentation = cluster_id; 301 | current_cluster_indices.indices.push_back(neighbor_index); 302 | } 303 | } 304 | } 305 | } 306 | 307 | if((int)current_cluster_indices.indices.size() >= m_min_cluster_size && 308 | (int)current_cluster_indices.indices.size() <= m_max_cluster_size) 309 | { 310 | cluster_indices.push_back(current_cluster_indices); 311 | } 312 | 313 | cluster_id++; 314 | 315 | if(cluster_id == UINT8_MAX) 316 | cluster_id = 2; // the exact cluster_id is for visualization purposes only. what matters is it being >1 317 | 318 | } 319 | } 320 | } 321 | 322 | 323 | // if start and end overlap, merge clusters that represent the same object in the real world 324 | if(angle_between_ring_start_and_end <= 0.f) 325 | { 326 | // convert the overlapping angle into the number of points that are in the overlapping region on one side of the ring 327 | const float absolute_angle_between_ring_start_and_end = fabsf(angle_between_ring_start_and_end); 328 | float degrees_per_point = (360.f + absolute_angle_between_ring_start_and_end) / (float)segments_cloud->width; 329 | int overlap_in_number_of_points = (int)std::ceil(absolute_angle_between_ring_start_and_end / degrees_per_point); 330 | int min_bound = overlap_in_number_of_points; 331 | int max_bound = (int)segments_cloud->width - overlap_in_number_of_points; 332 | 333 | int points_without_overlap = (int)segments_cloud->width - overlap_in_number_of_points; 334 | 335 | // find all clusters that could be in the overlapping region 336 | std::vector horizontally_peripheral_clusters; 337 | for(size_t cluster_index = 0; cluster_index < cluster_indices.size(); cluster_index++) 338 | { 339 | for(size_t point_index = 0; point_index < cluster_indices[cluster_index].indices.size(); point_index++) 340 | { 341 | int current_col = cluster_indices[cluster_index].indices[point_index] % (int)segments_cloud->width; 342 | if(current_col <= min_bound || current_col >= max_bound) 343 | { 344 | horizontally_peripheral_clusters.push_back(cluster_index); 345 | break; 346 | } 347 | } 348 | } 349 | 350 | // for those clusters compute min and max indices ~ bounding boxes 351 | std::vector> min_max_col_indices; 352 | std::vector> min_max_row_indices; 353 | for(size_t i = 0; i < horizontally_peripheral_clusters.size(); i++) 354 | { 355 | size_t cluster_index = horizontally_peripheral_clusters[i]; 356 | int min_col_index = std::numeric_limits::max(); 357 | int max_col_index = -std::numeric_limits::max(); 358 | int min_row_index = std::numeric_limits::max(); 359 | int max_row_index = -std::numeric_limits::max(); 360 | for(size_t point_index = 0; point_index < cluster_indices[cluster_index].indices.size(); point_index++) 361 | { 362 | int current_col = cluster_indices[cluster_index].indices[point_index] % (int)segments_cloud->width; 363 | min_col_index = std::min(min_col_index, current_col); 364 | max_col_index = std::max(max_col_index, current_col); 365 | int current_row = cluster_indices[cluster_index].indices[point_index] / (int)segments_cloud->width; 366 | min_row_index = std::min(min_row_index, current_row); 367 | max_row_index = std::max(max_row_index, current_row); 368 | } 369 | // "modulo" on those that lay in the overlapping area at the end of a scan 370 | if(max_col_index >= points_without_overlap) 371 | { 372 | max_col_index -= points_without_overlap; 373 | min_col_index -= points_without_overlap; 374 | min_col_index = std::max(min_col_index, 0); 375 | } 376 | min_max_col_indices.emplace_back(std::pair(min_col_index, max_col_index)); 377 | min_max_row_indices.emplace_back(std::pair(min_row_index, max_row_index)); 378 | } 379 | 380 | // find clusters that overlap and merge those 381 | std::vector peripheral_cluster_merged(horizontally_peripheral_clusters.size(), false); 382 | for(size_t i = 0; i < horizontally_peripheral_clusters.size(); i++) 383 | { 384 | if(peripheral_cluster_merged[i]) 385 | continue; 386 | 387 | for(size_t j = i + 1; j < horizontally_peripheral_clusters.size(); j++) 388 | { 389 | if(peripheral_cluster_merged[j]) 390 | continue; 391 | 392 | // check for overlap 393 | Eigen::Array2i min_box_1(min_max_col_indices[i].first, min_max_row_indices[i].first); 394 | Eigen::Array2i max_box_1(min_max_col_indices[i].second + 1, min_max_row_indices[i].second + 1); 395 | Eigen::Array2i min_box_2(min_max_col_indices[j].first, min_max_row_indices[j].first); 396 | Eigen::Array2i max_box_2(min_max_col_indices[j].second + 1, min_max_row_indices[j].second + 1); 397 | 398 | Eigen::Array2i intersection_min_corner = min_box_2.max(min_box_1); 399 | Eigen::Array2i intersection_max_corner = max_box_2.min(max_box_1); 400 | 401 | Eigen::Array2i diff = (intersection_max_corner - intersection_min_corner).max(Eigen::Array2i::Zero()); 402 | 403 | int intersection_volume = diff.prod(); 404 | 405 | if(intersection_volume > 0) 406 | { 407 | peripheral_cluster_merged[j] = true; 408 | size_t first_cluster_index = horizontally_peripheral_clusters[i]; 409 | size_t second_cluster_index = horizontally_peripheral_clusters[j]; 410 | 411 | auto cluster_id_of_first = segments_cloud->points[cluster_indices[first_cluster_index].indices[0]].segmentation; 412 | for(int point_index = 0; point_index < (int)cluster_indices[second_cluster_index].indices.size(); point_index++) 413 | { 414 | const int current_point_index = cluster_indices[second_cluster_index].indices[point_index]; 415 | segments_cloud->points[current_point_index].segmentation = cluster_id_of_first; 416 | } 417 | 418 | cluster_indices[first_cluster_index].indices.insert(cluster_indices[first_cluster_index].indices.end(), 419 | cluster_indices[second_cluster_index].indices.begin(), 420 | cluster_indices[second_cluster_index].indices.end()); 421 | cluster_indices[second_cluster_index].indices.clear(); 422 | } 423 | } 424 | } 425 | } 426 | } 427 | else 428 | { 429 | // filter out background points 430 | segments_cloud->header = input_cloud->header; 431 | 432 | for(const auto& point : *input_cloud) 433 | { 434 | if(point.segmentation < m_min_certainty_thresh) 435 | continue; 436 | 437 | segments_cloud->push_back(point); 438 | } 439 | ROS_DEBUG("Object_detector: segments_cloud size is %lu ", segments_cloud->size()); 440 | 441 | if(segments_cloud->empty()) 442 | { 443 | ROS_DEBUG("Object_detection: No positive segmentation points left for detection"); 444 | publishEmpty(segments_cloud->header); 445 | return; 446 | } 447 | 448 | // workaround for usage with nodelet 449 | pcl::PointCloud::Ptr cloud_xyz = boost::make_shared>(); 450 | pcl::copyPointCloud(*segments_cloud, *cloud_xyz); 451 | 452 | if(segments_cloud->size() != cloud_xyz->size()) 453 | ROS_ERROR_STREAM("Object_detection: cloud sizes do not match after copy."); 454 | 455 | // Cluster segments by distance 456 | if(!euclideanClustering(cloud_xyz, cluster_indices, m_cluster_tolerance, m_min_cluster_size, m_max_cluster_size)) 457 | ROS_DEBUG("Object_detection: Clustering resulted in no detections."); 458 | 459 | } 460 | 461 | // transform cloud to fixed frame 462 | if(!transformCloud(segments_cloud, m_fixed_frame)) 463 | { 464 | ROS_ERROR("Object_detection: Transform error."); 465 | publishEmpty(segments_cloud->header); 466 | return; 467 | } 468 | 469 | // propagate all hypotheses' positions into the time of the current cloud 470 | std::vector predicted_states; 471 | { 472 | std::lock_guard guard(m_hypotheses_mutex); 473 | predicted_states.reserve(m_current_hypotheses.states.size()); 474 | 475 | double dt = (header.stamp - m_current_hypotheses.header.stamp).toSec(); 476 | for(const auto& state : m_current_hypotheses.states) 477 | { 478 | predicted_states.emplace_back(Eigen::Vector3d(state.position.x + dt * state.velocity.x, 479 | state.position.y + dt * state.velocity.y, 480 | state.position.z + dt * state.velocity.z)); 481 | } 482 | } 483 | 484 | multi_hypothesis_tracking_msgs::ObjectDetectionsPtr detections_msg(new multi_hypothesis_tracking_msgs::ObjectDetections()); 485 | detections_msg->header.frame_id = m_fixed_frame; 486 | detections_msg->header.stamp = header.stamp; 487 | 488 | visualization_msgs::MarkerArray marker_array; 489 | 490 | geometry_msgs::PoseArray pose_msgs; 491 | pose_msgs.header = detections_msg->header; 492 | 493 | // filter clusters and publish as marker array 494 | for(std::size_t i = 0; i < cluster_indices.size(); ++i) 495 | { 496 | if(cluster_indices[i].indices.empty()) 497 | continue; 498 | 499 | Eigen::Vector3f mean; 500 | Eigen::Array3f min, max; 501 | bool is_peripheral = false; 502 | getClusterProperties(segments_cloud, cluster_indices[i], mean, min, max, is_peripheral); 503 | 504 | Eigen::Vector3f size = max.matrix() - min.matrix(); 505 | 506 | // search for a hypothesis that is close to this cluster 507 | bool loosen_thresholds = false; 508 | for(const auto& predicted_state : predicted_states) 509 | { 510 | float distance = (mean - predicted_state.cast()).norm(); 511 | if(distance < m_amplify_threshold) 512 | { 513 | loosen_thresholds = true; 514 | break; 515 | } 516 | } 517 | 518 | bool filter_cluster = false; 519 | 520 | // check if detection fits description 521 | if(min.z() > m_max_object_altitude) 522 | filter_cluster = true; 523 | 524 | if(size.z() < m_min_object_height && !is_peripheral) 525 | filter_cluster = true; 526 | 527 | if(size.z() > m_max_object_height) 528 | filter_cluster = true; 529 | 530 | float object_width = static_cast(sqrt(pow(size.x(), 2) + pow(size.y(), 2))); 531 | if(object_width > m_max_object_width) 532 | filter_cluster = true; 533 | 534 | bool optimal_detection = !filter_cluster; // flag to mark those detections that don't fit the real description 535 | 536 | if(filter_cluster && loosen_thresholds) 537 | { 538 | if(object_width < m_max_object_width * 2) 539 | filter_cluster = false; 540 | 541 | // only consider max_object_height without min_object_height 542 | if(size.z() < m_max_object_height) 543 | filter_cluster = false; 544 | 545 | optimal_detection = false; 546 | } 547 | 548 | // if filter_cluster still true then filter this cluster out 549 | if(filter_cluster) 550 | continue; 551 | 552 | if(!optimal_detection) 553 | ROS_DEBUG_STREAM("loosening description."); 554 | 555 | // Fill object detection message 556 | if(m_pub_detections.getNumSubscribers() > 0) 557 | { 558 | multi_hypothesis_tracking_msgs::ObjectDetection msg_detection; 559 | msg_detection.centroid.x = mean.x(); 560 | msg_detection.centroid.y = mean.y(); 561 | msg_detection.centroid.z = mean.z(); 562 | 563 | msg_detection.position_covariance_xx = 0.03f * 0.03f; 564 | msg_detection.position_covariance_yy = 0.03f * 0.03f; 565 | msg_detection.position_covariance_zz = 0.03f * 0.03f; 566 | 567 | pcl::PointCloud::Ptr detection_cloud(new pcl::PointCloud); 568 | detection_cloud->points.reserve(cluster_indices[i].indices.size()); 569 | detection_cloud->width = static_cast(cluster_indices[i].indices.size()); 570 | detection_cloud->height = 1; 571 | msg_detection.point_ids.clear(); 572 | for(auto idx : cluster_indices[i].indices) 573 | { 574 | msg_detection.point_ids.push_back(idx); 575 | detection_cloud->points.emplace_back(pcl::PointXYZ(segments_cloud->points[idx].x, segments_cloud->points[idx].y, segments_cloud->points[idx].z)); 576 | } 577 | pcl::toROSMsg(*detection_cloud, msg_detection.cloud); 578 | 579 | msg_detection.class_a_detection = optimal_detection; 580 | 581 | detections_msg->object_detections.push_back(msg_detection); 582 | } 583 | 584 | // Fill visualization message 585 | if(m_pub_vis_marker.getNumSubscribers() > 0) 586 | { 587 | visualization_msgs::Marker marker; 588 | marker.header.frame_id = m_fixed_frame; 589 | marker.header.stamp = header.stamp; 590 | marker.ns = "object_detector_namespace"; 591 | marker.id = i; 592 | marker.type = visualization_msgs::Marker::SPHERE; 593 | marker.action = visualization_msgs::Marker::ADD; 594 | marker.pose.position.x = mean.x(); 595 | marker.pose.position.y = mean.y(); 596 | marker.pose.position.z = mean.z(); 597 | marker.pose.orientation.x = 0.0; 598 | marker.pose.orientation.y = 0.0; 599 | marker.pose.orientation.z = 0.0; 600 | marker.pose.orientation.w = 1.0; 601 | marker.scale.x = std::max(static_cast(size.x()), 0.2f); 602 | marker.scale.y = std::max(static_cast(size.y()), 0.2f); 603 | marker.scale.z = std::max(static_cast(size.z()), 0.2f); 604 | marker.color.a = 0.8f; 605 | marker.color.r = 1.0; 606 | marker.color.g = 0.5; 607 | marker.color.b = 0.0; 608 | marker.lifetime = ros::Duration(0, 100000000); // 0.1 seconds 609 | 610 | marker_array.markers.push_back(marker); 611 | } 612 | } 613 | 614 | ROS_DEBUG_STREAM("Object_detector: Number of detections after filtering " << marker_array.markers.size() << ". "); 615 | 616 | if(m_pub_detections.getNumSubscribers() > 0) 617 | m_pub_detections.publish(detections_msg); 618 | if(m_pub_vis_marker.getNumSubscribers() > 0) 619 | m_pub_vis_marker.publish(marker_array); 620 | if(m_pub_clustered_cloud.getNumSubscribers() > 0) 621 | m_pub_clustered_cloud.publish(segments_cloud); 622 | 623 | if(m_measure_time && !input_cloud->empty()) 624 | { 625 | std::chrono::microseconds time_for_one_callback = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - callback_start_time); 626 | ROS_DEBUG_STREAM("Time for detection one cloud: " << time_for_one_callback.count() << " microseconds."); 627 | m_time_file << (double)time_for_one_callback.count()/1000.0 << std::endl; 628 | m_summed_time_for_callbacks += time_for_one_callback; 629 | m_number_of_callbacks++; 630 | ROS_DEBUG_STREAM("Mean time for detection one cloud: " << m_summed_time_for_callbacks.count() / m_number_of_callbacks << " microseconds."); 631 | } 632 | } 633 | 634 | void Detector::publishEmpty(const pcl::PCLHeader& header) 635 | { 636 | multi_hypothesis_tracking_msgs::ObjectDetectionsPtr detections_msg(new multi_hypothesis_tracking_msgs::ObjectDetections()); 637 | detections_msg->header.frame_id = m_fixed_frame; 638 | detections_msg->header.stamp = pcl_conversions::fromPCL(header.stamp); 639 | 640 | visualization_msgs::MarkerArray marker_array; 641 | 642 | geometry_msgs::PoseArray pose_msgs; 643 | pose_msgs.header = detections_msg->header; 644 | 645 | if(m_pub_detections.getNumSubscribers() > 0) 646 | m_pub_detections.publish(detections_msg); 647 | if(m_pub_vis_marker.getNumSubscribers() > 0) 648 | m_pub_vis_marker.publish(marker_array); 649 | } 650 | 651 | } 652 | -------------------------------------------------------------------------------- /object_detection/src/object_detection_node.cpp: -------------------------------------------------------------------------------- 1 | /** @file 2 | * 3 | * This ROS node detects potential objects in point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | /** 12 | * Main node entry point. 13 | */ 14 | int main(int argc, char **argv) 15 | { 16 | ros::init(argc, argv, "object_detection_node"); 17 | ros::NodeHandle node; 18 | ros::NodeHandle priv_nh("~"); 19 | 20 | object_detection::Detector detector(node, priv_nh); 21 | 22 | // handle callbacks until shut down 23 | ros::spin(); 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /object_detection/src/object_detection_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /** @file 2 | * 3 | * This ROS nodelet detects potential objects in point clouds 4 | * 5 | * @author Jan Razlaw 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace object_detection 15 | { 16 | /** 17 | * @brief Detector in a nodelet. 18 | * @see Detector 19 | */ 20 | class ObjectDetectionNodelet: public nodelet::Nodelet 21 | { 22 | public: 23 | 24 | ObjectDetectionNodelet() {} 25 | ~ObjectDetectionNodelet() {} 26 | 27 | private: 28 | 29 | virtual void onInit(); 30 | boost::shared_ptr detector_; 31 | }; 32 | 33 | /** 34 | * @brief Nodelet initialization. 35 | */ 36 | void ObjectDetectionNodelet::onInit() 37 | { 38 | detector_.reset(new Detector(getNodeHandle(), getPrivateNodeHandle())); 39 | } 40 | 41 | } // namespace object_detection 42 | 43 | 44 | // Register this plugin with pluginlib. Names must match nodelet_plugin.xml. 45 | PLUGINLIB_EXPORT_CLASS(object_detection::ObjectDetectionNodelet, nodelet::Nodelet); 46 | --------------------------------------------------------------------------------