Repository: jkk-research/urban_road_filter Branch: ros2 Commit: a157e15e5174 Files: 16 Total size: 94.0 KB Directory structure: gitextract_lmdcgdfo/ ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config/ │ ├── parameters.yaml │ └── urban_road_filter.rviz ├── img/ │ └── README.md ├── include/ │ └── urban_road_filter/ │ └── data_structures.hpp ├── launch/ │ └── urban_road_filter.launch.py ├── package.xml └── src/ ├── blind_spots.cpp ├── lidar_segmentation.cpp ├── main.cpp ├── star_shaped_search.cpp ├── x_zero_method.cpp └── z_zero_method.cpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: CITATION.cff ================================================ cff-version: 1.2.0 message: "If you use this software, please cite it as below." authors: - family-names: "Horváth" given-names: "Ernő" orcid: "https://orcid.org/0000-0001-5083-2073" - family-names: "Pozna" given-names: "Claudiu" - family-names: "Unger" given-names: "Miklós" orcid: "https://orcid.org/0000-0003-3518-1107" title: "urban_road_filter" version: 1.1.2 date-released: 2023-05-24 url: "https://github.com/jkk-research/urban_road_filter" preferred-citation: type: article authors: - family-names: "Horváth" given-names: "Ernő" orcid: "https://orcid.org/0000-0001-5083-2073" - family-names: "Pozna" given-names: "Claudiu" - family-names: "Unger" given-names: "Miklós" orcid: "https://orcid.org/0000-0003-3518-1107" doi: "10.3390/s22010194" journal: "Sensors" title: "Real-Time LIDAR-Based Urban Road and Sidewalk Detection for Autonomous Vehicles" year: 2022 ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.8) project(urban_road_filter) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() # Find required dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(PCL REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pcl_ros REQUIRED) # Include directories include_directories( include ${PCL_INCLUDE_DIRS} ) # Add compile options add_compile_options(-Wall -Wextra -O2) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) # Create the executable add_executable(urban_road_filter src/lidar_segmentation.cpp src/main.cpp src/star_shaped_search.cpp src/x_zero_method.cpp src/z_zero_method.cpp src/blind_spots.cpp ) # Link the executable ament_target_dependencies(urban_road_filter rclcpp sensor_msgs std_msgs visualization_msgs pcl_conversions pcl_ros ) target_link_libraries(urban_road_filter ${PCL_LIBRARIES}) # Install the executable install(TARGETS urban_road_filter DESTINATION lib/${PROJECT_NAME}) # Install launch files install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) # Install config files install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) ament_package() ================================================ FILE: LICENSE ================================================ BSD 3-Clause License Copyright (c) 2021, JKK - Vehicle Industry Research Center All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: README.md ================================================ # `urban_road_filter`: a real-time LIDAR-based urban road and sidewalk detection algorithm for autonomous vehicles # Dependency - [ROS 2](https://docs.ros.org/en/humble/index.html) (tested with [![Static Badge](https://img.shields.io/badge/ROS_2-Humble-34aec5)](https://docs.ros.org/en/humble/) and [![Static Badge](https://img.shields.io/badge/ROS_2-Jazzy-34aec5)](https://docs.ros.org/en/jazzy/)) - [PCL](https://pointclouds.org/) # Install (download and build) Use the following commands to download and compile the package. ```bash cd ~/ros2_ws/src ``` ```bash git clone https://github.com/jkk-research/urban_road_filter -b ros2 ``` And build: ```bash cd ~/ros2_ws ``` ```bash colcon build --packages-select urban_road_filter --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release ``` # Getting started Issue the following commands to start ROS 2, play sample data, and start the algorithm with visualization. In a **new terminal** start ROS 2: Don't forget to source your workspace first
source ~/ros2_ws/install/setup.bash
```bash ros2 launch urban_road_filter urban_road_filter.launch.py ``` # Features The `urban_road_filter` package provides the following features: 1. **Detection Methods**: - `x_zero_method`: Detects roadside points by analyzing the angle between three points while keeping the X-coordinate constant. - `z_zero_method`: Detects roadside points by analyzing the angle between two vectors while keeping the Z-coordinate constant. - `star_shaped_method`: Uses a star-shaped algorithm to detect roadside points within a sector. 2. **Configurable Parameters**: - Detection area dimensions (`min_x`, `max_x`, `min_y`, `max_y`, `min_z`, `max_z`). - LIDAR vertical resolution (`interval`). - Curb detection parameters (`curb_height`, `curb_points`). - Polygon simplification options (`polysimp_allow`, `polysimp`, `polyz`). 3. **ROS Topics**: - `/road`: Filtered points representing the drivable road. - `/curb`: Filtered points representing curbs. - `/roi`: Filtered points within the region of interest. - `/road_marker`: Visualization markers for detected roads. 4. **Performance Metrics**: - Publishes statistics about the segmentation results, including the number of points classified as road, curb, or non-road. # Configuration The behavior of the `urban_road_filter` node can be customized using the `parameters.yaml` file. Key parameters include: - `fixed_frame`: The fixed frame for the LIDAR data. - `topic_name`: The topic to subscribe to for point cloud data. - `x_zero_method`, `z_zero_method`, `star_shaped_method`: Enable or disable specific detection methods. - `interval`: Acceptable interval for the LIDAR's vertical angular resolution. - `curb_height`, `curb_points`: Parameters for curb detection. - `polysimp_allow`, `polysimp`, `polyz`: Parameters for polygon simplification. # Cite & paper If you use any of this code, please consider citing the [paper](https://www.mdpi.com/1424-8220/22/1/194): ```bibtex @Article{roadfilt2022horv, title = {Real-Time LIDAR-Based Urban Road and Sidewalk Detection for Autonomous Vehicles}, author = {Horváth, Ernő and Pozna, Claudiu and Unger, Miklós}, journal = {Sensors}, volume = {22}, year = {2022}, number = {1}, url = {https://www.mdpi.com/1424-8220/22/1/194}, issn = {1424-8220}, doi = {10.3390/s22010194} } ``` # Related solutions - [`points_preprocessor`](https://github.com/Autoware-AI/core_perception/tree/master/points_preprocessor) `ray_ground_filter` and `ring_ground_filter` (ROS) - [`linefit_ground_segmentation`](https://github.com/lorenwel/linefit_ground_segmentation) (ROS) - [`curb_detection`](https://github.com/linyliny/curb_detection) (ROS) - [`3DLidar_curb_detection`](https://github.com/SohaibAl-emara/3D_Lidar_Curb_Detection) (ROS) - [`lidar_filter`](https://github.com/ZoltanTozser/lidar_filter) - Many more algorithms without code mentioned in the [paper](https://doi.org/10.3390/s22010194). # Videos and images [](https://www.youtube.com/watch?v=T2qi4pldR-E) [](https://www.youtube.com/watch?v=HHnj4VcbSy4) [](https://www.youtube.com/watch?v=9tdzo2AyaHM) [](https://www.youtube.com/watch?v=lp6q_QvWA-Y) # ROS publications / subscriptions ```mermaid flowchart LR P[points]:::gray -->|sensor_msgs/PointCloud2| U([urban_road_filt
node]):::gray U --> |sensor_msgs/PointCloud2| A[curb]:::gray U --> |sensor_msgs/PointCloud2| B[road]:::gray U --> |sensor_msgs/PointCloud2| C[road_probably]:::gray U --> |sensor_msgs/PointCloud2| D[roi]:::gray U --> |visualization_msgs/MarkerArray| E[road_marker]:::gray classDef light fill:#34aec5,stroke:#152742,stroke-width:2px,color:#152742 classDef dark fill:#152742,stroke:#34aec5,stroke-width:2px,color:#34aec5 classDef white fill:#ffffff,stroke:#152742,stroke-width:2px,color:#152742 classDef gray fill:#f6f8fa,stroke:#152742,stroke-width:2px,color:#152742 classDef red fill:#ef4638,stroke:#152742,stroke-width:2px,color:#fff ``` ================================================ FILE: config/parameters.yaml ================================================ # This file consolidates all parameters for the urban_road_filter package. urban_road_filter: ros__parameters: fixed_frame: "left_os1/os1_lidar" # Fixed frame. (Restart needed if it changes.) topic_name: "/left_os1/os1_cloud_node/points" # The desired topic to subscribe to. # Enabling individual detection methods. x_zero_method: true # x zero method. z_zero_method: true # z zero method. star_shaped_method: true # star shaped method. blind_spots: true # Filtering blind spots. # Blindspot detection (x-direction) xDirection: true # Added xDirection parameter with a valid boolean value. # LIDAR's vertical resolution interval: 0.18 # Estimated minimum height of the curb (in meters). curb_height: 0.05 # Estimated number of points on the curb. curb_points: 5 # Width of the beam zone (in degrees). beamZone: 30 # Size of the examined area. min_x: 0.0 max_x: 30.0 min_y: -10.0 max_y: 10.0 min_z: -3.0 max_z: -1.0 # Parameters for detection methods. cylinder_deg_x: 150.0 # Included angle of the examined triangle (three points) for x_zero_method (in degrees). cylinder_deg_z: 140.0 # Included angle of the examined triangle (two vectors) for z_zero_method (in degrees). curb_slope_deg: 50.0 # Radial threshold for star_shaped_method (in degrees). # Coefficients for CSP (Curvature Simplification Process). kdev_param: 1.225 # Dispersion coefficient. kdist_param: 2.0 # Distance coefficient. # Star-beam Filter starbeam_filter: false # Use rectangular beams for detection instead of the whole sectors (StarShaped). dmin_param: 10 # Minimum number of points for dispersion (CSP). # Polygon simplification settings (CSP). simple_poly_allow: true # Use simplified polygon (height data will be lost). poly_s_param: 0.7 # Coefficient of polygon simplification. poly_z_manual: -1.5 # Set a constant polygon height. poly_z_avg_allow: true # Set polygon height to average value. ================================================ FILE: config/urban_road_filter.rviz ================================================ Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Road PointCloud1 - /Curb PointCloud1 - /ROI PointCloud1 - /Road Marker1 Splitter Ratio: 0.5 Tree Height: 725 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: Curb PointCloud Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 0; 255; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: Road PointCloud Position Transformer: XYZ Selectable: true Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /road Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: Curb PointCloud Position Transformer: XYZ Selectable: true Size (Pixels): 3 Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /curb Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 3750 Min Color: 0; 0; 0 Min Intensity: 3 Name: ROI PointCloud Position Transformer: XYZ Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /roi Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: Road Marker Namespaces: "": true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /road_marker Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: left_os1/os1_lidar Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose Covariance x: 0.25 Covariance y: 0.25 Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit Distance: 19.73822784423828 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0 Y: 0 Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.315398246049881 Target Frame: Value: Orbit (rviz) Yaw: 3.1354026794433594 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1016 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd0000000400000000000002480000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003d50000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1848 X: 487 Y: 130 ================================================ FILE: img/README.md ================================================ # Images to help understand the algoritm better ![](rosgraph01.svg) ![](urban_road_filter_explain01.svg) ![](urban_road_filter_example01.png) ![](urban_road_filter_example02.png) ![](urban_road_filter_example02zoom.svg) ================================================ FILE: include/urban_road_filter/data_structures.hpp ================================================ #pragma once /*Basic includes.*/ #include #include #include #include #include #include #include #include /*Includes for ROS 2.*/ #include "rclcpp/rclcpp.hpp" /*Includes for Markers.*/ #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" /*Includes for PCL.*/ #include #include #include #include #include /*ROS 2 specific message types*/ #include "sensor_msgs/msg/point_cloud2.hpp" /*ramer-douglas-peucker*/ #include #include #include #include #include "std_msgs/msg/float32_multi_array.hpp" using namespace boost::assign; typedef boost::geometry::model::d2::point_xy xy; struct Point2D{ pcl::PointXYZI p; float d; float alpha; short isCurbPoint; }; struct Point3D:public Point2D{ float newY; }; struct polar //polar-coordinate struct for the points { int id; //original ID of point (from input cloud) float r; //radial coordinate float fi; //angular coordinate (ccw angle from x-axis) }; struct box //struct for detection beams { std::vector p; //points within the beam's area //box *l, *r; //pointer to adjacent beams (currently not used) bool yx; //whether it is aligned more with the y-axis (than the x-axis) float o, d; //internal parameters (trigonometry) }; namespace params{ extern std::string fixedFrame; /* Fixed Frame.*/ extern std::string topicName; /* subscribed topic.*/ extern bool x_zero_method, z_zero_method, star_shaped_method ; /*Methods of roadside detection*/ extern bool blind_spots; /*Vakfolt javító algoritmus.*/ extern int xDirection; /*A vakfolt levágás milyen irányú.*/ extern float interval; /*A LIDAR vertikális szögfelbontásának, elfogadott intervalluma.*/ extern float curbHeight; /*Becsült minimum szegély magasság.*/ extern int curbPoints; /*A pontok becsült száma, a szegélyen.*/ extern float beamZone; /*A vizsgált sugárzóna mérete.*/ extern float angleFilter1; /*X = 0 érték mellett, három pont által bezárt szög.*/ extern float angleFilter2; /*Z = 0 érték mellett, két vektor által bezárt szög.*/ extern float angleFilter3; /*Csaplár László kódjához szükséges. Sugár irányú határérték (fokban).*/ extern float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsgált terület méretei.*/ extern float kdev_param; //(see below) extern float kdist_param; //(see below) extern bool starbeam_filter; //Toggle usage of rectangular beams for starshaped algorithm instead of the whole sector (containing the beam) extern int dmin_param; //(see below) extern bool polysimp_allow; /*polygon-eygszerűsítés engedélyezése*/ extern bool zavg_allow; /*egyszerűsített polygon z-koordinátái átlagból (engedély)*/ extern float polysimp; /*polygon-egyszerűsítési tényező (Ramer-Douglas-Peucker)*/ extern float polyz; /*manuálisan megadott z-koordináta (polygon)*/ }; /*For pointcloud filtering*/ template class FilteringCondition : public pcl::ConditionBase { public: typedef std::shared_ptr> Ptr; typedef std::shared_ptr> ConstPtr; typedef std::function FunctorT; FilteringCondition(FunctorT evaluator): pcl::ConditionBase(),_evaluator( evaluator ) {} virtual bool evaluate (const PointT &point) const { // just delegate ALL the work to the injected std::function return _evaluator(point); } private: FunctorT _evaluator; }; class Detector : public rclcpp::Node { public: Detector(); void initialize(); int partition(std::vector>& array3D, int arc, int low, int high); void quickSort(std::vector>& array3D, int arc, int low, int high); void filtered(const pcl::PointCloud &cloud); void starShapedSearch(std::vector& array2D); void beam_init(); void xZeroMethod(std::vector>& array3D, int index, int* indexArray); void zZeroMethod(std::vector>& array3D, int index, int* indexArray); void blindSpots(std::vector>& array3D, int index, int* indexArray, float* maxDistance); // Additional methods for ROS 2 void declare_and_get_parameters(); rcl_interfaces::msg::SetParametersResult on_parameter_changed(const std::vector ¶meters); void update_global_params(); private: // Parameter callback handle OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; // Parameter declarations std::string fixed_frame_; std::string topic_name_; bool x_zero_method_; bool z_zero_method_; bool star_shaped_method_; bool blind_spots_; bool xDirection_; double interval_; double curb_height_; int curb_points_; int beamZone_; double cylinder_deg_x_; double cylinder_deg_z_; double curb_slope_deg_; double min_x_; double max_x_; double min_y_; double max_y_; double min_z_; double max_z_; double kdev_param_; double kdist_param_; bool starbeam_filter_; int dmin_param_; bool simple_poly_allow_; double poly_s_param_; bool poly_z_avg_allow_; double poly_z_manual_; // Publishers rclcpp::Publisher::SharedPtr pub_road; rclcpp::Publisher::SharedPtr pub_high; rclcpp::Publisher::SharedPtr pub_box; rclcpp::Publisher::SharedPtr pub_pobroad; rclcpp::Publisher::SharedPtr pub_marker; rclcpp::Publisher::SharedPtr pub_stats; // Statistics publisher // Subscriber rclcpp::Subscription::SharedPtr sub; // Callback function for point cloud processing void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg); // For Ramer-Douglas-Peucker algorithm boost::geometry::model::linestring line; boost::geometry::model::linestring simplified; }; ================================================ FILE: launch/urban_road_filter.launch.py ================================================ from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration import os from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # Get the package directory pkg_dir = get_package_share_directory('urban_road_filter') # Path to the parameters.yaml file config_file_path = os.path.join(pkg_dir, 'config', 'parameters.yaml') # Create the default rviz config path default_rviz_config_path = os.path.join(pkg_dir, 'config', 'urban_road_filter.rviz') # Create launch configuration variables use_sim_time = LaunchConfiguration('use_sim_time', default='true') # Changed to true for bag playback rviz_config_file = LaunchConfiguration('rviz_config_file', default=default_rviz_config_path) # Declare the launch arguments declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='true', # Changed to true for bag playback description='Use simulation (Gazebo) clock if true') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=default_rviz_config_path, description='Full path to the RVIZ config file to use') # Start the urban_road_filter node urban_road_filter_cmd = Node( package='urban_road_filter', executable='urban_road_filter', name='urban_road_filter', output='screen', parameters=[config_file_path] ) # Start RViz rviz_cmd = Node( package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', rviz_config_file], output='screen' ) # Make sure the rviz configuration directory exists os.makedirs(os.path.join(pkg_dir, 'config'), exist_ok=True) # Create the launch description and populate ld = LaunchDescription() # Add launch arguments ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_rviz_config_file_cmd) # Add nodes to the launch description ld.add_action(urban_road_filter_cmd) ld.add_action(rviz_cmd) return ld ================================================ FILE: package.xml ================================================ urban_road_filter 1.0.0 ROS 2 Real-Time LIDAR-based urban road and sidewalk detection algorithm for autonomous vehicles Barham Farraj AbdelrahmanAlabdallah Ernő Horváth Claudiu Pozna Miklós Unger MIT https://github.com/jkk-research/urban_road_filter https://doi.org/10.3390/s22010194 ament_cmake rclcpp sensor_msgs std_msgs visualization_msgs pcl_conversions pcl_ros rclcpp_components libpcl-all-dev ament_lint_auto ament_lint_common ament_cmake ================================================ FILE: src/blind_spots.cpp ================================================ #include "urban_road_filter/data_structures.hpp" int params::xDirection; //direction of the blindspot cutoff bool params::blind_spots; //enable blindspot correction float params::beamZone; //size of hte examined beamzone void Detector::blindSpots(std::vector>& array3D,int index,int* indexArray,float* maxDistance){ /*Blind spot detection: We examine the second arc. (First one gives inaccurate results.) The intervals (segments) [90°-180° --- 180°-270°] and [0°-90° --- 270°-360°] are of greatest significance (at the two sides of the car). We search for 2 pieces of high points in both intervals. If there are two high points on the first arc in the interval, then the area between has a high chance of being a blind spot.*/ float q1 = 0, q2 = 180, q3 = 180, q4 = 360; //the four segments (quarters) of the arc int i,j,k,l; //"temporary" variables if (params::blind_spots) { for (i = 0; i < indexArray[1]; i++) { if(array3D[1][i].isCurbPoint==2) { if (array3D[1][i].alpha >= 0 && array3D[1][i].alpha < 90) { if (array3D[1][i].alpha > q1) { q1 = array3D[1][i].alpha; } } else if (array3D[1][i].alpha >= 90 && array3D[1][i].alpha < 180) { if (array3D[1][i].alpha < q2) { q2 = array3D[1][i].alpha; } } else if (array3D[1][i].alpha >= 180 && array3D[1][i].alpha < 270) { if (array3D[1][i].alpha > q3) { q3 = array3D[1][i].alpha; } } else { if (array3D[1][i].alpha < q4) { q4 = array3D[1][i].alpha; } } } } } float arcDistance; //arc length at the given angle - It is important to use the same arc length to examine every arc. int notRoad; //If there is a high point in the given segment on the given arc, then value 1 will be assigned to it, 0 otherwise. int blindSpot; //blind spots by the car float currentDegree; //the angle on the current arc /*determining arc length*/ arcDistance = ((maxDistance[0] * M_PI) / 180) * params::beamZone; /*from 0° to [360° - beamZone]*/ for (i = 0; i <= 360 - params::beamZone; i++) { blindSpot = 0; if (params::blind_spots) { /*If these conditions are met, then we have reached a blind spot and we stop checking.*/ if (params::xDirection == 0) { /*evaluating the points in both directions (+-X)*/ if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3)) { blindSpot = 1; } } else if (params::xDirection == 1) { /*evaluating the points in +X direction.*/ if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270))) { blindSpot = 1; } } else { /*evaluating the points in -X direction.*/ if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90)) { blindSpot = 1; } } } if (blindSpot == 0) { /*By default settings there's no high point in the given segment.*/ notRoad = 0; /*evaluation of the given segment of the first arc*/ for (j = 0; array3D[0][j].alpha <= i + params::beamZone && j < indexArray[0]; j++) { if (array3D[0][j].alpha >= i) { /*The segment needs no further checking if a high point is found.*/ if (array3D[0][j].isCurbPoint == 2) { notRoad = 1; break; } } } /*If no high point is found in the given segment of the first arc, we can proceed to the next arc.*/ if (notRoad == 0) { /*We accept the segment of the first arc.*/ for (j = 0; array3D[0][j].alpha <= i + params::beamZone && j < indexArray[0]; j++) { if (array3D[0][j].alpha >= i) { array3D[0][j].isCurbPoint = 1; } } /*checking the rest of the arcs*/ for (k = 1; k < index; k++) { /*A new angle needs to be defined to get the same arc length at every radius.*/ if (i == 360 - params::beamZone) { currentDegree = 360; } else { currentDegree = i + arcDistance / ((maxDistance[k] * M_PI) / 180); } /*checking the points of the new arc*/ for (l = 0; array3D[k][l].alpha <= currentDegree && l < indexArray[k]; l++) { if (array3D[k][l].alpha >= i) { /*No further processing is needed if a high point is found within the segment.*/ if (array3D[k][l].isCurbPoint == 2) { notRoad = 1; break; } } } /*The rest of the arcs do not need to be checked if the beam stops at a high point.*/ if (notRoad == 1) break; /*else: the given segment of the given arc is accepted*/ for (l = 0; array3D[k][l].alpha <= currentDegree && l < indexArray[k]; l++) { if (array3D[k][l].alpha >= i) { array3D[k][l].isCurbPoint = 1; } } } } } } /*same as before but we check from 360° to [0° + beamZone] this time*/ for (i = 360; i >= 0 + params::beamZone; --i) { blindSpot = 0; if (params::blind_spots) { /*If these conditions are met, then we have reached a blind spot and we stop checking.*/ if (params::xDirection == 0) { /*evaluating the points in both directions (+-X)*/ if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3)) { blindSpot = 1; } } else if (params::xDirection == 1) { /*evaluating the points in +X direction.*/ if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270))) { blindSpot = 1; } } else { /*evaluating the points in -X direction.*/ if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90)) { blindSpot = 1; } } } if (blindSpot == 0) { /*By default settings there's no high point in the given segment.*/ notRoad = 0; /*evaluation of the given segment of the first arc*/ for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - params::beamZone && j >= 0; --j) { if (array3D[0][j].alpha <= i) { /*The segment needs no further checking if a high point is found.*/ if (array3D[0][j].isCurbPoint == 2) { notRoad = 1; break; } } } /*If no high point is found in the given segment of the first arc, we can proceed to the next arc.*/ if (notRoad == 0) { /*We accept the segment of the first arc.*/ for (j = indexArray[0] - 1; array3D[0][j].alpha >= i - params::beamZone && j >= 0; --j) { if (array3D[0][j].alpha <= i) { array3D[0][j].isCurbPoint = 1; } } /*checking the rest of the arcs*/ for (k = 1; k < index; k++) { /*A new angle needs to be defined to get the same arc length at every radius.*/ if (i == 0 + params::beamZone) { currentDegree = 0; } else { currentDegree = i - arcDistance / ((maxDistance[k] * M_PI) / 180); } /*checking the points of the new arc*/ for (l = indexArray[k] - 1; array3D[k][l].alpha >= currentDegree && l >= 0; --l) { if (array3D[k][l].alpha <= i) { /*The segment needs no further processing if a high point is found.*/ if (array3D[k][l].isCurbPoint == 2) { notRoad = 1; break; } } } /*The rest of the arcs do not need to be checked if the beam stops at a high point.*/ if (notRoad == 1) break; /*else: the given segment of the given arc is accepted*/ for (l = indexArray[k] - 1; array3D[k][l].alpha >= currentDegree && l >= 0; --l) { if (array3D[k][l].alpha <= i) { array3D[k][l].isCurbPoint = 1; } } } } } } } ================================================ FILE: src/lidar_segmentation.cpp ================================================ #include "urban_road_filter/data_structures.hpp" /*Global variables.*/ int channels = 64; //The number of channels of the LIDAR . std::string params::fixedFrame; //Fixed Frame. std::string params::topicName; //subscribed topic bool params::x_zero_method, params::z_zero_method, params::star_shaped_method; //methods of roadside detection float params::interval; //acceptable interval for the LIDAR's vertical angular resolution float params::min_X, params::max_X, params::min_Y, params::max_Y, params::min_Z, params::max_Z; //dimensions of detection area bool params::polysimp_allow = true; //enable polygon simplification bool params::zavg_allow = true; //enable usage of average 'z' value as polygon height float params::polysimp = 0.5; //coefficient of polygon simplification (ramer-douglas-peucker) float params::polyz = -1.5; //manually set z-coordinate (output polygon) int ghostcount = 0; //counter variable helping to remove obsolete markers (ghosts) void marker_init(visualization_msgs::msg::Marker& m) { m.pose.position.x = 0; m.pose.position.y = 0; m.pose.position.z = 0; m.pose.orientation.x = 0.0; m.pose.orientation.y = 0.0; m.pose.orientation.z = 0.0; m.pose.orientation.w = 1.0; m.scale.x = 0.5; m.scale.y = 0.5; m.scale.z = 0.5; } inline std_msgs::msg::ColorRGBA setcolor(float r, float g, float b, float a) { std_msgs::msg::ColorRGBA c; c.r = r; c.g = g; c.b = b; c.a = a; return c; } Detector::Detector() : rclcpp::Node("urban_road_filter") { RCLCPP_INFO(this->get_logger(), "Initializing %s", this->get_name()); // Declare parameters declare_and_get_parameters(); // Update global parameters (kept for compatibility with existing code) update_global_params(); // Initialize lidar processing components initialize(); } void Detector::initialize() { /*Initialize publishers and subscribers*/ /*subscribing to the given topic*/ sub = this->create_subscription( params::topicName, rclcpp::SensorDataQoS(), std::bind(&Detector::pointCloudCallback, this, std::placeholders::_1)); /*publishing filtered points*/ pub_road = this->create_publisher("road", 10); pub_high = this->create_publisher("curb", 10); pub_box = this->create_publisher("roi", 10); // ROI - region of interest pub_pobroad = this->create_publisher("road_probably", 10); pub_marker = this->create_publisher("road_marker", 10); pub_stats = this->create_publisher("statistics", 10); // Statistics publisher Detector::beam_init(); RCLCPP_INFO(this->get_logger(), "Ready"); } void Detector::pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg) { RCLCPP_INFO(this->get_logger(), "Received point cloud with %d points, frame_id: %s", cloud_msg->width * cloud_msg->height, cloud_msg->header.frame_id.c_str()); pcl::PointCloud cloud; pcl::fromROSMsg(*cloud_msg, cloud); filtered(cloud); } void Detector::declare_and_get_parameters() { // Declare all parameters with defaults this->declare_parameter("fixed_frame", "velodyne"); this->declare_parameter("topic_name", "/velodyne_points"); this->declare_parameter("x_zero_method", true); this->declare_parameter("z_zero_method", true); this->declare_parameter("star_shaped_method", true); this->declare_parameter("blind_spots", true); this->declare_parameter("xDirection", true); this->declare_parameter("interval", 1.0); this->declare_parameter("curb_height", 0.03); this->declare_parameter("curb_points", 5); this->declare_parameter("beamZone", 15); this->declare_parameter("cylinder_deg_x", 2.5); this->declare_parameter("cylinder_deg_z", 2.5); this->declare_parameter("curb_slope_deg", 15.0); this->declare_parameter("min_x", 0.0); this->declare_parameter("max_x", 35.0); this->declare_parameter("min_y", -10.0); this->declare_parameter("max_y", 10.0); this->declare_parameter("min_z", -3.0); this->declare_parameter("max_z", 3.0); this->declare_parameter("kdev_param", 25.0); this->declare_parameter("kdist_param", 0.8); this->declare_parameter("starbeam_filter", true); this->declare_parameter("dmin_param", 10); this->declare_parameter("simple_poly_allow", true); this->declare_parameter("poly_s_param", 0.5); this->declare_parameter("poly_z_avg_allow", false); this->declare_parameter("poly_z_manual", 0.0); // Get parameter values fixed_frame_ = this->get_parameter("fixed_frame").as_string(); topic_name_ = this->get_parameter("topic_name").as_string(); x_zero_method_ = this->get_parameter("x_zero_method").as_bool(); z_zero_method_ = this->get_parameter("z_zero_method").as_bool(); star_shaped_method_ = this->get_parameter("star_shaped_method").as_bool(); blind_spots_ = this->get_parameter("blind_spots").as_bool(); xDirection_ = this->get_parameter("xDirection").as_bool(); interval_ = this->get_parameter("interval").as_double(); curb_height_ = this->get_parameter("curb_height").as_double(); curb_points_ = this->get_parameter("curb_points").as_int(); beamZone_ = this->get_parameter("beamZone").as_int(); cylinder_deg_x_ = this->get_parameter("cylinder_deg_x").as_double(); cylinder_deg_z_ = this->get_parameter("cylinder_deg_z").as_double(); curb_slope_deg_ = this->get_parameter("curb_slope_deg").as_double(); min_x_ = this->get_parameter("min_x").as_double(); max_x_ = this->get_parameter("max_x").as_double(); min_y_ = this->get_parameter("min_y").as_double(); max_y_ = this->get_parameter("max_y").as_double(); min_z_ = this->get_parameter("min_z").as_double(); max_z_ = this->get_parameter("max_z").as_double(); kdev_param_ = this->get_parameter("kdev_param").as_double(); kdist_param_ = this->get_parameter("kdist_param").as_double(); starbeam_filter_ = this->get_parameter("starbeam_filter").as_bool(); dmin_param_ = this->get_parameter("dmin_param").as_int(); simple_poly_allow_ = this->get_parameter("simple_poly_allow").as_bool(); poly_s_param_ = this->get_parameter("poly_s_param").as_double(); poly_z_avg_allow_ = this->get_parameter("poly_z_avg_allow").as_bool(); poly_z_manual_ = this->get_parameter("poly_z_manual").as_double(); // Set up parameter callback param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&Detector::on_parameter_changed, this, std::placeholders::_1)); } rcl_interfaces::msg::SetParametersResult Detector::on_parameter_changed( const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; for (const auto ¶m : parameters) { if (param.get_name() == "fixed_frame") { fixed_frame_ = param.as_string(); } else if (param.get_name() == "topic_name") { topic_name_ = param.as_string(); } else if (param.get_name() == "x_zero_method") { x_zero_method_ = param.as_bool(); } else if (param.get_name() == "z_zero_method") { z_zero_method_ = param.as_bool(); } else if (param.get_name() == "star_shaped_method") { star_shaped_method_ = param.as_bool(); } else if (param.get_name() == "blind_spots") { blind_spots_ = param.as_bool(); } else if (param.get_name() == "xDirection") { xDirection_ = param.as_bool(); } else if (param.get_name() == "interval") { interval_ = param.as_double(); } else if (param.get_name() == "curb_height") { curb_height_ = param.as_double(); } else if (param.get_name() == "curb_points") { curb_points_ = param.as_int(); } else if (param.get_name() == "beamZone") { beamZone_ = param.as_int(); } else if (param.get_name() == "cylinder_deg_x") { cylinder_deg_x_ = param.as_double(); } else if (param.get_name() == "cylinder_deg_z") { cylinder_deg_z_ = param.as_double(); } else if (param.get_name() == "curb_slope_deg") { curb_slope_deg_ = param.as_double(); } else if (param.get_name() == "min_x") { min_x_ = param.as_double(); } else if (param.get_name() == "max_x") { max_x_ = param.as_double(); } else if (param.get_name() == "min_y") { min_y_ = param.as_double(); } else if (param.get_name() == "max_y") { max_y_ = param.as_double(); } else if (param.get_name() == "min_z") { min_z_ = param.as_double(); } else if (param.get_name() == "max_z") { max_z_ = param.as_double(); } else if (param.get_name() == "kdev_param") { kdev_param_ = param.as_double(); } else if (param.get_name() == "kdist_param") { kdist_param_ = param.as_double(); } else if (param.get_name() == "starbeam_filter") { starbeam_filter_ = param.as_bool(); } else if (param.get_name() == "dmin_param") { dmin_param_ = param.as_int(); } else if (param.get_name() == "simple_poly_allow") { simple_poly_allow_ = param.as_bool(); } else if (param.get_name() == "poly_s_param") { poly_s_param_ = param.as_double(); } else if (param.get_name() == "poly_z_avg_allow") { poly_z_avg_allow_ = param.as_bool(); } else if (param.get_name() == "poly_z_manual") { poly_z_manual_ = param.as_double(); } } // Update global parameters to match node parameters update_global_params(); RCLCPP_INFO(this->get_logger(), "Parameters updated for %s", this->get_name()); return result; } void Detector::update_global_params() { // Update global parameters (kept for compatibility with existing code) params::fixedFrame = fixed_frame_; params::topicName = topic_name_; params::x_zero_method = x_zero_method_; params::z_zero_method = z_zero_method_; params::star_shaped_method = star_shaped_method_; params::blind_spots = blind_spots_; params::xDirection = xDirection_; params::interval = interval_; params::curbHeight = curb_height_; params::curbPoints = curb_points_; params::beamZone = beamZone_; params::angleFilter1 = cylinder_deg_x_; params::angleFilter2 = cylinder_deg_z_; params::angleFilter3 = curb_slope_deg_; params::min_X = min_x_; params::max_X = max_x_; params::min_Y = min_y_; params::max_Y = max_y_; params::min_Z = min_z_; params::max_Z = max_z_; params::kdev_param = kdev_param_; params::kdist_param = kdist_param_; params::starbeam_filter = starbeam_filter_; params::dmin_param = dmin_param_; params::polysimp_allow = simple_poly_allow_; params::polysimp = poly_s_param_; params::zavg_allow = poly_z_avg_allow_; params::polyz = poly_z_manual_; } /*FUNCTIONS*/ /*recursive, quick sorting function (1/2)*/ int Detector::partition(std::vector>& array3D, int arc,int low, int high) { float pivot = array3D[arc][high].alpha; int i = (low - 1); for (int j = low; j <= high - 1; j++){ if (array3D[arc][j].alpha < pivot){ i++; std::swap(array3D[arc][i],array3D[arc][j]); } } std::swap(array3D[arc][i+1],array3D[arc][high]); return (i + 1); } /*recursive, quick sorting function (2/2)*/ void Detector::quickSort(std::vector>& array3D, int arc, int low, int high) { if (low < high) { int pi = partition(array3D, arc, low, high); quickSort(array3D, arc, low, pi - 1); quickSort(array3D, arc, pi + 1, high); } } void Detector::filtered(const pcl::PointCloud &cloud){ /*variables for the "for" loops*/ size_t i, j, k; // Removed unused variable 'l' // Debug print to see if the filtered method is executed RCLCPP_INFO(this->get_logger(), "Starting filtered processing with %ld input points", cloud.size()); auto start_time = std::chrono::high_resolution_clock::now(); pcl::PointXYZI pt; //temporary variable for storing a point auto cloud_filtered_Box = std::make_shared>(cloud); //all points in the detection area pcl::PointCloud cloud_filtered_Road; //filtered points (driveable road) pcl::PointCloud cloud_filtered_ProbablyRoad; //filtered points (non-driveable road) pcl::PointCloud cloud_filtered_High; //filtered points (non-road) auto filterCondition = std::make_shared>( [=](const pcl::PointXYZI& point){ return point.x >= params::min_X && point.x <= params::max_X && point.y >= params::min_Y && point.y <= params::max_Y && point.z >= params::min_Z && point.z <= params::max_Z && point.x + point.y + point.z != 0; } ); pcl::ConditionalRemoval condition_removal; condition_removal.setCondition(filterCondition); condition_removal.setInputCloud(cloud_filtered_Box); condition_removal.filter(*cloud_filtered_Box); /*number of points in the detection area*/ size_t piece = cloud_filtered_Box->points.size(); /*A minimum of 30 points are requested in the detection area to avoid errors. Also, there is not much point in evaluating less data than that.*/ if (piece < 30){ return; } std::vector array2D(piece); /*variable for storing the input for trigonometric functions*/ float bracket; /*A 1D array containing the various angular resolutions. This equals to the number of LiDAR channels. It is important to fill it with 0 values.*/ float angle[channels] = {0}; /*This helps to fill the 1D array containing the angular resolutions.*/ int index = 0; /*whether the given angle corresponds to a new arc*/ int newCircle; /*filling the 2D array*/ for (size_t i = 0; i < piece; i++){ /*--- filling the first 4 columns ---*/ array2D[i].p = cloud_filtered_Box->points[i]; array2D[i].d = sqrt(pow(array2D[i].p.x, 2) + pow(array2D[i].p.y, 2) + pow(array2D[i].p.z, 2)); /*--- filling the 5. column ---*/ bracket = abs(array2D[i].p.z) / array2D[i].d; /*required because of rounding errors*/ if (bracket < -1) bracket = -1; else if (bracket > 1) bracket = 1; /*calculation and conversion to degrees*/ if (array2D[i].p.z < 0) { array2D[i].alpha = acos(bracket) * 180 / M_PI; } else{ array2D[i].alpha = (asin(bracket) * 180 / M_PI) + 90; } /*setting the index*/ /*Our basic assumption is that the angle corresponds to a new circle/arc.*/ newCircle = 1; /*If this value has already occured (within the specified interval), then this is not a new arc. Which means that "newCircle = 0", we can exit the loop, no further processing required.*/ for (j = 0; j < channels; j++) { if (angle[j] == 0) break; if (abs(angle[j] - array2D[i].alpha) <= params::interval) { newCircle = 0; break; } } /*If no such value is registered in the array, then it's a new circle/arc.*/ if (newCircle == 1) { /*We cannot allow the program to halt with a segmentation fault error. If for any reason there would come to be more than 64 arcs/circles, an error would occur.*/ if (index < channels) { angle[index] = array2D[i].alpha; index++; } } } /*calling starShapedSearch algorithm*/ if (params::star_shaped_method) Detector::starShapedSearch(array2D); /*Sorting the angular resolutions by ascending order... The smallest will be the first arc, etc..*/ std::sort(angle, angle + index); std::vector> array3D(channels,std::vector(piece)); /*This is required to set up the row indices of the groups ("channels") containing the arcs. It is important to fill it with 0 values.*/ int indexArray[channels] = {0}; /*A 1D array. The values of points that have the greatest distance from the origo.*/ float maxDistance[channels] = {0}; /*variable helping to handle errors caused by wrong number of channels.*/ int results; /*filling the 3D array*/ for (size_t i = 0; i < piece; i++) { results = 0; /*selecting the desired arc*/ for (j = 0; j < index; j++) { if (abs(angle[j] - array2D[i].alpha) <= params::interval) { results = 1; break; } } if (results == 1) { /*assigning values from the 2D array*/ array3D[j][indexArray[j]].p = array2D[i].p; /*the known "high" points*/ if (params::star_shaped_method) array3D[j][indexArray[j]].isCurbPoint = array2D[i].isCurbPoint; /*The only difference here is that the distance is calculated in 2D - with no regard to the 'z' value.*/ array3D[j][indexArray[j]].d = sqrt(pow(array2D[i].p.x, 2) + pow(array2D[i].p.y, 2)); /*filling the 5. column with the angular position of points, in degrees.*/ bracket = (abs(array3D[j][indexArray[j]].p.x)) / (array3D[j][indexArray[j]].d); if (bracket < -1) bracket = -1; else if (bracket > 1) bracket = 1; if (array3D[j][indexArray[j]].p.x >= 0 && array3D[j][indexArray[j]].p.y <= 0) { array3D[j][indexArray[j]].alpha = asin(bracket) * 180 / M_PI; } else if (array3D[j][indexArray[j]].p.x >= 0 && array3D[j][indexArray[j]].p.y > 0) { array3D[j][indexArray[j]].alpha = 180 - (asin(bracket) * 180 / M_PI); } else if (array3D[j][indexArray[j]].p.x < 0 && array3D[j][indexArray[j]].p.y >= 0) { array3D[j][indexArray[j]].alpha = 180 + (asin(bracket) * 180 / M_PI); } else { array3D[j][indexArray[j]].alpha = 360 - (asin(bracket) * 180 / M_PI); } if (array3D[j][indexArray[j]].d > maxDistance[j]) { maxDistance[j] = array3D[j][indexArray[j]].d; } indexArray[j]++; } } if(params::x_zero_method) Detector::xZeroMethod(array3D,index,indexArray); if(params::z_zero_method) Detector::zZeroMethod(array3D,index,indexArray); float d; /*-- step 2.: filtering road points --*/ /*ordering the elements of the array by angle on every arc*/ for (i = 0; i < index; i++){ quickSort(array3D, i, 0, indexArray[i] - 1); } /*blindspot detection*/ Detector::blindSpots(array3D,index,indexArray,maxDistance); /*-- step 3: searching for marker points - the farthest green point within the given angle --*/ /*It contains the points of the marker. The first three columns contain the X - Y - Z coordinates and the fourth column contains value 0 or 1 depending on whether there is a point within the given angle that is not marked as road.*/ float markerPointsArray[piece][4]; float maxDistanceRoad; //the distance of the farthest green point within the given angle int cM = 0; //variable helping to fill the marker with points (c - counter, M - Marker) int ID1, ID2; //which arc does the point fall onto (ID1) and (ordinally) which point is it (ID2) int redPoints; //whether there is a high point in the examined segment or a point that has not been marked as either road or high point /*checking the points by 1 degree at a time*/ for (i = 0; i <= 360; i++) { ID1 = -1; ID2 = -1; maxDistanceRoad = 0; redPoints = 0; /*iterating through all the points of all the arcs*/ for (j = 0; j < index; j++) { for (k = 0; k < indexArray[j]; k++) { /*If a non-road point is found, then we break the loop, because there will not be a road point found later on and value 1 will be assigned to the variable "redPoints".*/ if (array3D[j][k].isCurbPoint != 1 && array3D[j][k].alpha >= i && array3D[j][k].alpha < i + 1) { redPoints = 1; break; } /*checking the distance for the detected green point*/ if (array3D[j][k].isCurbPoint == 1 && array3D[j][k].alpha >= i && array3D[j][k].alpha < i + 1) { d = sqrt(pow(0 - array3D[j][k].p.x, 2) + pow(0 - array3D[j][k].p.y, 2)); if (d > maxDistanceRoad) { maxDistanceRoad = d; ID1 = j; ID2 = k; } } } /*The previous "break" was used to exit the current circle, this one will exit all of them and proceed to the next angle.*/ if (redPoints == 1) break; } /*adding the marker points to the array*/ if (ID1 != -1 && ID2 != -1) { markerPointsArray[cM][0] = array3D[ID1][ID2].p.x; markerPointsArray[cM][1] = array3D[ID1][ID2].p.y; markerPointsArray[cM][2] = array3D[ID1][ID2].p.z; markerPointsArray[cM][3] = redPoints; cM++; } } /*-- step 4.: filling the groups --*/ for (i = 0; i < index; i++) { for (j = 0; j < indexArray[i]; j++) { pt = array3D[i][j].p; /*road points*/ if (array3D[i][j].isCurbPoint == 1) cloud_filtered_Road.push_back(pt); /*high points*/ else if (array3D[i][j].isCurbPoint == 2) cloud_filtered_High.push_back(pt); } } /*-- step 5.: setting up the marker --*/ /*There need to be at least 3 points to connect, otherwise errors might occur.*/ if (cM > 2) { /*There might be a case where points are in red-green-red (or the other way around) order next to each other. This is bad is because the green / red marker (line strip) in this case will only consist of 1 point. This is not recommended, every point needs to have a pair of the same color. If the 3. column of "markerPointsArray" has the value 1 then it belongs to the red line strip, otherwise it belongs to the green one.*/ /*If the first point is green but the second one is red, then the first one will be added to the red line strip too.*/ if (markerPointsArray[0][3] == 0 && markerPointsArray[1][3] == 1) markerPointsArray[0][3] = 1; /*If the last point is green but the second to last is red, then the last one will be added to the red line strip too.*/ if (markerPointsArray[cM - 1][3] == 0 && markerPointsArray[cM - 2][3] == 1) markerPointsArray[cM - 1][3] = 1; /*If the first point is red but the second one is green, then the first one will be added to the green line strip too.*/ if (markerPointsArray[0][3] == 1 && markerPointsArray[1][3] == 0) markerPointsArray[0][3] = 0; /*If the last point is red but the second to last is green, then the last one will be added to the green line strip too.*/ if (markerPointsArray[cM - 1][3] == 1 && markerPointsArray[cM - 2][3] == 0) markerPointsArray[cM - 1][3] = 0; /*Here we iterate through all the points. If a green point gets between two red ones, then it will be added to the red line strip too. The first two and last two points are not checked - they were already set before.*/ for (i = 2; i <= cM - 3; i++) { if (markerPointsArray[i][3] == 0 && markerPointsArray[i - 1][3] == 1 && markerPointsArray[i + 1][3] == 1) markerPointsArray[i][3] = 1; } /*Here we iterate through all the points. If a red point gets between two green ones, then it will be added to the green line strip too. The first two and last two points are not checked - they were already set before.*/ for (i = 2; i <= cM - 3; i++) { if (markerPointsArray[i][3] == 1 && markerPointsArray[i - 1][3] == 0 && markerPointsArray[i + 1][3] == 0) markerPointsArray[i][3] = 0; } visualization_msgs::msg::MarkerArray ma; //a marker array containing the green / red line strips visualization_msgs::msg::Marker line_strip; //the current green or red section / line strip geometry_msgs::msg::Point point; //point to fill the line strip with float zavg = 0.0; //average z value (for the simplified polygon) int lineStripID = 0; //ID of the given line strip line_strip.header.frame_id = params::fixedFrame; line_strip.header.stamp = this->now(); line_strip.type = visualization_msgs::msg::Marker::LINE_STRIP; line_strip.action = visualization_msgs::msg::Marker::ADD; /*We iterate through the points which will make up the marker.*/ for (i = 0; i < cM; i++) { /*adding the given point to a "geometry_msgs::msg::Point" type variable*/ point.x = markerPointsArray[i][0]; point.y = markerPointsArray[i][1]; point.z = markerPointsArray[i][2]; zavg *= i; zavg += point.z; zavg /= i+1; /*Adding the first point to the current line strip. No conditions need to be met for the first point.*/ if (i == 0) { line_strip.points.push_back(point); line += xy(point.x,point.y); } /*If the next point is from the same group (red or green) as the previous one then it will be added to the line strip aswell.*/ else if (markerPointsArray[i][3] == markerPointsArray[i - 1][3]) { line_strip.points.push_back(point); line += xy(point.x,point.y); /*In this "else if" section we will reach the last point and the last line strip will be created.*/ if (i == cM - 1) { line_strip.id = lineStripID; marker_init(line_strip); /*setting the color of the line strip*/ if (markerPointsArray[i][3] == 0) { line_strip.color = setcolor(0.0, 1.0, 0.0, 1.0); //green } else { line_strip.color = setcolor(1.0, 0.0, 0.0, 1.0); //red } if (params::polysimp_allow) { line_strip.points.clear(); boost::geometry::clear(simplified); boost::geometry::simplify(line, simplified, params::polysimp); for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) { geometry_msgs::msg::Point p; p.x = boost::geometry::get<0>(*it); p.y = boost::geometry::get<1>(*it); p.z = params::polyz; line_strip.points.push_back(p); } } ma.markers.push_back(line_strip); //adding the line strip to the marker array line_strip.points.clear(); //We clear the points from the last line strip as there's no need for them anymore. boost::geometry::clear(line); } } /*change of category: red -> green The line joining the two points is still red, so we add the point to the given line strip.*/ else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 0) { line_strip.points.push_back(point); line += xy(point.x,point.y); /*The following points belong to a new line strip - a red one is being made here.*/ line_strip.id = lineStripID; lineStripID++; marker_init(line_strip); line_strip.color = setcolor(1.0, 0.0, 0.0, 1.0); //red if (params::polysimp_allow) { line_strip.points.clear(); boost::geometry::clear(simplified); boost::geometry::simplify(line, simplified, params::polysimp); for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) { geometry_msgs::msg::Point p; p.x = boost::geometry::get<0>(*it); p.y = boost::geometry::get<1>(*it); p.z = params::polyz; line_strip.points.push_back(p); } } ma.markers.push_back(line_strip); //adding the line strip to the marker array line_strip.points.clear(); //the points are not needed anymore boost::geometry::clear(line); line_strip.points.push_back(point); //This point is needed for the next line strip aswell, so we add it. line += xy(point.x,point.y); } /*change of category: green -> red First we set up the green line strip, then we add the last point to the red one aswell, since there is always a red line strip between a green and a red point.*/ else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 1) { /*the green marker*/ line_strip.id = lineStripID; lineStripID++; marker_init(line_strip); line_strip.color = setcolor(0.0, 1.0, 0.0, 1.0); //green if (params::polysimp_allow) { line_strip.points.clear(); boost::geometry::clear(simplified); boost::geometry::simplify(line, simplified, params::polysimp); for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) { geometry_msgs::msg::Point p; p.x = boost::geometry::get<0>(*it); p.y = boost::geometry::get<1>(*it); p.z = params::polyz; line_strip.points.push_back(p); } } ma.markers.push_back(line_strip); //adding the line strip to the marker array line_strip.points.clear(); //These points are not needed anymore. boost::geometry::clear(line); /*The previous point is required for the next line strip aswell.*/ point.x = markerPointsArray[i - 1][0]; point.y = markerPointsArray[i - 1][1]; point.z = markerPointsArray[i - 1][2]; line_strip.points.push_back(point); line += xy(point.x,point.y); /*The current point is required for the next line strip aswell.*/ point.x = markerPointsArray[i][0]; point.y = markerPointsArray[i][1]; point.z = markerPointsArray[i][2]; line_strip.points.push_back(point); line += xy(point.x,point.y); } line_strip.lifetime = rclcpp::Duration(0, 0); // No timeout } if (params::zavg_allow) { for (int seg=0; seg < (int)ma.markers.size(); seg++) { for (int mz = 0; mz < (int)ma.markers[seg].points.size(); mz++) //setting the height of the polygon from the average height of points { ma.markers[seg].points[mz].z = zavg; } } } /*removal of obsolete markers*/ line_strip.action = visualization_msgs::msg::Marker::DELETE; for (int del = lineStripID; delpublish(ma); } for (j = 0; j < indexArray[10]; j++){ pt = array3D[10][j].p; cloud_filtered_ProbablyRoad.push_back(pt); } /*Road and High topic header*/ cloud_filtered_Road.header = cloud.header; cloud_filtered_ProbablyRoad.header = cloud.header; cloud_filtered_High.header = cloud.header; cloud_filtered_Box->header = cloud.header; /*Convert PCL cloud to ROS message and publish*/ sensor_msgs::msg::PointCloud2 cloud_msg_road; sensor_msgs::msg::PointCloud2 cloud_msg_high; sensor_msgs::msg::PointCloud2 cloud_msg_box; sensor_msgs::msg::PointCloud2 cloud_msg_prob; pcl::toROSMsg(cloud_filtered_Road, cloud_msg_road); pcl::toROSMsg(cloud_filtered_High, cloud_msg_high); pcl::toROSMsg(*cloud_filtered_Box, cloud_msg_box); pcl::toROSMsg(cloud_filtered_ProbablyRoad, cloud_msg_prob); cloud_msg_road.header.frame_id = params::fixedFrame; cloud_msg_high.header.frame_id = params::fixedFrame; cloud_msg_box.header.frame_id = params::fixedFrame; cloud_msg_prob.header.frame_id = params::fixedFrame; cloud_msg_road.header.stamp = this->now(); cloud_msg_high.header.stamp = this->now(); cloud_msg_box.header.stamp = this->now(); cloud_msg_prob.header.stamp = this->now(); /*publishing*/ pub_road->publish(cloud_msg_road); //filtered points (driveable road) pub_high->publish(cloud_msg_high); //filtered points (non-driveable road) pub_box->publish(cloud_msg_box); //filtered points (non-road) pub_pobroad->publish(cloud_msg_prob); //filtered points (probably road) // Add detailed performance metrics at the end of the function, before publishing auto end_time = std::chrono::high_resolution_clock::now(); auto duration = std::chrono::duration_cast(end_time - start_time).count(); // Calculate percentages float road_percentage = (cloud.size() > 0) ? (cloud_filtered_Road.size() * 100.0 / cloud.size()) : 0; float probably_road_percentage = (cloud.size() > 0) ? (cloud_filtered_ProbablyRoad.size() * 100.0 / cloud.size()) : 0; float non_road_percentage = (cloud.size() > 0) ? (cloud_filtered_High.size() * 100.0 / cloud.size()) : 0; // Log statistics about the segmentation results RCLCPP_INFO(this->get_logger(), "Point cloud segmentation completed in %ld ms", duration); RCLCPP_INFO(this->get_logger(), "Statistics: Input points: %ld", cloud.size()); RCLCPP_INFO(this->get_logger(), "Road points: %ld (%.1f%%)", cloud_filtered_Road.size(), road_percentage); RCLCPP_INFO(this->get_logger(), "Probably road points: %ld (%.1f%%)", cloud_filtered_ProbablyRoad.size(), probably_road_percentage); RCLCPP_INFO(this->get_logger(), "Non-road points: %ld (%.1f%%)", cloud_filtered_High.size(), non_road_percentage); // Publish statistics as a ROS topic std_msgs::msg::Float32MultiArray stats_msg; stats_msg.data = { static_cast(cloud.size()), // Total points static_cast(cloud_filtered_Road.size()), // Road points static_cast(cloud_filtered_ProbablyRoad.size()), // Probably road points static_cast(cloud_filtered_High.size()), // Non-road points road_percentage, // Road percentage probably_road_percentage, // Probably road percentage non_road_percentage, // Non-road percentage static_cast(duration) // Processing time (ms) }; pub_stats->publish(stats_msg); pcl::PointCloud::Ptr cloud_xyz_Road (new pcl::PointCloud); pcl::copyPointCloud(cloud_filtered_Road, *cloud_xyz_Road); pcl::PointCloud::Ptr cloud_xyz_ProbablyRoad (new pcl::PointCloud); pcl::copyPointCloud(cloud_filtered_ProbablyRoad, *cloud_xyz_ProbablyRoad); pcl::PointCloud::Ptr cloud_xyz_High (new pcl::PointCloud); pcl::copyPointCloud(cloud_filtered_High, *cloud_xyz_High); } ================================================ FILE: src/main.cpp ================================================ #include "urban_road_filter/data_structures.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char **argv) { // Initialize ROS 2 rclcpp::init(argc, argv); // Create detector node auto node = std::make_shared(); // Spin the node rclcpp::spin(node); // Clean up rclcpp::shutdown(); return 0; } ================================================ FILE: src/star_shaped_search.cpp ================================================ /* starShapedSearch algorithm (by László Csaplár) description: a complementary algorithm for roadside detection, part of the "urban_road_filter" package */ #include "urban_road_filter/data_structures.hpp" int rep = 360; //number of detection beams (how many parts/sectors will the pointcloud be divided along the angular direction -> one beam per sector) float width = 0.2; //width of beams float Kfi; //internal parameter, for assigning the points to their corresponding sectors ( = 1 / [2pi/rep] = 1 / [angle between neighboring beams] ) float params::angleFilter3; //"slope" parameter for edge detection (in degrees, given by 2 points, in radial direction/plane) float slope_param; //same but in radians float params::kdev_param; //coefficient: weighting the impact of deviation (difference) from average ( = overall sensitivity to changes) float params::kdist_param; //coefficient: weighting the impact of the distance between points ( = sensitivity for height error at close points) bool params::starbeam_filter; //toggle usage of rectangular beams for starshaped algorithm instead of the whole sector (containing the beam) int params::dmin_param; //minimal number of points required to begin adaptive evaluation std::vector beams(rep); //beams std::vector beamp(rep + 1); //pointers to the beams (+1 -> 0 AND 360) bool ptcmpr(polar a, polar b) //comparison by r-coordinates { return (a.r < b.r); } float slope(float x0, float y0, float x1, float y1) //get slope between 2 points given by x and y coordinates { return (y1 - y0) / (x1 - x0); } void Detector::beam_init() //beam initialization { { float fi, off = 0.5 * width; //temporary variables for (int i = 0; i < rep; i++) //for every beam... { fi = i * 2 * M_PI / rep; //angle of beam if (abs(tan(fi)) > 1) //"slope" of beam ( x <---> y ) { beams[i].yx = true; //aligning more with y-direction beams[i].d = tan(0.5 * M_PI - fi); // = 1/tan(fi) [coefficient] beams[i].o = abs(off / sin(fi)); //projection of half beam-width in the x-direction (how far from its centerline is the edge of the beam in the x-dir) } else { beams[i].yx = false; //aligning more with x-direction beams[i].d = tan(fi); //coefficient beams[i].o = abs(off / cos(fi)); //y-axis projection of half beam-width } beamp[i] = &beams[i]; //initializing the pointers } } /* for (int i = 0, j = 1; j < rep; i++, j++) //initializing the pointers to adjacent beams (currently unused/not needed) { beams[i].l = &beams[j]; beams[j].r = &beams[i]; } beams[0].r = &beams[rep-1]; beams[rep-1].l = &beams[0]; */ Kfi = rep / (2 * M_PI); //should be 2pi/rep, but then we would have to divide by it every time - using division only once and then multiplying later on should be somewhat faster (?) } void beamfunc(const int tid, std::vector &array2D) //beam algorithm (filtering, sorting, edge-/roadside detection) - input: beam ID (ordinal position/"which one" by angle), pointcloud (as std::vector, see: 'array2D' of 'lidarSegmentation') { int i = 0, s = beams[tid].p.size(); //loop variables float c; //temporary variable to simplify things if (params::starbeam_filter) { if (beams[tid].yx) //filtering the points lying outside the area of the beam... (case 1/2, y-direction) { while (i < s) //iterating through points in the current sector (instead of a for loop - since 's' is not constant and 'i' needs to be incremented conditionally) { c = beams[tid].d * array2D[beams[tid].p[i].id].p.y; //x-coordinate of the beam's centerline at the point (at the "height" of its y-coordinate) if ((c - beams[tid].o) < array2D[beams[tid].p[i].id].p.x && array2D[beams[tid].p[i].id].p.x < (c + beams[tid].o)) //whether it is inside the beam (by checking only x values on the line/"height" of the point's y-coordinate: whether the [x-coordinate of the] point falls between the [x-coordinates of the] two sides/borders of the beam { i++; //okay, next one } else //if outside the area { beams[tid].p.erase(beams[tid].p.begin() + i); //remove point s--; //the size has shrunk because of the deletion of a point (and its place is taken by the next point, so 'i' does not need to be changed) } } } else //same but with x and y swapped (case 2/2, x-direction) { while (i < s) { c = beams[tid].d * array2D[beams[tid].p[i].id].p.x; if ((c - beams[tid].o) < array2D[beams[tid].p[i].id].p.y && array2D[beams[tid].p[i].id].p.y < (c + beams[tid].o)) { i++; } else { beams[tid].p.erase(beams[tid].p.begin() + i); s--; } } } } std::sort(beams[tid].p.begin(), beams[tid].p.end(), ptcmpr); //sorting points by r-coordinate (radius) { //edge detection (edge of the roadside) if (s > 1) //for less than 2 points it would be pointless { float kdev = params::kdev_param; //coefficient: weighting the impact of deviation (difference) from average ( = overall sensitivity to changes) float kdist = params::kdist_param; //coefficient: weighting the impact of the distance between points ( = sensitivity for height error at close points) int dmin = params::dmin_param; //minimal number of points required to begin adaptive evaluation float avg = 0, dev = 0, nan = 0; //average value and absolute average deviation of the slope for adaptive detection + handling Not-a-Number values float ax, ay, bx, by, slp; //temporary variables (points 'a' and 'b' + slope) bx = beams[tid].p[0].r; //x = r-coordinate of the first point (radial position) by = array2D[beams[tid].p[0].id].p.z; //y = z-coordinate of the first point (height) for (int i = 1; i < s; i++) //edge detection based on the slope between point a and b { //updating points (a=b, b=next) ax = bx; bx = beams[tid].p[i].r; ay = by; by = array2D[beams[tid].p[i].id].p.z; slp = slope(ax, ay, bx, by); if (isnan(slp)) nan++; //Not-a-Number correction else //calculating (updating) average and deviation { avg *= i - nan - 1; //"unpacking" average value (average -> sum, + NaN correction) avg += slp; //insertion of new element avg *= 1 / (i - nan); //sum -> average dev *= i - nan - 1; //Absolute Average Deviation -> sum ("unpacking") dev += abs(slp - avg); //insertion of new element (absolute difference from average) dev *= 1 / (i - nan); //sum -> AAD } if ( slp > slope_param || //evaluation of the slope -> using a constant + adaptive: (i > dmin && (slp * slp - avg * avg) * kdev * ((bx - ax) * kdist) > dev) //if sufficient number of points: does the (weighted) "squared difference" from average - corrected with... ) //... the (weighted) distance of adjacent points - exceed the average absolute deviation? { array2D[beams[tid].p[i].id].isCurbPoint = 2; //the point in the 2D array gets marked as curbpoint break; //(the roadside is found, time to break the loop) } } } } beams[tid].p.clear(); //evaluation done, the points are no longer needed } void Detector::starShapedSearch(std::vector &array2D) //entry point to the code, everything gets called here (except for initialization - that needs to be called separately, at the start of the program - "beam_init()") { beamp.push_back(&beams[0]); //initializing "360 deg = 0 deg" pointer int f, s = array2D.size(); //temporary variables float r, fi; //polar coordinates slope_param = params::angleFilter3 * (M_PI / 180); for (int i = 0; i < s; i++) //points to polar coordinate-system + sorting into sectors { r = sqrt(array2D[i].p.x * array2D[i].p.x + array2D[i].p.y * array2D[i].p.y); //r = sqRoot(x^2+y^2) = distance of point from sensor fi = atan2(array2D[i].p.y, array2D[i].p.x); //angular position of point if (fi < 0) fi += 2 * M_PI; //handling negative values (-180...+180 -> 0...360) f = (int)(fi * Kfi); //which one of the beams (sectors, containing the beam) does it fall into beamp[f]->p.push_back(polar{i, r, fi}); //adding the point to the 'f'-th beam (still unfiltered) } beamp.pop_back(); //removing pointer (to prevent "double free" error) for (int i = 0; i < rep; i++) //for every beam... { beamfunc(i, array2D); //the heart of the starshaped method (beam algorithm) } } ================================================ FILE: src/x_zero_method.cpp ================================================ #include "urban_road_filter/data_structures.hpp" float params::angleFilter1; //angle given by three points, while keeping X = 0 float params::curbHeight; //estimated minimal height of the curb int params::curbPoints; //estimated number of points on the curb void Detector::xZeroMethod(std::vector>& array3D,int index,int* indexArray){ /*-- step 1.: filtering the NON-road points --*/ int p2, p3; //2nd and 3rd of the points that are being examined /* --> alpha - angle between the three points and the two vectors --> x1, x2, x3 - length of the sides of the triangle given by the three points --> curbPoints - estimated number of points on the roadside --> d - distance between the two outermost points - the rotation of the LiDAR and arc gaps make it necessary d - this variable also gets used later on in the code */ float alpha, x1, x2, x3, d, bracket; for (int i = 0; i < index; i++) { /*assigning new Y values while keeping X = 0 */ for (int j = 1; j < indexArray[i]; j++) { array3D[i][j].newY = array3D[i][j-1].newY + 0.0100; } /*evaluation of the points in an arc - x-zero method*/ for (int j = params::curbPoints; j <= (indexArray[i] - 1) - params::curbPoints; j++) { p2 = j + params::curbPoints / 2; p3 = j + params::curbPoints; d = sqrt( pow(array3D[i][p3].p.x - array3D[i][j].p.x , 2) + pow(array3D[i][p3].p.y - array3D[i][j].p.y , 2)); /*set the distance to be less than 5 meters*/ if (d < 5.0000) { x1 = sqrt( pow(array3D[i][p2].newY - array3D[i][j].newY , 2) + pow(array3D[i][p2].p.z - array3D[i][j].p.z , 2)); x2 = sqrt( pow(array3D[i][p3].newY - array3D[i][p2].newY, 2) + pow(array3D[i][p3].p.z - array3D[i][p2].p.z , 2)); x3 = sqrt( pow(array3D[i][p3].newY - array3D[i][j].newY , 2) + pow(array3D[i][p3].p.z - array3D[i][j].p.z , 2)); bracket = (pow(x3, 2) - pow(x1, 2) - pow(x2, 2)) / (-2 * x1 * x2); if (bracket < -1) bracket = -1; else if (bracket > 1) bracket = 1; alpha = acos(bracket) * 180 / M_PI; /*condition and assignment to group*/ if (alpha <= params::angleFilter1 && (abs(array3D[i][j].p.z - array3D[i][p2].p.z ) >= params::curbHeight || abs(array3D[i][p3].p.z - array3D[i][p2].p.z ) >= params::curbHeight) && abs(array3D[i][j].p.z - array3D[i][p3].p.z ) >= 0.05) { array3D[i][p2].isCurbPoint = 2; } } } } } ================================================ FILE: src/z_zero_method.cpp ================================================ #include "urban_road_filter/data_structures.hpp" float params::angleFilter2; //angle between two vectors, while keeping Z = 0 void Detector::zZeroMethod(std::vector>& array3D,int index,int* indexArray){ /*-- step 1.: filtering the NON-road points --*/ /* --> alpha - angle between the three points and the two vectors --> x1, x2, x3 - length of the sides of the triangle given by the three points --> curbPoints - estimated number of points on the roadside --> va1, va2, vb1, vb2 - the two vectors --> max1, max2 - not only angle, but height needs to be checked as well --> d - distance between the two outermost points - the rotation of the LiDAR and arc gaps make it necessary d - this variable also gets used later on in the code */ float alpha, x1, x2, x3, va1, va2, vb1, vb2, max1, max2, d, bracket; for (int i = 0; i < index; i++){ /*evaluation of points in an arc - z-zero method*/ for (int j = params::curbPoints; j <= (indexArray[i] - 1) - params::curbPoints; j++) { d = sqrt( pow(array3D[i][j+params::curbPoints].p.x - array3D[i][j-params::curbPoints].p.x, 2) + pow(array3D[i][j+params::curbPoints].p.y - array3D[i][j-params::curbPoints].p.y, 2)); /*set the distance to be less than 5 meters*/ if (d < 5.0000) { /*initialization*/ max1 = max2 = abs(array3D[i][j].p.z); va1 = va2 = vb1 = vb2 = 0; /*initializing vector 'a' and maximal height*/ for (int k = j - 1; k >= j - params::curbPoints; k--) { va1 = va1 + (array3D[i][k].p.x - array3D[i][j].p.x); va2 = va2 + (array3D[i][k].p.y - array3D[i][j].p.y); if (abs(array3D[i][k].p.z) > max1) max1 = abs(array3D[i][k].p.z); } /*initializing vector 'b' and maximal height*/ for (int k = j + 1; k <= j + params::curbPoints; k++) { vb1 = vb1 + (array3D[i][k].p.x - array3D[i][j].p.x ); vb2 = vb2 + (array3D[i][k].p.y - array3D[i][j].p.y ); if (abs(array3D[i][k].p.z ) > max2) max2 = abs(array3D[i][k].p.z); } va1 = (1 / (float)params::curbPoints) * va1; va2 = (1 / (float)params::curbPoints) * va2; vb1 = (1 / (float)params::curbPoints) * vb1; vb2 = (1 / (float)params::curbPoints) * vb2; bracket = (va1 * vb1 + va2 * vb2) / (sqrt(pow(va1, 2) + pow(va2, 2)) * sqrt(pow(vb1, 2) + pow(vb2, 2))); if (bracket < -1) bracket = -1; else if (bracket > 1) bracket = 1; alpha = acos(bracket) * 180 / M_PI; /*condition and assignment to group*/ if (alpha <= params::angleFilter2 && (max1 - abs(array3D[i][j].p.z ) >= params::curbHeight || max2 - abs(array3D[i][j].p.z) >= params::curbHeight) && abs(max1 - max2) >= 0.05) { array3D[i][j].isCurbPoint = 2; } } } } }