diff --git a/CMakeLists.txt b/CMakeLists.txt index 803ad1d..78369b0 100644 --- a/CMakeLists.txt +++ b/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}) \ No newline at end of file diff --git a/README.md b/README.md index 507ab87..54c2cad 100644 --- a/README.md +++ b/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) diff --git a/cfg/LidarFilters.cfg b/cfg/LidarFilters.cfg index fa1dab4..c54174b 100755 --- a/cfg/LidarFilters.cfg +++ b/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")) \ No newline at end of file +exit(gen.generate(PACKAGE, "urban_road_filter", "LidarFilters")) diff --git a/include/urban_road_filter/data_structures.hpp b/include/urban_road_filter/data_structures.hpp new file mode 100644 index 0000000..2e9ffd5 --- /dev/null +++ b/include/urban_road_filter/data_structures.hpp @@ -0,0 +1,142 @@ +#pragma once + +/*Basic includes.*/ +#include +#include +#include +#include +#include +#include +#include + +/*Includes for ROS.*/ +#include + +/*Includes for Markers.*/ +#include +#include + +/*Includes for GUI.*/ +#include +#include + +/*Includes for PCL.*/ +#include +#include +#include +#include +#include + +/*ramer-douglas-peucker*/ +#include +#include +#include +#include + +using namespace boost::assign; + +typedef boost::geometry::model::d2::point_xy xy; + +struct Point2D{ + pcl::PointXYZI p; + float d; + float alpha; + short isCurbPoint; +}; + +struct Point3D:public Point2D{ + float newY; +}; + +struct polar //polar-coordinate struct for the points +{ + int id; //original ID of point (from input cloud) + float r; //radial coordinate + float fi; //angular coordinate (ccw angle from x-axis) +}; + +struct box //struct for detection beams +{ + std::vector p; //points within the beam's area + box *l, *r; //pointer to adjacent beams (currently not used) + bool yx; //whether it is aligned more with the y-axis (than the x-axis) + float o, d; //internal parameters (trigonometry) +}; + +namespace params{ + extern std::string fixedFrame; /* Fixed Frame.*/ + extern std::string topicName; /* subscribed topic.*/ + extern bool x_zero_method, z_zero_method, star_shaped_method ; /*Methods of roadside detection*/ + extern bool blind_spots; /*Vakfolt javító algoritmus.*/ + extern int xDirection; /*A vakfolt levágás milyen irányú.*/ + extern float interval; /*A LIDAR vertikális szögfelbontásának, elfogadott intervalluma.*/ + extern float curbHeight; /*Becsült minimum szegély magasság.*/ + extern int curbPoints; /*A pontok becsült száma, a szegélyen.*/ + extern float beamZone; /*A vizsgált sugárzóna mérete.*/ + extern float angleFilter1; /*X = 0 érték mellett, három pont által bezárt szög.*/ + extern float angleFilter2; /*Z = 0 érték mellett, két vektor által bezárt szög.*/ + extern float angleFilter3; /*Csaplár László kódjához szükséges. Sugár irányú határérték (fokban).*/ + extern float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsgált terület méretei.*/ + extern 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 +class FilteringCondition : public pcl::ConditionBase +{ +public: + typedef std::shared_ptr> Ptr; + typedef std::shared_ptr> ConstPtr; + typedef std::function FunctorT; + + FilteringCondition(FunctorT evaluator): + pcl::ConditionBase(),_evaluator( evaluator ) + {} + + virtual bool evaluate (const PointT &point) const { + // just delegate ALL the work to the injected std::function + return _evaluator(point); + } +private: + FunctorT _evaluator; +}; + +class Detector{ + public: + Detector(ros::NodeHandle* nh); + + int partition(std::vector>& array3D, int arc,int low, int high); + + void quickSort(std::vector>& array3D, int arc, int low, int high); + + //void filtered(const pcl::PointCloud &cloud); + + void filtered(const pcl::PointCloud &cloud); + + void starShapedSearch(std::vector& array2D); + + void beam_init(); + + void xZeroMethod(std::vector>& array3D,int index,int* indexArray); + + void zZeroMethod(std::vector>& array3D,int index,int* indexArray); + + void blindSpots(std::vector>& array3D,int index,int* indexArray,float* maxDistance); + + 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 line; + boost::geometry::model::linestring simplified; +}; \ No newline at end of file diff --git a/src/blind_spots.cpp b/src/blind_spots.cpp new file mode 100644 index 0000000..62f3caf --- /dev/null +++ b/src/blind_spots.cpp @@ -0,0 +1,284 @@ +#include "urban_road_filter/data_structures.hpp" + +int params::xDirection; //direction of the blindspot cutoff +bool params::blind_spots; //enable blindspot correction +float params::beamZone; //size of hte examined beamzone + +void Detector::blindSpots(std::vector>& array3D,int index,int* indexArray,float* maxDistance){ + /*Blind spot detection: + We examine the second arc. (First one gives inaccurate results.) + The intervals (segments) [90°-180° --- 180°-270°] and [0°-90° --- 270°-360°] are of greatest significance (at the two sides of the car). + We search for 2 pieces of high points in both intervals. + If there are two high points on the first arc in the interval, then the area between has a high chance of being a blind spot.*/ + float q1 = 0, q2 = 180, q3 = 180, q4 = 360; //the four segments (quarters) of the arc + int c1 = -1, c2 = -1, c3 = -1, c4 = -1; //ID of the points found on the first 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; + c1 = i; + } + } + else if (array3D[1][i].alpha >= 90 && array3D[1][i].alpha < 180) + { + if (array3D[1][i].alpha < q2) + { + q2 = array3D[1][i].alpha; + c2 = i; + } + } + else if (array3D[1][i].alpha >= 180 && array3D[1][i].alpha < 270) + { + if (array3D[1][i].alpha > q3) + { + q3 = array3D[1][i].alpha; + c3 = i; + } + } + else + { + if (array3D[1][i].alpha < q4) + { + q4 = array3D[1][i].alpha; + c4 = i; + } + } + } + } + } + + 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; + } + } + } + } + } + } +} \ No newline at end of file diff --git a/src/lidarSegmentation.cpp b/src/lidarSegmentation.cpp deleted file mode 100644 index c14d09d..0000000 --- a/src/lidarSegmentation.cpp +++ /dev/null @@ -1,1211 +0,0 @@ -/*Basic includes.*/ -#include -#include -#include -#include -#include -#include - -/*Includes for ROS.*/ -#include - -/*Includes for Markers.*/ -#include -#include - -/*Includes for GUI.*/ -#include -#include - -/*Includes for PCL.*/ -#include -#include -#include -#include - - -#include "starShapedSearch.cpp" - -/*ramer-douglas-peucker*/ -#include -#include -#include -#include - -using namespace boost::assign; - -typedef boost::geometry::model::d2::point_xy xy; -boost::geometry::model::linestring line; -boost::geometry::model::linestring simplified; - - -/*Global variables.*/ -int channels = 64; /* The number of channels of the LIDAR .*/ -std::string fixedFrame; /* Fixed Frame.*/ -std::string topicName; /* subscribed topic.*/ -bool x_zero_method, y_zero_method, star_shaped_method ; /*Methods of roadside detection*/ -bool blind_spots; /*Vakfolt javító algoritmus.*/ -int xDirection; /*A vakfolt levágás milyen irányú.*/ -float interval; /*A LIDAR vertikális szögfelbontásának, elfogadott intervalluma.*/ -float curbHeight; /*Becsült minimum szegély magasság.*/ -int curbPoints; /*A pontok becsült száma, a szegélyen.*/ -float beamZone; /*A vizsgált sugárzóna mérete.*/ -float angleFilter1; /*X = 0 érték mellett, három pont által bezárt szög.*/ -float angleFilter2; /*Z = 0 érték mellett, két vektor által bezárt szög.*/ -float angleFilter3; /*Csaplár László kódjához szükséges. Sugár irányú határérték (fokban).*/ -float min_X, max_X, min_Y, max_Y, min_Z, max_Z; /*A vizsgált terület méretei.*/ - -bool polysimp_allow = true; /*polygon-eygszerűsítés engedélyezése*/ -bool zavg_allow = true; /*egyszerűsített polygon z-koordinátái átlagból (engedély)*/ -float polysimp = 0.5; /*polygon-egyszerűsítési tényező (Ramer-Douglas-Peucker)*/ -float polyz = -1.5; /*manuálisan megadott z-koordináta (polygon)*/ - -int ghostcount = 0; /* segédváltozó az elavult markerek (ghost) eltávolításához */ - -int cols = 8; /* number of columns in the 3D array */ -int cols2d = 7; /* number of columns in the 2D array */ - - -/*A paraméterek beállítása.*/ -void paramsCallback(lidar_filters_pkg::LidarFiltersConfig &config, uint32_t level) -{ - fixedFrame = config.fixed_frame; - topicName = config.topic_name; - x_zero_method = config.x_zero_method; - y_zero_method = config.z_zero_method; - star_shaped_method = config.star_shaped_method ; - blind_spots = config.blind_spots; - xDirection = config.xDirection; - interval = config.interval; - curbHeight = config.curb_height; - curbPoints = config.curb_points; - beamZone = config.beamZone; - angleFilter1 = config.cylinder_deg_x; - angleFilter2 = config.cylinder_deg_z; - angleFilter3 = config.sector_deg; - min_X = config.min_x; - max_X = config.max_x; - min_Y = config.min_y; - max_Y = config.max_y; - min_Z = config.min_z; - max_Z = config.max_z; - dmin_param = config.dmin_param; - kdev_param = config.kdev_param; - kdist_param = config.kdist_param; - polysimp_allow = config.simple_poly_allow; - polysimp = config.poly_s_param; - zavg_allow = config.poly_z_avg_allow; - polyz = config.poly_z_manual; - ROS_INFO("Updated params %s", ros::this_node::getName().c_str()); -} - -/*Publish.*/ -ros::Publisher pub_road; /*Szűrt pontok (járható úttest).*/ -ros::Publisher pub_high; /*Szűrt pontok (nem járható úttest).*/ -ros::Publisher pub_box; /*Szűrt pontok (nem úttest).*/ -ros::Publisher pub_pobroad; /*A vizsgált terület, összes pontja.*/ -ros::Publisher pub_marker; /*Marker.*/ - -/*FÜGGVÉNYEK*/ -/*Rekurziv, gyors rendező függvény. (1/3)*/ -void swap(float *a, float *b) -{ - float t = *a; - *a = *b; - *b = t; -} - -/*Rekurziv, gyors rendező függvény. (2/3)*/ -int partition(float *array3D, int arc, int piece, int low, int high) -{ - float pivot = *(array3D + arc * piece * cols + high * cols + 4); - int i = (low - 1); - for (int j = low; j <= high - 1; j++) - { - if (*(array3D + arc * piece * cols + j * cols + 4) < pivot) - { - i++; - for(int sw = 0; sw < cols; sw++){ - swap(&*(array3D + arc * piece * cols + i * cols + sw), &*(array3D + arc * piece * cols + j * cols + sw)); - } - } - } - for(int sw = 0; sw < cols; sw++){ - swap(&*(array3D + arc * piece * cols + (i + 1) * cols + sw), &*(array3D + arc * piece * cols + high * cols + sw)); - } - return (i + 1); -} - -/*Rekurziv, gyors rendező függvény. (3/3)*/ -void quickSort(float *array3D, int arc, int piece, int low, int high) -{ - if (low < high) - { - int pi = partition(array3D, arc, piece, low, high); - quickSort(array3D, arc, piece, low, pi - 1); - quickSort(array3D, arc, piece, pi + 1, high); - } -} - -/*Ez a függvény végzi a szűrést.*/ -void filtered(const pcl::PointCloud &cloud) -{ - /*Segédváltozók, a "for" ciklusokhoz.*/ - int i, j, k, l; - - pcl::PointXYZI pt; /*Egy db pont tárolásához szükséges.*/ - pcl::PointCloud cloud_filtered_Road; /*Szűrt pontok (járható úttest).*/ - pcl::PointCloud cloud_filtered_ProbablyRoad; /*Szűrt pontok (nem járható úttest).*/ - pcl::PointCloud cloud_filtered_High; /*Szűrt pontok (nem úttest).*/ - pcl::PointCloud cloud_filtered_Box; /*A vizsgált terület, összes pontja.*/ - - /*A "cloud_filtered_Box" topic feltöltése a pontokkal. - Végigmegyünk az input felhőn, és azokat a pontokat, melyek megfelelnek a feltételeknek, - hozzáadjuk a "cloud_filtered_Box" topichoz.*/ - for (i = 0; i <= cloud.size(); i++) - { - if (cloud.points[i].x >= min_X && cloud.points[i].x <= max_X && - cloud.points[i].y >= min_Y && cloud.points[i].y <= max_Y && - cloud.points[i].z >= min_Z && cloud.points[i].z <= max_Z && - cloud.points[i].x + cloud.points[i].y + cloud.points[i].z != 0) - { - pt.x = cloud.points[i].x; - pt.y = cloud.points[i].y; - pt.z = cloud.points[i].z; - pt.intensity = cloud.points[i].intensity; - cloud_filtered_Box.push_back(pt); - } - } - - /*A pontok darabszáma, a vizsgált területen.*/ - int piece = cloud_filtered_Box.points.size(); - - /*Minimum 30 pont legyen a vizsgált területen, különben programhibák lesznek. - Illetve nincs is értelme vizsgálni ilyen kevés pontot.*/ - if (piece >= 30) - { - /*Csaplár László kódjának meghívása és a szükséges határérték beállítása.*/ - if (star_shaped_method ) - { - slope_param = angleFilter3 * (M_PI / 180); - callback(cloud_filtered_Box); - } - - /* - 2D tömb: - - A pontok értékei: (0: X, 1: Y, 2: Z), - - A pontok origótól vett távolsága: (3: D), - - A pontok szögfelbontása: (4: Alpha), - - Csaplár László kódja alapján felismert szegély pontok: Csoportszámok (5: Road = 1, High = 2). - - intenzitás-értékek (6: intensity) - */ - float *array2D = new float[piece * cols2d](); - - /*A szögfüggvények, zárójelbeli értékeinek tárolásához.*/ - float bracket; - - /*Egy 1D tömb, melyben a különböző szögfelbontások találhatóak. - Ez a LIDAR csatornaszámával egyenlő. - Fontos, hogy 0 értékekkel töltsük fel.*/ - float angle[channels] = {0}; - - /*A szögfelbontásokat tároló 1D tömböt, segít feltölteni.*/ - int index = 0; - - /*Új körvonalhoz tartozik az adott szög?*/ - int newCircle; - - /*A 2D tömb feltöltése.*/ - for (i = 0; i < piece; i++) - { - /*--- Az első 4 oszlop feltöltése. ---*/ - *(array2D + i * cols2d + 0) = cloud_filtered_Box.points[i].x; - *(array2D + i * cols2d + 1) = cloud_filtered_Box.points[i].y; - *(array2D + i * cols2d + 2) = cloud_filtered_Box.points[i].z; - *(array2D + i * cols2d + 3) = sqrt(pow(*(array2D + i * cols2d + 0), 2) + pow(*(array2D + i * cols2d + 1), 2) + pow(*(array2D + i * cols2d + 2), 2)); - *(array2D + i * cols2d + 6) = cloud_filtered_Box.points[i].intensity; - - /*--- Az 5. oszlop feltöltése. ---*/ - bracket = abs(*(array2D + i * cols2d + 2)) / *(array2D + i * cols2d + 3); - - /*A kerekítési hibák miatt szükséges sorok.*/ - if (bracket < -1) - bracket = -1; - else if (bracket > 1) - bracket = 1; - - /*Számolás és konvertálás fokba.*/ - if (*(array2D + i * cols2d + 2) < 0) - { - *(array2D + i * cols2d + 4) = acos(bracket) * 180 / M_PI; - } - else if (*(array2D + i * cols2d + 2) >= 0) - { - *(array2D + i * cols2d + 4) = (asin(bracket) * 180 / M_PI) + 90; - } - - /*A megfelelő index beállítása.*/ - /*Azt vesszük alapul, hogy az adott szög, egy új körvonahoz tartozik (azaz newCircle = 1).*/ - newCircle = 1; - - /*Ha már korábban volt ilyen érték (a meghatározott intervallumon belül), akkor ez nem egy új körív. - Azaz "newCircle = 0", a folyamatból kiléphetünk, nincs szükség további vizsgálatra.*/ - for (j = 0; j < channels; j++) - { - if (angle[j] == 0) - break; - - if (abs(angle[j] - *(array2D + i * cols2d + 4)) <= interval) - { - newCircle = 0; - break; - } - } - - /*Ha ilyen érték, nem szerepel még a tömbben, akkor ez egy új körív.*/ - if (newCircle == 1) - { - /*Feltétel, hogy a program ne lépjen ki szegmentálási hibával. - Ha valamilyen okból, több körív keletkezne mint 64, hiba lépne fel.*/ - if (index < channels) - { - angle[index] = *(array2D + i * cols2d + 4); - index++; - } - } - } - - /*--- A 6. oszlop feltöltése. ---*/ - /*Csaplár László kódja által, magaspontoknak jelölt pontok felvétele a 2D tömbbe.*/ - if (star_shaped_method ) - { - for (i = 0; i < padkaIDs.size(); i++) - { - *(array2D + padkaIDs[i] * cols2d + 5) = 2; - } - } - - /*A szögfelbontások növekvő sorrendbe rendezése. - A legkisebb lesz az első körív és így tovább.*/ - std::sort(angle, angle + index); - - /* - 3D tömb: - - A pontok értékei: (0: X, 1: Y, 2: Z), - - A pontok origótól vett távolsága, z = 0 értékkel: (3: D), - - A pontok helyzete egy körben (360°). (4: Alpha), - - X = 0 érték mellett, az új Y koordináták (5: új Y), - - Csoportszámok (6: Road = 1, High = 2). - */ - float *array3D = new float[channels * piece * cols](); - - /*Az adott köríveket tartalmazó csoportok (azaz a "channels"), - megfelelő sorindexeinek beállításához szükséges. - Fontos, hogy 0 értékekkel töltsük fel.*/ - int indexArray[channels] = {0}; - - /*Egy 1D tömb. Az adott köríven, az origótól, a legnagyobb távolsággal rendelkező pontok értékei.*/ - float maxDistance[channels] = {0}; - - /*Hibás LIDAR csatornaszám esetén szükséges.*/ - int results; - - /*A 3D tömb feltöltése.*/ - for (i = 0; i < piece; i++) - { - results = 0; - - /*A megfelelő körív kiválasztása.*/ - for (j = 0; j < index; j++) - { - if (abs(angle[j] - *(array2D + i * cols2d + 4)) <= interval) - { - results = 1; - break; - } - } - - if (results == 1) - { - /*Az értékek hozzáadás, a 2D tömbből.*/ - *(array3D + j * piece * cols + indexArray[j] * cols + 0) = *(array2D + i * cols2d + 0); /*X koordináta.*/ - *(array3D + j * piece * cols + indexArray[j] * cols + 1) = *(array2D + i * cols2d + 1); /*Y koordináta.*/ - *(array3D + j * piece * cols + indexArray[j] * cols + 2) = *(array2D + i * cols2d + 2); /*Z koordináta.*/ - *(array3D + j * piece * cols + indexArray[j] * cols + 7) = *(array2D + i * cols2d + 6); /* Intenzitás. */ - - /*A már ismert magaspontok.*/ - if (star_shaped_method ) - *(array3D + j * piece * cols + indexArray[j] * cols + 6) = *(array2D + i * cols2d + 5); - - /*Itt annyi különbség lesz, hogy a "z" érték nélkül adjuk hozzá a távolságot.*/ - *(array3D + j * piece * cols + indexArray[j] * cols + 3) = sqrt(pow(*(array2D + i * cols2d + 0), 2) + pow(*(array2D + i * cols2d + 1), 2)); - - /*Az 5. oszlop feltöltése, a szögekkel. 360 fokban, minden pontnak van egy szöge.*/ - bracket = (abs(*(array3D + j * piece * cols + indexArray[j] * cols + 0))) / (*(array3D + j * piece * cols + indexArray[j] * cols + 3)); - if (bracket < -1) - bracket = -1; - else if (bracket > 1) - bracket = 1; - - if (*(array3D + j * piece * cols + indexArray[j] * cols + 0) >= 0 && *(array3D + j * piece * cols + indexArray[j] * cols + 1) <= 0) - { - *(array3D + j * piece * cols + indexArray[j] * cols + 4) = asin(bracket) * 180 / M_PI; - } - else if (*(array3D + j * piece * cols + indexArray[j] * cols + 0) >= 0 && *(array3D + j * piece * cols + indexArray[j] * cols + 1) > 0) - { - *(array3D + j * piece * cols + indexArray[j] * cols + 4) = 180 - (asin(bracket) * 180 / M_PI); - } - else if (*(array3D + j * piece * cols + indexArray[j] * cols + 0) < 0 && *(array3D + j * piece * cols + indexArray[j] * cols + 1) >= 0) - { - *(array3D + j * piece * cols + indexArray[j] * cols + 4) = 180 + (asin(bracket) * 180 / M_PI); - } - else - { - *(array3D + j * piece * cols + indexArray[j] * cols + 4) = 360 - (asin(bracket) * 180 / M_PI); - } - - if (*(array3D + j * piece * cols + indexArray[j] * cols + 3) > maxDistance[j]) - { - maxDistance[j] = *(array3D + j * piece * cols + indexArray[j] * cols + 3); - } - - indexArray[j]++; - } - } - - /*A lefoglalt terület felszabadítása.*/ - delete[] array2D; - - /*-- 1. lépés: A NEM út pontok szűrése. --*/ - int p2, p3; /*p2, p3 - A három vizsgált pontból, a második és a harmadik.*/ - - /* - --> alpha - A három pont és a két vektor által bezárt szög. - --> x1, x2, x3 - A három pont által bezárt háromszög oldalainak hossza. - --> curbPoints - A pontok becsült száma, a szegélyen. - --> va1, va2, vb1, vb2 - A két vektor. - --> max1, max2 - Nem csak a szöget, hanem a magasságot is vizsgálni kell. - --> d - A két szélső pont közötti távolság. A LIDAR forgása és a körív szakadások miatt. - d - Ez a változó a program későbbi részein is felhasználásra kerül. - */ - float alpha, x1, x2, x3, va1, va2, vb1, vb2, max1, max2, d; - - /*Az összes kör vizsgálata.*/ - for (i = 0; i < index; i++) - { - if (x_zero_method) - { - /*Új Y értékek beállítása, X = 0 értékek mellett.*/ - for (j = 1; j < indexArray[i]; j++) - { - *(array3D + i * piece * cols + j * cols + 5) = *(array3D + i * piece * cols + (j - 1) * cols + 5) + 0.0100; - } - - /*A kör pontjainak vizsgálata. X = 0 módszer.*/ - for (j = curbPoints; j <= (indexArray[i] - 1) - curbPoints; j++) - { - p2 = j + curbPoints / 2; - p3 = j + curbPoints; - - d = sqrt( - pow(*(array3D + i * piece * cols + p3 * cols + 0) - *(array3D + i * piece * cols + j * cols + 0), 2) + - pow(*(array3D + i * piece * cols + p3 * cols + 1) - *(array3D + i * piece * cols + j * cols + 1), 2)); - - /*A távolság, 5 méternél kisebb legyen.*/ - if (d < 5.0000) - { - x1 = sqrt( - pow(*(array3D + i * piece * cols + p2 * cols + 5) - *(array3D + i * piece * cols + j * cols + 5), 2) + - pow(*(array3D + i * piece * cols + p2 * cols + 2) - *(array3D + i * piece * cols + j * cols + 2), 2)); - x2 = sqrt( - pow(*(array3D + i * piece * cols + p3 * cols + 5) - *(array3D + i * piece * cols + p2 * cols + 5), 2) + - pow(*(array3D + i * piece * cols + p3 * cols + 2) - *(array3D + i * piece * cols + p2 * cols + 2), 2)); - x3 = sqrt( - pow(*(array3D + i * piece * cols + p3 * cols + 5) - *(array3D + i * piece * cols + j * cols + 5), 2) + - pow(*(array3D + i * piece * cols + p3 * cols + 2) - *(array3D + i * piece * cols + j * cols + 2), 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; - - /*Feltétel és csoporthoz adás.*/ - if (alpha <= angleFilter1 && - (abs(*(array3D + i * piece * cols + j * cols + 2) - *(array3D + i * piece * cols + p2 * cols + 2)) >= curbHeight || - abs(*(array3D + i * piece * cols + p3 * cols + 2) - *(array3D + i * piece * cols + p2 * cols + 2)) >= curbHeight) && - abs(*(array3D + i * piece * cols + j * cols + 2) - *(array3D + i * piece * cols + p3 * cols + 2)) >= 0.05) - { - *(array3D + i * piece * cols + p2 * cols + 6) = 2; - } - } - } - } - - if (y_zero_method) - { - /*A kör pontjainak vizsgálata. Z = 0 módszer.*/ - for (j = curbPoints; j <= (indexArray[i] - 1) - curbPoints; j++) - { - d = sqrt( - pow(*(array3D + i * piece * cols + (j + curbPoints) * cols + 0) - *(array3D + i * piece * cols + (j - curbPoints) * cols + 0), 2) + - pow(*(array3D + i * piece * cols + (j + curbPoints) * cols + 1) - *(array3D + i * piece * cols + (j - curbPoints) * cols + 1), 2)); - - /*A távolság, 5 méternél kisebb legyen.*/ - if (d < 5.0000) - { - /*Kezdeti értékek beállítása.*/ - max1 = abs(*(array3D + i * piece * cols + j * cols + 2)), max2 = abs(*(array3D + i * piece * cols + j * cols + 2)); - va1 = 0, va2 = 0, vb1 = 0, vb2 = 0; - - /*Az 'a' vektor és a legnagyobb magasság beállítása.*/ - for (k = j - 1; k >= j - curbPoints; k--) - { - va1 = va1 + (*(array3D + i * piece * cols + k * cols + 0) - *(array3D + i * piece * cols + j * cols + 0)); - va2 = va2 + (*(array3D + i * piece * cols + k * cols + 1) - *(array3D + i * piece * cols + j * cols + 1)); - if (abs(*(array3D + i * piece * cols + k * cols + 2)) > max1) - max1 = abs(*(array3D + i * piece * cols + k * cols + 2)); - } - - /*A 'b' vektor és a legnagyobb magasság beállítása.*/ - for (k = j + 1; k <= j + curbPoints; k++) - { - vb1 = vb1 + (*(array3D + i * piece * cols + k * cols + 0) - *(array3D + i * piece * cols + j * cols + 0)); - vb2 = vb2 + (*(array3D + i * piece * cols + k * cols + 1) - *(array3D + i * piece * cols + j * cols + 1)); - if (abs(*(array3D + i * piece * cols + k * cols + 2)) > max2) - max2 = abs(*(array3D + i * piece * cols + k * cols + 2)); - } - - va1 = (1 / (float)curbPoints) * va1; - va2 = (1 / (float)curbPoints) * va2; - vb1 = (1 / (float)curbPoints) * vb1; - vb2 = (1 / (float)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; - - /*Feltétel és csoporthoz adás.*/ - if (alpha <= angleFilter2 && - (max1 - abs(*(array3D + i * piece * cols + j * cols + 2)) >= curbHeight || - max2 - abs(*(array3D + i * piece * cols + j * cols + 2)) >= curbHeight) && - abs(max1 - max2) >= 0.05) - { - *(array3D + i * piece * cols + j * cols + 6) = 2; - } - } - } - } - } - - /*-- 2. lépés: Az út pontok szűrése. --*/ - /*A tömb elemeinek rendezése oly módon, hogy körönként, a szögeknek megfelelő sorrendben legyenek.*/ - for (i = 0; i < index; i++) - { - quickSort(array3D, i, piece, 0, indexArray[i] - 1); - } - - /*Vakfolt keresés: - Megvizsgáljuk a második körvonalat. (Az elsővel pontatlan.) - Főleg a [90°-180° --- 180°-270°] tartomány és a [0°-90° --- 270°-360°] tartomány a lényeg, az autó két oldalán. - Mind a kettő tartományban keresünk 2db magaspontot. Ha az adott tartományban az első körvonalon van két magaspont, - a közte lévő terület nagy valószínűséggel egy vakfolt lesz.*/ - float q1 = 0, q2 = 180, q3 = 180, q4 = 360; /*A kör négy része. (1. 2. 3. 4. negyed.)*/ - int c1 = -1, c2 = -1, c3 = -1, c4 = -1; /*A talált pontok ID-ja az első körvonalon.*/ - - if (blind_spots) - { - for (i = 0; i < indexArray[1]; i++) - { - if (*(array3D + 1 * piece * cols + i * cols + 6) == 2) - { - if (*(array3D + 1 * piece * cols + i * cols + 4) >= 0 && *(array3D + 1 * piece * cols + i * cols + 4) < 90) - { - if (*(array3D + 1 * piece * cols + i * cols + 4) > q1) - { - q1 = *(array3D + 1 * piece * cols + i * cols + 4); - c1 = i; - } - } - else if (*(array3D + 1 * piece * cols + i * cols + 4) >= 90 && *(array3D + 1 * piece * cols + i * cols + 4) < 180) - { - if (*(array3D + 1 * piece * cols + i * cols + 4) < q2) - { - q2 = *(array3D + 1 * piece * cols + i * cols + 4); - c2 = i; - } - } - else if (*(array3D + 1 * piece * cols + i * cols + 4) >= 180 && *(array3D + 1 * piece * cols + i * cols + 4) < 270) - { - if (*(array3D + 1 * piece * cols + i * cols + 4) > q3) - { - q3 = *(array3D + 1 * piece * cols + i * cols + 4); - c3 = i; - } - } - else - { - if (*(array3D + 1 * piece * cols + i * cols + 4) < q4) - { - q4 = *(array3D + 1 * piece * cols + i * cols + 4); - c4 = i; - } - } - } - } - } - - float arcDistance; /*Körív mérete, a megadott foknál. Fontos minden körön, ugyanakkora körív méretet vizsgálni.*/ - int notRoad; /*Ha az adott köríven, az adott szakaszon, található magaspont, akkor 1-es értéket vesz fel, amúgy 0-át.*/ - int blindSpot; /*Vakfoltok az autó mellett.*/ - float currentDegree; /*Az aktuális köríven, a szög nagysága.*/ - - /*A körív méret meghatározása.*/ - arcDistance = ((maxDistance[0] * M_PI) / 180) * beamZone; - - /*0°-tól 360° - beamZone-ig.*/ - for (i = 0; i <= 360 - beamZone; i++) - { - blindSpot = 0; - - if (blind_spots) - { - /*Ha ezek a feltételek teljesülnek, akkor egy vakfoltba léptünk és itt nem vizsgálódunk.*/ - if (xDirection == 0) - { - /*+-X irányba is vizsgáljuk a pontokat.*/ - if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3)) - { - blindSpot = 1; - } - } - else if (xDirection == 1) - { - /*+X irányba vizsgáljuk a pontokat.*/ - if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270))) - { - blindSpot = 1; - } - } - else - { - /*-X vizsgáljuk a pontokat.*/ - if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90)) - { - blindSpot = 1; - } - } - } - - if (blindSpot == 0) - { - /*Alap beállítás, hogy az adott szakaszon nincs magaspont.*/ - notRoad = 0; - - /*Az első kör adott szakaszának vizsgálata.*/ - for (j = 0; *(array3D + 0 * piece * cols + j * cols + 4) <= i + beamZone && j < indexArray[0]; j++) - { - if (*(array3D + 0 * piece * cols + j * cols + 4) >= i) - { - /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/ - if (*(array3D + 0 * piece * cols + j * cols + 6) == 2) - { - notRoad = 1; - break; - } - } - } - - /*Ha nem találtunk az első kör, adott szakaszán magaspontot, továbbléphetünk a következő körre.*/ - if (notRoad == 0) - { - /*Az első kör szakaszát elfogadjuk.*/ - for (j = 0; *(array3D + 0 * piece * cols + j * cols + 4) <= i + beamZone && j < indexArray[0]; j++) - { - if (*(array3D + 0 * piece * cols + j * cols + 4) >= i) - { - *(array3D + 0 * piece * cols + j * cols + 6) = 1; - } - } - - /*A további körök vizsgálata.*/ - for (k = 1; k < index; k++) - { - /*Új szöget kell meghatározni, hogy a távolabbi körvonalakon is, ugyanakkora körív hosszt vizsgáljunk.*/ - if (i == 360 - beamZone) - { - currentDegree = 360; - } - else - { - currentDegree = i + arcDistance / ((maxDistance[k] * M_PI) / 180); - } - - /*Az új kör pontjait vizsgáljuk.*/ - for (l = 0; *(array3D + k * piece * cols + l * cols + 4) <= currentDegree && l < indexArray[k]; l++) - { - if (*(array3D + k * piece * cols + l * cols + 4) >= i) - { - /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/ - if (*(array3D + k * piece * cols + l * cols + 6) == 2) - { - notRoad = 1; - break; - } - } - } - - /*A többi kört nem vizsgáljuk, ha a sugár, elakadt egy magasponton.*/ - if (notRoad == 1) - break; - - /*Egyébként, elfogadjuk az adott kör, adott szakaszát.*/ - for (l = 0; *(array3D + k * piece * cols + l * cols + 4) <= currentDegree && l < indexArray[k]; l++) - { - if (*(array3D + k * piece * cols + l * cols + 4) >= i) - { - *(array3D + k * piece * cols + l * cols + 6) = 1; - } - } - } - } - } - } - - /*Ugyanaz, mint az előző, csak itt 360°-tól 0° + beamZone-ig vizsgáljuk a pontokat.*/ - for (i = 360; i >= 0 + beamZone; --i) - { - blindSpot = 0; - - if (blind_spots) - { - /*Ha ezek a feltételek teljesülnek, akkor egy vakfoltba léptünk és itt nem vizsgálódunk.*/ - if (xDirection == 0) - { - /*+-X irányba is vizsgáljuk a pontokat.*/ - if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3)) - { - blindSpot = 1; - } - } - else if (xDirection == 1) - { - /*+X irányba vizsgáljuk a pontokat.*/ - if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270))) - { - blindSpot = 1; - } - } - else - { - /*-X vizsgáljuk a pontokat.*/ - if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90)) - { - blindSpot = 1; - } - } - } - - if (blindSpot == 0) - { - /*Alap beállítás, hogy az adott szakaszon nincs magaspont.*/ - notRoad = 0; - - /*Az első kör adott szakaszának vizsgálata.*/ - for (j = indexArray[0] - 1; *(array3D + 0 * piece * cols + j * cols + 4) >= i - beamZone && j >= 0; --j) - { - if (*(array3D + 0 * piece * cols + j * cols + 4) <= i) - { - /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/ - if (*(array3D + 0 * piece * cols + j * cols + 6) == 2) - { - notRoad = 1; - break; - } - } - } - - /*Ha nem találtunk az első kör, adott szakaszán magaspontot, továbbléphetünk a következő körre.*/ - if (notRoad == 0) - { - /*Az első kör szakaszát elfogadjuk.*/ - for (j = indexArray[0] - 1; *(array3D + 0 * piece * cols + j * cols + 4) >= i - beamZone && j >= 0; --j) - { - if (*(array3D + 0 * piece * cols + j * cols + 4) <= i) - { - *(array3D + 0 * piece * cols + j * cols + 6) = 1; - } - } - - /*A további körök vizsgálata.*/ - for (k = 1; k < index; k++) - { - /*Új szöget kell meghatározni, hogy a távolabbi körvonalakon is, ugyanakkora körív hosszt vizsgáljunk.*/ - if (i == 0 + beamZone) - { - currentDegree = 0; - } - else - { - currentDegree = i - arcDistance / ((maxDistance[k] * M_PI) / 180); - } - - /*Az új kör pontjait vizsgáljuk.*/ - for (l = indexArray[k] - 1; *(array3D + k * piece * cols + l * cols + 4) >= currentDegree && l >= 0; --l) - { - if (*(array3D + k * piece * cols + l * cols + 4) <= i) - { - /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/ - if (*(array3D + k * piece * cols + l * cols + 6) == 2) - { - notRoad = 1; - break; - } - } - } - - /*A többi kört nem vizsgáljuk, ha a sugár, elakadt egy magasponton.*/ - if (notRoad == 1) - break; - - /*Egyébként, elfogadjuk az adott kör, adott szakaszát.*/ - for (l = indexArray[k] - 1; *(array3D + k * piece * cols + l * cols + 4) >= currentDegree && l >= 0; --l) - { - if (*(array3D + k * piece * cols + l * cols + 4) <= i) - { - *(array3D + k * piece * cols + l * cols + 6) = 1; - } - } - } - } - } - } - - /*-- 3. lépés: A marker pontjainak keresése. Adott fokban a legtávolabbi zöld pont. --*/ - /*A marker pontjait tartalmazza. Az első három oszlopban az X - Y - Z koordinátát, - a negyedikben pedig 0 - 1 érték szerepel attól függően, hogy az adott fokban található-e olyan pont, ami nincs útnak jelölve.*/ - float markerPointsArray[piece][4]; - float maxDistanceRoad; /*Adott fokban, a legtávolabbi zöld pont távolsága.*/ - int cM = 0; /*Segédváltozó, a marker pontok feltöltéséhez (c - counter, M - Marker).*/ - int ID1, ID2; /*Az adott pont melyik körvonalon van (ID1) és hányadik pont (ID2).*/ - int redPoints; /*A vizsgált sávban van-e magaspont, vagy olyan pont, amit nem jelölt a program útnak, se magaspontnak.*/ - - /*360 fokban megvizsgáljuk a pontokat, 1 fokonként.*/ - for (i = 0; i <= 360; i++) - { - ID1 = -1; - ID2 = -1; - maxDistanceRoad = 0; - redPoints = 0; - - /*Itt végigmegyünk az összes körvonal, összes pontján.*/ - for (j = 0; j < index; j++) - { - for (k = 0; k < indexArray[j]; k++) - { - /*Ha találunk az adott fokban nem út pontot, akkor kilépünk, mert utána úgyse lesz már út pont és a "redPoints" változó 1-es érétket kap.*/ - if (*(array3D + j * piece * cols + k * cols + 6) != 1 && *(array3D + j * piece * cols + k * cols + 4) >= i && *(array3D + j * piece * cols + k * cols + 4) < i + 1) - { - redPoints = 1; - break; - } - - /*A talált zöld pont távolságának vizsgálata.*/ - if (*(array3D + j * piece * cols + k * cols + 6) == 1 && *(array3D + j * piece * cols + k * cols + 4) >= i && *(array3D + j * piece * cols + k * cols + 4) < i + 1) - { - d = sqrt(pow(0 - *(array3D + j * piece * cols + k * cols + 0), 2) + pow(0 - *(array3D + j * piece * cols + k * cols + 1), 2)); - - if (d > maxDistanceRoad) - { - maxDistanceRoad = d; - ID1 = j; - ID2 = k; - } - } - } - /*A korábbi "break", kilépett az adott körvonalból, ez a továbbiakból is kilép, és jön a következő fok vizsgálata.*/ - if (redPoints == 1) - break; - } - - /*A marker pontok hozzáadása a tömbhöz.*/ - if (ID1 != -1 && ID2 != -1) - { - markerPointsArray[cM][0] = *(array3D + ID1 * piece * cols + ID2 * cols + 0); - markerPointsArray[cM][1] = *(array3D + ID1 * piece * cols + ID2 * cols + 1); - markerPointsArray[cM][2] = *(array3D + ID1 * piece * cols + ID2 * cols + 2); - markerPointsArray[cM][3] = redPoints; - cM++; - } - } - - /*-- 4. lépés: A csoportok feltöltése. --*/ - for (i = 0; i < index; i++) - { - for (j = 0; j < indexArray[i]; j++) - { - /*Az út pontok.*/ - if (*(array3D + i * piece * cols + j * cols + 6) == 1) - { - pt.x = *(array3D + i * piece * cols + j * cols + 0); - pt.y = *(array3D + i * piece * cols + j * cols + 1); - pt.z = *(array3D + i * piece * cols + j * cols + 2); - pt.intensity = *(array3D + i * piece * cols + j * cols + 7); - cloud_filtered_Road.push_back(pt); - } - - /*A magas pontok.*/ - else if (*(array3D + i * piece * cols + j * cols + 6) == 2) - { - pt.x = *(array3D + i * piece * cols + j * cols + 0); - pt.y = *(array3D + i * piece * cols + j * cols + 1); - pt.z = *(array3D + i * piece * cols + j * cols + 2); - pt.intensity = *(array3D + i * piece * cols + j * cols + 7); - cloud_filtered_High.push_back(pt); - } - } - } - - /*-- 5. lépés: A marker beállítása. --*/ - /*Legyen minimum 3 pont, amit össze lehet kötni, különben hibák lépnének fel.*/ - if (cM > 2) - { - /*Lehet olyan eset, hogy piros - zöld - prios (vagy fordítva) pont van egymás mellett. - Ez azért rossz, mert a zöld / piros marker (line strip) ebben az esetben csak 1 pont lesz. - Ez nem javasolt, ezért minden pontnak lennie kell egy azonos színű párjának. - A "markerPointsArray" tömb 3. oszlopában ha 1-es szerepel, akkor az a piros line strip-hez tartozik, - ellenkező esetben a zöldhöz.*/ - - /*Ha az első pont zöld, de a második piros, akkor az első is a piros line strip-be kerül.*/ - if (markerPointsArray[0][3] == 0 && markerPointsArray[1][3] == 1) - markerPointsArray[0][3] = 1; - - /*Ha az utolsó pont zöld, de az utolsó előtti piros, akkor az utolsó is a piros line strip-be kerül.*/ - if (markerPointsArray[cM - 1][3] == 0 && markerPointsArray[cM - 2][3] == 1) - markerPointsArray[cM - 1][3] = 1; - - /*Ha az első pont piros, de a második zöld, akkor az első is a zöld line strip-be kerül.*/ - if (markerPointsArray[0][3] == 1 && markerPointsArray[1][3] == 0) - markerPointsArray[0][3] = 0; - - /*Ha az utolsó pont piros, de az utolsó előtti zöld, akkor az utolsó is a zöld line strip-be kerül.*/ - if (markerPointsArray[cM - 1][3] == 1 && markerPointsArray[cM - 2][3] == 0) - markerPointsArray[cM - 1][3] = 0; - - /*Itt végig megyünk a pontokon, ha egy zöld pontot két piros fog közre, akkor az is a piros line strip-be kerül. - Az első kettő és az utolsó kettő pontot nem vizsgáljuk, ezek már be vannak állítva.*/ - 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; - } - - /*Itt végig megyünk az összes ponton, ha egy piros pontot két zöld fog közre, akkor az is a zöld line strip-be kerül. - Az első kettő és az utolsó kettő pontot nem vizsgáljuk, ezek már be vannak állítva.*/ - 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::MarkerArray ma; /*Egy marker array, amiben a zöld / piros line strip-ek kerülnek.*/ - visualization_msgs::Marker line_strip; /*Az adott zöld vagy piros szakasz / line strip.*/ - geometry_msgs::Point point; /*Az adott pont értékei. Ez tölti fel az adott line stip-et.*/ -float zavg = 0.0; /*Átlagos z-érték (egyszerűsített polygonhoz)*/ - - int lineStripID = 0; /*Az adott line strip ID-ja.*/ - - line_strip.header.frame_id = fixedFrame; - line_strip.header.stamp = ros::Time(); - line_strip.type = visualization_msgs::Marker::LINE_STRIP; - line_strip.action = visualization_msgs::Marker::ADD; - - /*Végigmegyünk a pontokon, amik a markert fogják alkotni.*/ - for (i = 0; i < cM; i++) - { - /*Az adott pont hozzáadása egy "geometry_msgs::Point" típusú változóhoz.*/ - point.x = markerPointsArray[i][0]; - point.y = markerPointsArray[i][1]; - point.z = markerPointsArray[i][2]; - zavg *= i; - zavg += point.z; - zavg /= i+1; - - /*Az első pont hozzáadása az adott line strip-hez. - Az első pontnál semmilyen feltétel nem kell.*/ - if (i == 0) - { - line_strip.points.push_back(point); - line += xy(point.x,point.y); - } - - /*Ha a következő pont is ugyanabba a csoportba (piros vagy zöld) tartozik, mint az előző, - akkor ezt is hozzáadjuk az adott line strip-hez.*/ - else if (markerPointsArray[i][3] == markerPointsArray[i - 1][3]) - { - line_strip.points.push_back(point); - line += xy(point.x,point.y); - - /*Ebben az "else if" feltételben fogjuk elérni az utolsó pontot, itt elkészül az utolsó line strip.*/ - if (i == cM - 1) - { - line_strip.id = lineStripID; - - line_strip.pose.position.x = 0; - line_strip.pose.position.y = 0; - line_strip.pose.position.z = 0; - - line_strip.pose.orientation.x = 0.0; - line_strip.pose.orientation.y = 0.0; - line_strip.pose.orientation.z = 0.0; - line_strip.pose.orientation.w = 1.0; - - line_strip.scale.x = 0.5; - line_strip.scale.y = 0.5; - line_strip.scale.z = 0.5; - - /*A line strip színének a beállítása.*/ - if (markerPointsArray[i][3] == 0) - { - line_strip.color.a = 1.0; - line_strip.color.r = 0.0; - line_strip.color.g = 1.0; - line_strip.color.b = 0.0; - } - else - { - line_strip.color.a = 1.0; - line_strip.color.r = 1.0; - line_strip.color.g = 0.0; - line_strip.color.b = 0.0; - } - - if (polysimp_allow) - { - line_strip.points.clear(); - boost::geometry::clear(simplified); - boost::geometry::simplify(line, simplified, polysimp); - for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) - { - geometry_msgs::Point p; - p.x = boost::geometry::get<0>(*it); - p.y = boost::geometry::get<1>(*it); - p.z = polyz; - - line_strip.points.push_back(p); - } - } - - ma.markers.push_back(line_strip); /*A line strip hozzáadása a marker array-hez.*/ - line_strip.points.clear(); /*Az utolsó line strip-ből is töröljük a pontokat, feleslegesen ne tárolódjon.*/ - boost::geometry::clear(line); - } - } - - /*Csoportváltozás --> pirosról - zöldre. - Ilyenkor még piros marker köti össze a két pontot, szóval hozzáadjuk a pontot az adott line strip-hez.*/ - 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); - - /*A következő pontok már új line strip-hez fognak tartozni, szóval itt elkészül az egyik piros.*/ - line_strip.id = lineStripID; - lineStripID++; - - line_strip.pose.position.x = 0; - line_strip.pose.position.y = 0; - line_strip.pose.position.z = 0; - - line_strip.pose.orientation.x = 0.0; - line_strip.pose.orientation.y = 0.0; - line_strip.pose.orientation.z = 0.0; - line_strip.pose.orientation.w = 1.0; - - line_strip.scale.x = 0.5; - line_strip.scale.y = 0.5; - line_strip.scale.z = 0.5; - - line_strip.color.a = 1.0; - line_strip.color.r = 1.0; - line_strip.color.g = 0.0; - line_strip.color.b = 0.0; - - if (polysimp_allow) - { - line_strip.points.clear(); - boost::geometry::clear(simplified); - boost::geometry::simplify(line, simplified, polysimp); - for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) - { - geometry_msgs::Point p; - p.x = boost::geometry::get<0>(*it); - p.y = boost::geometry::get<1>(*it); - p.z = polyz; - - line_strip.points.push_back(p); - } - } - - ma.markers.push_back(line_strip); /*A line strip hozzáadása a marker array-hez.*/ - line_strip.points.clear(); /*A benne lévő pontok már nem kellenek.*/ - boost::geometry::clear(line); - line_strip.points.push_back(point); /*A következő zöld line strip-nél is szükség van erre a pontra, szóval hozzáadjuk.*/ - line += xy(point.x,point.y); - } - - /*Csoportváltozás --> zöldről - pirosra. - Előszőr beállítjuk a zöld line stip-et, majd az utolsó pontot hozzáadjuk a piroshoz is, - mivel zöld és piros pont között mindig piros line srip van.*/ - else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 1) - { - /*A zöld marker.*/ - line_strip.id = lineStripID; - lineStripID++; - - line_strip.pose.position.x = 0; - line_strip.pose.position.y = 0; - line_strip.pose.position.z = 0; - - line_strip.pose.orientation.x = 0.0; - line_strip.pose.orientation.y = 0.0; - line_strip.pose.orientation.z = 0.0; - line_strip.pose.orientation.w = 1.0; - - line_strip.scale.x = 0.5; - line_strip.scale.y = 0.5; - line_strip.scale.z = 0.5; - - line_strip.color.a = 1.0; - line_strip.color.r = 0.0; - line_strip.color.g = 1.0; - line_strip.color.b = 0.0; - - if (polysimp_allow) - { - line_strip.points.clear(); - boost::geometry::clear(simplified); - boost::geometry::simplify(line, simplified, polysimp); - for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) - { - geometry_msgs::Point p; - p.x = boost::geometry::get<0>(*it); - p.y = boost::geometry::get<1>(*it); - p.z = polyz; - - line_strip.points.push_back(p); - } - } - - ma.markers.push_back(line_strip); /*A line strip hozzáadása a marker array-hez.*/ - line_strip.points.clear(); /*A benne lévő pontok már nem kellenek.*/ - boost::geometry::clear(line); - - /*A követkető piros line srip-hez szükség van az előző pontra is.*/ - 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); - - /*A követkető piros line srip-hez szükség van a jelenlegi pontra.*/ - 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 = ros::Duration(0); - } - if (zavg_allow) - { - for (int seg=0; seg < ma.markers.size(); seg++) - { - for (int mz = 0; mz < ma.markers[seg].points.size(); mz++) /*Egyszerűsített polygon z-koordinátáinak megadása átlagból. */ - { - ma.markers[seg].points[mz].z = zavg; - } - } - /* polyz = zavg; /* Be- és kikapcsolással a konstans z-érték az átlagérték alapján beállítja magát. (kell?) */ - } - - /*érvényét vesztett markerek eltávolítása*/ - line_strip.action = visualization_msgs::Marker::DELETE; - for (int del = lineStripID; del server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(¶msCallback, _1, _2); - server.setCallback(f); - - /*NodeHandle*/ - ros::NodeHandle nh; - - /*Feliratkozás az adott topicra.*/ - ros::Subscriber sub = nh.subscribe(topicName, 1, filtered); - - /*A szűrt adatok hírdetése.*/ - pub_road = nh.advertise("road", 1); - pub_high = nh.advertise("curb", 1); - pub_box = nh.advertise("roi", 1); // ROI - region of interest - pub_pobroad = nh.advertise("road_probably", 1); - pub_marker = nh.advertise("road_marker", 1); - - /*Csaplár László kódjához szükséges.*/ - beam_init(); - - ROS_INFO("Ready"); - - ros::spin(); - return 0; -} \ No newline at end of file diff --git a/src/lidar_segmentation.cpp b/src/lidar_segmentation.cpp new file mode 100644 index 0000000..da81301 --- /dev/null +++ b/src/lidar_segmentation.cpp @@ -0,0 +1,622 @@ +#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::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::ColorRGBA setcolor(float r, float g, float b, float a) +{ + std_msgs::ColorRGBA c; + c.r = r; + c.g = g; + c.b = b; + c.a = a; + return c; +} + +Detector::Detector(ros::NodeHandle* nh){ + /*subscribing to the given topic*/ + sub = nh->subscribe(params::topicName, 1, &Detector::filtered,this); + /*publishing filtered points*/ + pub_road = nh->advertise("road", 1); + pub_high = nh->advertise("curb", 1); + pub_box = nh->advertise("roi", 1); // ROI - region of interest + pub_pobroad = nh->advertise("road_probably", 1); + pub_marker = nh->advertise("road_marker", 1); + + Detector::beam_init(); + + ROS_INFO("Ready"); + +} + +/*FUNCTIONS*/ + +/*recursive, quick sorting function (1/2)*/ +int Detector::partition(std::vector>& array3D, int arc,int low, int high) +{ + float pivot = array3D[arc][high].alpha; + int i = (low - 1); + for (int j = low; j <= high - 1; j++){ + if (array3D[arc][j].alpha < pivot){ + i++; + std::swap(array3D[arc][i],array3D[arc][j]); + } + } + std::swap(array3D[arc][i+1],array3D[arc][high]); + return (i + 1); +} + +/*recursive, quick sorting function (2/2)*/ +void Detector::quickSort(std::vector>& array3D, int arc, int low, int high) +{ + if (low < high) + { + int pi = partition(array3D, arc, low, high); + quickSort(array3D, arc, low, pi - 1); + quickSort(array3D, arc, pi + 1, high); + } +} + +void Detector::filtered(const pcl::PointCloud &cloud){ + /*variables for the "for" loops*/ + int i, j, k, l; + + pcl::PointXYZI pt; //temporary variable for storing a point + auto cloud_filtered_Box = boost::make_shared>(cloud); //all points in the detection area + pcl::PointCloud cloud_filtered_Road; //filtered points (driveable road) + pcl::PointCloud cloud_filtered_ProbablyRoad; //filtered points (non-driveable road) + pcl::PointCloud cloud_filtered_High; //filtered points (non-road) + + + auto filterCondition = boost::make_shared>( + [=](const pcl::PointXYZI& point){ + return point.x >= params::min_X && point.x <= params::max_X && + point.y >= params::min_Y && point.y <= params::max_Y && + point.z >= params::min_Z && point.z <= params::max_Z && + point.x + point.y + point.z != 0; + } + ); + pcl::ConditionalRemoval condition_removal; + condition_removal.setCondition(filterCondition); + condition_removal.setInputCloud(cloud_filtered_Box); + condition_removal.filter(*cloud_filtered_Box); + + /*number of points in the detection area*/ + size_t piece = cloud_filtered_Box->points.size(); + + /*A minimum of 30 points are requested in the detection area to avoid errors. + Also, there is not much point in evaluating less data than that.*/ + if (piece < 30){ + return; + } + + std::vector array2D(piece); + + /*variable for storing the input for trigonometric functions*/ + float bracket; + + /*A 1D array containing the various angular resolutions. + This equals to the number of LiDAR channels. + It is important to fill it with 0 values.*/ + float angle[channels] = {0}; + + /*This helps to fill the 1D array containing the angular resolutions.*/ + int index = 0; + + /*whether the given angle corresponds to a new arc*/ + int newCircle; + + /*filling the 2D array*/ + for (i = 0; i < piece; i++){ + /*--- filling the first 4 columns ---*/ + array2D[i].p = cloud_filtered_Box->points[i]; + array2D[i].d = sqrt(pow(array2D[i].p.x, 2) + pow(array2D[i].p.y, 2) + pow(array2D[i].p.z, 2)); + + /*--- filling the 5. column ---*/ + bracket = abs(array2D[i].p.z) / array2D[i].d; + + /*required because of rounding errors*/ + if (bracket < -1) + bracket = -1; + else if (bracket > 1) + bracket = 1; + + /*calculation and conversion to degrees*/ + if (array2D[i].p.z < 0) + { + array2D[i].alpha = acos(bracket) * 180 / M_PI; + } + else{ + array2D[i].alpha = (asin(bracket) * 180 / M_PI) + 90; + } + + /*setting the index*/ + /*Our basic assumption is that the angle corresponds to a new circle/arc.*/ + newCircle = 1; + + /*If this value has already occured (within the specified interval), then this is not a new arc. + Which means that "newCircle = 0", we can exit the loop, no further processing required.*/ + for (j = 0; j < channels; j++) + { + if (angle[j] == 0) + break; + + if (abs(angle[j] - array2D[i].alpha) <= params::interval) + { + newCircle = 0; + break; + } + } + + /*If no such value is registered in the array, then it's a new circle/arc.*/ + if (newCircle == 1) + { + /*We cannot allow the program to halt with a segmentation fault error. + If for any reason there would come to be more than 64 arcs/circles, an error would occur.*/ + if (index < channels) + { + angle[index] = array2D[i].alpha; + index++; + } + } + } + /*calling starShapedSearch algorithm*/ + if (params::star_shaped_method ) + Detector::starShapedSearch(array2D); + + + /*Sorting the angular resolutions by ascending order... + The smallest will be the first arc, etc..*/ + std::sort(angle, angle + index); + + std::vector> array3D(channels,std::vector(piece)); + + /*This is required to set up the row indices of + the groups ("channels") containing the arcs. + It is important to fill it with 0 values.*/ + int indexArray[channels] = {0}; + + /*A 1D array. The values of points that have the greatest distance from the origo.*/ + float maxDistance[channels] = {0}; + + /*variable helping to handle errors caused by wrong number of channels.*/ + int results; + + /*filling the 3D array*/ + for (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::MarkerArray ma; //a marker array containing the green / red line strips + visualization_msgs::Marker line_strip; //the current green or red section / line strip + geometry_msgs::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 = ros::Time(); + line_strip.type = visualization_msgs::Marker::LINE_STRIP; + line_strip.action = visualization_msgs::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::Point" type variable*/ + point.x = markerPointsArray[i][0]; + point.y = markerPointsArray[i][1]; + point.z = markerPointsArray[i][2]; + zavg *= i; + zavg += point.z; + zavg /= i+1; + + /*Adding the first point to the current line strip. + No conditions need to be met for the first point.*/ + if (i == 0) + { + line_strip.points.push_back(point); + line += xy(point.x,point.y); + } + + /*If the next point is from the same group (red or green) as the previous one + then it will be added to the line strip aswell.*/ + else if (markerPointsArray[i][3] == markerPointsArray[i - 1][3]) + { + line_strip.points.push_back(point); + line += xy(point.x,point.y); + + /*In this "else if" section we will reach the last point and the last line strip will be created.*/ + if (i == cM - 1) + { + line_strip.id = lineStripID; + marker_init(line_strip); + + /*setting the color of the line strip*/ + if (markerPointsArray[i][3] == 0) + { + line_strip.color = setcolor(0.0, 1.0, 0.0, 1.0); //green + } + else + { + line_strip.color = setcolor(1.0, 0.0, 0.0, 1.0); //red + } + + if (params::polysimp_allow) + { + line_strip.points.clear(); + boost::geometry::clear(simplified); + boost::geometry::simplify(line, simplified, params::polysimp); + for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) + { + geometry_msgs::Point p; + p.x = boost::geometry::get<0>(*it); + p.y = boost::geometry::get<1>(*it); + p.z = params::polyz; + + line_strip.points.push_back(p); + } + } + + ma.markers.push_back(line_strip); //adding the line strip to the marker array + line_strip.points.clear(); //We clear the points from the last line strip as there's no need for them anymore. + boost::geometry::clear(line); + } + } + + /*change of category: red -> green + The line joining the two points is still red, so we add the point to the given line strip.*/ + else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 0) + { + line_strip.points.push_back(point); + line += xy(point.x,point.y); + + /*The following points belong to a new line strip - a red one is being made here.*/ + line_strip.id = lineStripID; + lineStripID++; + + marker_init(line_strip); + + line_strip.color = setcolor(1.0, 0.0, 0.0, 1.0); //red + + if (params::polysimp_allow) + { + line_strip.points.clear(); + boost::geometry::clear(simplified); + boost::geometry::simplify(line, simplified, params::polysimp); + for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) + { + geometry_msgs::Point p; + p.x = boost::geometry::get<0>(*it); + p.y = boost::geometry::get<1>(*it); + p.z = params::polyz; + + line_strip.points.push_back(p); + } + } + + ma.markers.push_back(line_strip); //adding the line strip to the marker array + line_strip.points.clear(); //the points are not needed anymore + boost::geometry::clear(line); + line_strip.points.push_back(point); //This point is needed for the next line strip aswell, so we add it. + line += xy(point.x,point.y); + } + + /*change of category: green -> red + First we set up the green line strip, then we add the last point to the red one aswell, + since there is always a red line strip between a green and a red point.*/ + else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 1) + { + /*the green marker*/ + line_strip.id = lineStripID; + lineStripID++; + + marker_init(line_strip); + + line_strip.color = setcolor(0.0, 1.0, 0.0, 1.0); //green + + if (params::polysimp_allow) + { + line_strip.points.clear(); + boost::geometry::clear(simplified); + boost::geometry::simplify(line, simplified, params::polysimp); + for(boost::geometry::model::linestring::const_iterator it = simplified.begin(); it != simplified.end(); it++) + { + geometry_msgs::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 = ros::Duration(0); + } + if (params::zavg_allow) + { + for (int seg=0; seg < ma.markers.size(); seg++) + { + for (int mz = 0; mz < 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::Marker::DELETE; + for (int del = lineStripID; delheader = cloud.header; + + /*publishing*/ + pub_road.publish(cloud_filtered_Road); //filtered points (driveable road) + pub_high.publish(cloud_filtered_High); //filtered points (non-driveable road) + pub_box.publish(cloud_filtered_Box); //filtered points (non-road) + pub_pobroad.publish(cloud_filtered_ProbablyRoad); +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..626d215 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,55 @@ +#include "urban_road_filter/data_structures.hpp" + +/*parameter setup*/ +void paramsCallback(urban_road_filter::LidarFiltersConfig &config, uint32_t level){ + params::fixedFrame = config.fixed_frame; + params::topicName = config.topic_name; + params::x_zero_method = config.x_zero_method; + params::z_zero_method = config.z_zero_method; + params::star_shaped_method = config.star_shaped_method ; + params::blind_spots = config.blind_spots; + params::xDirection = config.xDirection; + params::interval = config.interval; + params::curbHeight = config.curb_height; + params::curbPoints = config.curb_points; + params::beamZone = config.beamZone; + params::angleFilter1 = config.cylinder_deg_x; + params::angleFilter2 = config.cylinder_deg_z; + params::angleFilter3 = config.sector_deg; + params::min_X = config.min_x; + params::max_X = config.max_x; + params::min_Y = config.min_y; + params::max_Y = config.max_y; + params::min_Z = config.min_z; + params::max_Z = config.max_z; + params::dmin_param = config.dmin_param; + params::kdev_param = config.kdev_param; + params::kdist_param = config.kdist_param; + params::polysimp_allow = config.simple_poly_allow; + params::polysimp = config.poly_s_param; + params::zavg_allow = config.poly_z_avg_allow; + params::polyz = config.poly_z_manual; + ROS_INFO("Updated params %s", ros::this_node::getName().c_str()); +} + +/*MAIN*/ +int main(int argc, char **argv) +{ + + /*initializing ROS*/ + ros::init(argc, argv, "urban_road_filt"); + ROS_INFO("Initializing %s", ros::this_node::getName().c_str()); + + /*code needed for GUI*/ + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(¶msCallback, _1, _2); + server.setCallback(f); + + /*NodeHandle*/ + ros::NodeHandle nh; + Detector detector(&nh); + + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/src/starShapedSearch.cpp b/src/star_shaped_search.cpp similarity index 64% rename from src/starShapedSearch.cpp rename to src/star_shaped_search.cpp index 522d885..7689c5b 100644 --- a/src/starShapedSearch.cpp +++ b/src/star_shaped_search.cpp @@ -2,35 +2,17 @@ (by László Csaplár) description: a complementary algorithm for roadside detection, part of the "urban_road_filter" package - input: const pcl::PointCloud [see: void callback(...)] - output: (non-return) std::vector padkaIDs - list of IDs of the points (one per beam) detected as roadside */ +#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 rmin = 2.0; //minimum radius of filter (keep points only farther than this) -float rmax = 60.0; //maximum radius of filter (keep points only nearer than this) float Kfi; //internal parameter, for assigning the points to their corresponding sectors ( = 1 / [2pi/rep] = 1 / [angle between neighboring beams] ) float slope_param; //"slope" parameter for edge detection (given by 2 points, in radial direction/plane) -int dmin_param; //(see below) -float kdev_param; //(see below) -float kdist_param; //(see below) -std::vector padkaIDs(rep); //original ID of points marked as roadside (from input cloud) - -struct polar //polar-coordinate struct for the points -{ - int id; //original ID of point (from input cloud) - float r; //radial coordinate - float fi; //angular coordinate (ccw angle from x-axis) -}; - -struct box //struct for detection beams -{ - std::vector p; //points within the beam's area - box *l, *r; //pointer to adjacent beams (currently not used) - bool yx; //whether it is aligned more with the y-axis (than the x-axis) - float o, d; //internal parameters (trigonometry) -}; +int params::dmin_param; //(see below) +float params::kdev_param; //(see below) +float params::kdist_param; //(see below) +float params::angleFilter3; /*Csaplár László kódjához szükséges. Sugár irányú határérték (fokban).*/ std::vector beams(rep); //beams std::vector beamp(rep + 1); //pointers to the beams (+1 -> 0 AND 360) @@ -45,7 +27,7 @@ float slope(float x0, float y0, float x1, float y1) //get slope between 2 points return (y1 - y0) / (x1 - x0); } -void beam_init() //beam initialization +void Detector::beam_init() //beam initialization { { float fi, off = 0.5 * width; //temporary variables @@ -79,7 +61,7 @@ void beam_init() //beam initialization 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 threadfunc(const int tid, const pcl::PointCloud *cloud) //beam algorithm (filtering, sorting, edge-/roadside detection) - input: beam ID (ordinal position/"which one" by angle), pointcloud +void beamfunc(const int tid, std::vector &array2D) //beam algorithm (filtering, sorting, edge-/roadside detection) - input: beam ID (ordinal position/"which one" by angle), pointcloud (as std::vector, see: 'array2D' of 'lidarSegmentation') { int i = 0, s = beams[tid].p.size(); //loop variables float c; //temporary variable to simplify things @@ -88,8 +70,8 @@ void threadfunc(const int tid, const pcl::PointCloud *cloud) //b { 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 = abs(beams[tid].d * cloud->points[beams[tid].p[i].id].y); //x-coordinate of the beam's centerline at the point (at the "height" of its y-coordinate) - if ((c - beams[tid].o) < cloud->points[beams[tid].p[i].id].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 + c = abs(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 < (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 } @@ -104,8 +86,8 @@ void threadfunc(const int tid, const pcl::PointCloud *cloud) //b { while (i < s) { - c = abs(beams[tid].d * cloud->points[beams[tid].p[i].id].x); - if ((c - beams[tid].o) < cloud->points[beams[tid].p[i].id].y < (c + beams[tid].o)) + c = abs(beams[tid].d * array2D[beams[tid].p[i].id].p.x); + if ((c - beams[tid].o) < array2D[beams[tid].p[i].id].p.y < (c + beams[tid].o)) { i++; } @@ -122,21 +104,21 @@ void threadfunc(const int tid, const pcl::PointCloud *cloud) //b { //edge detection (edge of the roadside) if (s > 1) //for less than 2 points it would be pointless { - int dmin = dmin_param; //minimal number of points required to begin adaptive evaluation - float kdev = kdev_param; //coefficient: weighting the impact of deviation (difference) from average ( = overall sensitivity to changes) - float kdist = 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 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) 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 = cloud->points[beams[tid].p[0].id].z; //y = z-coordinate of the first point (height) + 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 = cloud->points[beams[tid].p[i].id].z; + by = array2D[beams[tid].p[i].id].p.z; slp = slope(ax, ay, bx, by); if (isnan(slp)) @@ -154,8 +136,8 @@ void threadfunc(const int tid, const pcl::PointCloud *cloud) //b (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? { - padkaIDs.push_back(beams[tid].p[i].id); //original ID of point gets registered as roadside - break; //(the roadside is found, time to break the loop) + 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) } } } @@ -163,17 +145,18 @@ void threadfunc(const int tid, const pcl::PointCloud *cloud) //b beams[tid].p.clear(); //evaluation done, the points are no longer needed } -void callback(const pcl::PointCloud &cloud) //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()") +void Detector::starShapedSearch(std::vector &array2D) //entry point to the code, everything gets called here (except for initialization - that needs to be called separately, at the start of the program - "beam_init()") { - beamp.push_back(&beams[0]); //initializing "360 deg = 0 deg" pointer - int f, s = cloud.size(); //temporary variables - float r, fi; //polar coordinates + 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(cloud.points[i].x * cloud.points[i].x + cloud.points[i].y * cloud.points[i].y); //r = sqRoot(x^2+y^2) = distance of point from sensor + 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(cloud.points[i].y, cloud.points[i].x); //angular position of point + 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) @@ -183,10 +166,9 @@ void callback(const pcl::PointCloud &cloud) //entry point to th 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) - padkaIDs.clear(); //clearing previous data for (int i = 0; i < rep; i++) //for every beam... { - threadfunc(i, &cloud); //the heart of the code (beam algorithm) + beamfunc(i, array2D); //the heart of the starshaped method (beam algorithm) } } \ No newline at end of file diff --git a/src/x_zero_method.cpp b/src/x_zero_method.cpp new file mode 100644 index 0000000..1ae3154 --- /dev/null +++ b/src/x_zero_method.cpp @@ -0,0 +1,71 @@ +#include "urban_road_filter/data_structures.hpp" + +float params::angleFilter1; //angle given by three points, while keeping X = 0 +float params::curbHeight; //estimated minimal height of the curb +int params::curbPoints; //estimated number of points on the curb + +void Detector::xZeroMethod(std::vector>& array3D,int index,int* indexArray){ + /*-- step 1.: filtering the NON-road points --*/ + int p2, p3; //2nd and 3rd of the points that are being examined + + /* + --> alpha - angle between the three points and the two vectors + --> x1, x2, x3 - length of the sides of the triangle given by the three points + --> curbPoints - estimated number of points on the roadside + --> 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++) + { + /*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; + } + } + } + } +} \ No newline at end of file diff --git a/src/z_zero_method.cpp b/src/z_zero_method.cpp new file mode 100644 index 0000000..bf2b708 --- /dev/null +++ b/src/z_zero_method.cpp @@ -0,0 +1,76 @@ +#include "urban_road_filter/data_structures.hpp" + +float params::angleFilter2; //angle between two vectors, while keeping Z = 0 + +void Detector::zZeroMethod(std::vector>& array3D,int index,int* indexArray){ + /*-- step 1.: filtering the NON-road points --*/ + 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 + --> 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; + } + } + } + } +} \ No newline at end of file