Refactor

CMakeLists.txt

@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(urban_road_filter)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+## Compile as C++17, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17 -O2)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -110,8 +110,8 @@ generate_dynamic_reconfigure_options(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES urban_road_filter
+  INCLUDE_DIRS include
+  LIBRARIES urban_road_filter
 #  CATKIN_DEPENDS nav_msgs roscpp rospy std_msgs
 #  DEPENDS system_lib
 )
@@ -124,9 +124,11 @@ catkin_package(
 ## Your package locations should be listed before other locations
 include_directories(
 # include
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
-add_executable(lidar_road src/lidarSegmentation.cpp)
+add_executable(lidar_road 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)
 add_dependencies(lidar_road ${PROJECT_NAME}_gencfg)
 target_link_libraries(lidar_road ${catkin_LIBRARIES})

README.md

@@ -69,7 +69,7 @@ If you use any of this code please consider citing the [paper](https://www.mdpi.
 }
 ```
 
-# Realated solutions
+# 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)

cfg/LidarFilters.cfg

@@ -1,24 +1,25 @@
 #!/usr/bin/env python
-PACKAGE = "lidar_filters_pkg"
+PACKAGE = "urban_road_filter"
 
 from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-#Parameter lista.
+#Parameter list.
 #Fixed frame.
 gen.add("fixed_frame", str_t, 0, "Fixed frame. (Restart needed if it changes.)", "left_os1/os1_lidar")
 
-#A topic, amire felszeretnenk iratkozni.
+#The desired topic to subscibe to.
 gen.add("topic_name", str_t, 0, "(Restart needed if it changes.)", "/left_os1/os1_cloud_node/points")
 
-#Modszerek engedelyezese.
+#Enabling individual detection methods.
 gen.add("x_zero_method", bool_t, 0, "x zero method.", True)
 gen.add("z_zero_method", bool_t, 0, "z zero method.", True)
 gen.add("star_shaped_method", bool_t, 0, "star shaped method", True)
 gen.add("blind_spots", bool_t, 0, "Filtering blind spots.", True)
 
 #Vakfolt (+-,+,-) X iranyu vizsgalata.
+#Blindspot detection (x-direction)
 size_enum = gen.enum([gen.const("bothX", int_t, 0, "Filtering in both negative and positive direction along the Lidars X axis"),
                     gen.const("positiveX", int_t, 1, "Filtering only in +X ."),
                     gen.const("negativeX", int_t, 2, "Filtering only in -X.")],
@@ -79,4 +80,4 @@ gen.add("poly_z_manual", double_t, 0, "Set a constant polygon height", -1.5, -5,
 #Egyszerusitett polygon z-koordinatai atlagbol (CSP)
 gen.add("poly_z_avg_allow", bool_t, 0, "Set polygon height to average value", True)
 
-exit(gen.generate(PACKAGE, "lidar_filters_pkg", "LidarFilters"))
+exit(gen.generate(PACKAGE, "urban_road_filter", "LidarFilters"))

include/urban_road_filter/data_structures.hpp

@@ -0,0 +1,142 @@
+#pragma once
+
+/*Basic includes.*/
+#include <stdio.h>
+#include <iostream>
+#include <algorithm>
+#include <math.h>
+#include <cmath>
+#include <vector>
+#include <memory>
+
+/*Includes for ROS.*/
+#include <ros/ros.h>
+
+/*Includes for Markers.*/
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+/*Includes for GUI.*/
+#include <dynamic_reconfigure/server.h>
+#include <urban_road_filter/LidarFiltersConfig.h>
+
+/*Includes for PCL.*/
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/conditional_removal.h>
+
+/*ramer-douglas-peucker*/
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/linestring.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/assign.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 int dmin_param;                 //(see below)
+  extern float kdev_param;               //(see below)
+  extern float kdist_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:
+    Detector(ros::NodeHandle* nh);
+
+    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 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);
+
+    private:
+    ros::Publisher pub_road;        
+    ros::Publisher pub_high;        
+    ros::Publisher pub_box;         
+    ros::Publisher pub_pobroad;    
+    ros::Publisher pub_marker;      
+
+    ros::Subscriber sub;
+
+    boost::geometry::model::linestring<xy> line;
+    boost::geometry::model::linestring<xy> simplified;
+};