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
<img src="img/urban_road_filter_anim01.gif" height=620/> <img src="img/urban_road_filter_static01.png" height=620/>
# Dependency
- [ROS 2](https://docs.ros.org/en/humble/index.html) (tested with [](https://docs.ros.org/en/humble/) and [](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
<details>
source ~/ros2_ws/install/setup.bash
</details>
```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
[<img src="img/yt_demo01.png" width=213/>](https://www.youtube.com/watch?v=T2qi4pldR-E)
[<img src="img/yt_tutorial01.png" width=213/>](https://www.youtube.com/watch?v=HHnj4VcbSy4)
[<img src="img/yt_demo02.png" width=213/>](https://www.youtube.com/watch?v=9tdzo2AyaHM)
[<img src="img/yt_demo03.png" width=213/>](https://www.youtube.com/watch?v=lp6q_QvWA-Y)
<img src="img/marker_poly01.png" width=440/>
<img src="img/marker_road_high01.png" width=440/>
<img src="img/marker_poly02.png" width=440/>
# ROS publications / subscriptions
```mermaid
flowchart LR
P[points]:::gray -->|sensor_msgs/PointCloud2| U([urban_road_filt</br>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: <Fixed 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: <Fixed 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
<img src="urban_road_filter_example01zoom.svg" width=280 />
<img src="urban_road_filter_anim01.gif" width=274/><img src="urban_road_filter_static01.png" width=274 />
================================================
FILE: include/urban_road_filter/data_structures.hpp
================================================
#pragma once
/*Basic includes.*/
#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <math.h>
#include <cmath>
#include <vector>
#include <memory>
#include <functional>
/*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 <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl_ros/transforms.hpp>
/*ROS 2 specific message types*/
#include "sensor_msgs/msg/point_cloud2.hpp"
/*ramer-douglas-peucker*/
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/assign.hpp>
#include "std_msgs/msg/float32_multi_array.hpp"
using namespace boost::assign;
typedef boost::geometry::model::d2::point_xy<float> 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<polar> 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 <typename PointT>
class FilteringCondition : public pcl::ConditionBase<PointT>
{
public:
typedef std::shared_ptr<FilteringCondition<PointT>> Ptr;
typedef std::shared_ptr<const FilteringCondition<PointT>> ConstPtr;
typedef std::function<bool(const PointT&)> FunctorT;
FilteringCondition(FunctorT evaluator):
pcl::ConditionBase<PointT>(),_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<std::vector<Point3D>>& array3D, int arc, int low, int high);
void quickSort(std::vector<std::vector<Point3D>>& array3D, int arc, int low, int high);
void filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud);
void starShapedSearch(std::vector<Point2D>& array2D);
void beam_init();
void xZeroMethod(std::vector<std::vector<Point3D>>& array3D, int index, int* indexArray);
void zZeroMethod(std::vector<std::vector<Point3D>>& array3D, int index, int* indexArray);
void blindSpots(std::vector<std::vector<Point3D>>& 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<rclcpp::Parameter> ¶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<sensor_msgs::msg::PointCloud2>::SharedPtr pub_road;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_high;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_box;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_pobroad;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr pub_stats; // Statistics publisher
// Subscriber
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::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<xy> line;
boost::geometry::model::linestring<xy> 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
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>urban_road_filter</name>
<version>1.0.0</version>
<description>ROS 2 Real-Time LIDAR-based urban road and sidewalk detection algorithm for autonomous vehicles</description>
<maintainer email="barhamfarraj@icloud.com">Barham Farraj</maintainer>
<maintainer email="abdelrahman97.alabdallah@gmail.com">AbdelrahmanAlabdallah</maintainer>
<author email="herno@sze.hu">Ernő Horváth</author>
<author>Claudiu Pozna</author>
<author>Miklós Unger</author>
<license>MIT</license>
<url type="website">https://github.com/jkk-research/urban_road_filter</url>
<url type="doi">https://doi.org/10.3390/s22010194</url>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>visualization_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<depend>rclcpp_components</depend>
<depend>libpcl-all-dev</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
================================================
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<std::vector<Point3D>>& 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<sensor_msgs::msg::PointCloud2>(
params::topicName,
rclcpp::SensorDataQoS(),
std::bind(&Detector::pointCloudCallback, this, std::placeholders::_1));
/*publishing filtered points*/
pub_road = this->create_publisher<sensor_msgs::msg::PointCloud2>("road", 10);
pub_high = this->create_publisher<sensor_msgs::msg::PointCloud2>("curb", 10);
pub_box = this->create_publisher<sensor_msgs::msg::PointCloud2>("roi", 10); // ROI - region of interest
pub_pobroad = this->create_publisher<sensor_msgs::msg::PointCloud2>("road_probably", 10);
pub_marker = this->create_publisher<visualization_msgs::msg::MarkerArray>("road_marker", 10);
pub_stats = this->create_publisher<std_msgs::msg::Float32MultiArray>("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<pcl::PointXYZI> 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<rclcpp::Parameter> ¶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<std::vector<Point3D>>& 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<std::vector<Point3D>>& 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<pcl::PointXYZI> &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<pcl::PointCloud<pcl::PointXYZI>>(cloud); //all points in the detection area
pcl::PointCloud<pcl::PointXYZI> cloud_filtered_Road; //filtered points (driveable road)
pcl::PointCloud<pcl::PointXYZI> cloud_filtered_ProbablyRoad; //filtered points (non-driveable road)
pcl::PointCloud<pcl::PointXYZI> cloud_filtered_High; //filtered points (non-road)
auto filterCondition = std::make_shared<FilteringCondition<pcl::PointXYZI>>(
[=](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<pcl::PointXYZI> 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<Point2D> 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<std::vector<Point3D>> array3D(channels,std::vector<Point3D>(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<xy>::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<xy>::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<xy>::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; del<ghostcount; del++)
{
line_strip.id++;
ma.markers.push_back(line_strip);
}
ghostcount = lineStripID;
/*publishing the marker array*/
pub_marker->publish(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<std::chrono::milliseconds>(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<float>(cloud.size()), // Total points
static_cast<float>(cloud_filtered_Road.size()), // Road points
static_cast<float>(cloud_filtered_ProbablyRoad.size()), // Probably road points
static_cast<float>(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<float>(duration) // Processing time (ms)
};
pub_stats->publish(stats_msg);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_Road (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(cloud_filtered_Road, *cloud_xyz_Road);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_ProbablyRoad (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(cloud_filtered_ProbablyRoad, *cloud_xyz_ProbablyRoad);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_High (new pcl::PointCloud<pcl::PointXYZ>);
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<Detector>();
// 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<box> beams(rep); //beams
std::vector<box *> 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<Point2D> &array2D) //beam algorithm (filtering, sorting, edge-/roadside detection) - input: beam ID (ordinal position/"which one" by angle), pointcloud (as std::vector<Point2D>, 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<Point2D> &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<std::vector<Point3D>>& 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<std::vector<Point3D>>& 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;
}
}
}
}
}
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
SYMBOL INDEX (16 symbols across 5 files)
FILE: include/urban_road_filter/data_structures.hpp
type Point2D (line 42) | struct Point2D{
type Point3D (line 49) | struct Point3D:public Point2D{
type polar (line 53) | struct polar //polar-coordinate struct for the points
type box (line 60) | struct box //struct for detection beams
type params (line 68) | namespace params{
class FilteringCondition (line 93) | class FilteringCondition : public pcl::ConditionBase<PointT>
method FilteringCondition (line 100) | FilteringCondition(FunctorT evaluator):
method evaluate (line 104) | virtual bool evaluate (const PointT &point) const {
class Detector (line 112) | class Detector : public rclcpp::Node {
FILE: launch/urban_road_filter.launch.py
function generate_launch_description (line 8) | def generate_launch_description():
FILE: src/lidar_segmentation.cpp
function marker_init (line 25) | void marker_init(visualization_msgs::msg::Marker& m)
function setcolor (line 41) | inline std_msgs::msg::ColorRGBA setcolor(float r, float g, float b, floa...
FILE: src/main.cpp
function main (line 4) | int main(int argc, char **argv)
FILE: src/star_shaped_search.cpp
function ptcmpr (line 22) | bool ptcmpr(polar a, polar b) //comparison by r-coordinates
function slope (line 27) | float slope(float x0, float y0, float x1, float y1) //get slope between ...
function beamfunc (line 68) | void beamfunc(const int tid, std::vector<Point2D> &array2D) //beam algor...
Condensed preview — 16 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (100K chars).
[
{
"path": "CITATION.cff",
"chars": 916,
"preview": "cff-version: 1.2.0\nmessage: \"If you use this software, please cite it as below.\"\nauthors:\n- family-names: \"Horváth\"\n gi"
},
{
"path": "CMakeLists.txt",
"chars": 1414,
"preview": "cmake_minimum_required(VERSION 3.8)\nproject(urban_road_filter)\n\n# Default to C++17\nif(NOT CMAKE_CXX_STANDARD)\n set(CMAK"
},
{
"path": "LICENSE",
"chars": 1546,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2021, JKK - Vehicle Industry Research Center\nAll rights reserved.\n\nRedistribution an"
},
{
"path": "README.md",
"chars": 5433,
"preview": "# `urban_road_filter`: a real-time LIDAR-based urban road and sidewalk detection algorithm for autonomous vehicles\n\n<img"
},
{
"path": "config/parameters.yaml",
"chars": 2059,
"preview": "# This file consolidates all parameters for the urban_road_filter package.\nurban_road_filter:\n ros__parameters:\n fix"
},
{
"path": "config/urban_road_filter.rviz",
"chars": 7829,
"preview": "Panels:\n - Class: rviz_common/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:"
},
{
"path": "img/README.md",
"chars": 387,
"preview": "# Images to help understand the algoritm better\n\n\n\n![](urban_road"
},
{
"path": "include/urban_road_filter/data_structures.hpp",
"chars": 7288,
"preview": "#pragma once\r\n\r\n/*Basic includes.*/\r\n#include <stdio.h>\r\n#include <iostream>\r\n#include <algorithm>\r\n#include <math.h>\r\n#"
},
{
"path": "launch/urban_road_filter.launch.py",
"chars": 2257,
"preview": "from launch import LaunchDescription\nfrom launch_ros.actions import Node\nfrom launch.actions import DeclareLaunchArgumen"
},
{
"path": "package.xml",
"chars": 1281,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "src/blind_spots.cpp",
"chars": 10793,
"preview": "#include \"urban_road_filter/data_structures.hpp\"\n\nint params::xDirection; //direction of the blindspot cutoff\nbool p"
},
{
"path": "src/lidar_segmentation.cpp",
"chars": 38349,
"preview": "#include \"urban_road_filter/data_structures.hpp\"\n\n/*Global variables.*/\nint channels = 64; //Th"
},
{
"path": "src/main.cpp",
"chars": 332,
"preview": "#include \"urban_road_filter/data_structures.hpp\"\n#include \"rclcpp/rclcpp.hpp\"\n\nint main(int argc, char **argv)\n{\n // In"
},
{
"path": "src/star_shaped_search.cpp",
"chars": 10079,
"preview": "/* starShapedSearch algorithm\n (by László Csaplár)\n\n description: a complementary algorithm for roadside detectio"
},
{
"path": "src/x_zero_method.cpp",
"chars": 2962,
"preview": "#include \"urban_road_filter/data_structures.hpp\"\n\nfloat params::angleFilter1; //angle given by three points, while k"
},
{
"path": "src/z_zero_method.cpp",
"chars": 3375,
"preview": "#include \"urban_road_filter/data_structures.hpp\"\n\nfloat params::angleFilter2; //angle between two vectors, while kee"
}
]
About this extraction
This page contains the full source code of the jkk-research/urban_road_filter GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 16 files (94.0 KB), approximately 25.2k tokens, and a symbol index with 16 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.