Skip to content

Commit

Permalink
refactor: Minor bug fix #1
Browse files Browse the repository at this point in the history
  • Loading branch information
@dobaybalazs
dobaybalazs committed Feb 22, 2022
1 parent 81e71c7 commit 78eeddd
Show file tree
Hide file tree
Showing 4 changed files with 283 additions and 268 deletions.
16 changes: 1 addition & 15 deletions include/urban_road_filter/DataStructures.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,21 +35,6 @@ using namespace boost::assign;

typedef boost::geometry::model::d2::point_xy<float> xy;

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)
};

struct Point2D{
pcl::PointXYZI p;
float d;
Expand Down Expand Up @@ -91,6 +76,7 @@ class Detector{
void quickSort(std::vector<std::vector<Point3D>>& array3D, int arc, int low, int high);

void filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud);

private:
ros::Publisher pub_road;
ros::Publisher pub_high;
Expand Down
236 changes: 36 additions & 200 deletions src/lidarSegmentation.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,8 @@
#include "urban_road_filter/DataStructures.hpp"
#include "starShapedSearch.cpp"

/*Global variables.*/
int channels = 64; /* The number of channels of the LIDAR .*/

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)
std::vector<int> padkaIDs(rep); //original ID of points marked as roadside (from input cloud)

std::string fixedFrame; /* Fixed Frame.*/
std::string topicName; /* subscribed topic.*/
bool x_zero_method, y_zero_method, star_shaped_method ; /*Methods of roadside detection*/
Expand All @@ -24,209 +16,49 @@ float angleFilter1; /*X = 0 érték mellett, h
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.*/
int dmin_param; //(see below)
float kdev_param; //(see below)
float kdist_param; //(see below)

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 */

std::vector<box> beams(rep); //beams
std::vector<box *> beamp(rep + 1); //pointers to the beams (+1 -> 0 AND 360)

bool ptcmpr(polar a, polar b) //comparison by r-coordinates
{
return (a.r < b.r);
}
Detector::Detector(ros::NodeHandle* nh){
/*Feliratkozás az adott topicra.*/
sub = nh->subscribe(topicName, 1, &Detector::filtered,this);
/*A szűrt adatok hírdetése.*/
pub_road = nh->advertise<pcl::PCLPointCloud2>("road", 1);
pub_high = nh->advertise<pcl::PCLPointCloud2>("curb", 1);
pub_box = nh->advertise<pcl::PCLPointCloud2>("roi", 1); // ROI - region of interest
pub_pobroad = nh->advertise<pcl::PCLPointCloud2>("road_probably", 1);
pub_marker = nh->advertise<visualization_msgs::MarkerArray>("road_marker", 1);

float slope(float x0, float y0, float x1, float y1) //get slope between 2 points given by x and y coordinates
{
return (y1 - y0) / (x1 - x0);
}
/*Csaplár László kódjához szükséges.*/
beam_init();

/*STAR-SHAPED METHOD*/
void beam_init() //beam initialization (needs to be called only at the start)
{
{
float fi, off = 0.5 * width; //temporary variables
for (int i = 0; i < rep; i++) //for every beam...
{
fi = i * 2 * M_PI / rep; //angle of beam
if (abs(tan(fi)) > 1) //"slope" of beam ( x <---> y )
{
beams[i].yx = true; //aligning more with y-direction
beams[i].d = tan(0.5 * M_PI - fi); // = 1/tan(fi) [coefficient]
beams[i].o = off / sin(fi); //projection of half beam-width in the x-direction (how far from its centerline is the edge of the beam in the x-dir)
}
else
{
beams[i].yx = false; //aligning more with x-direction
beams[i].d = tan(fi); //coefficient
beams[i].o = off / cos(fi); //y-axis projection of half beam-width
}
beamp[i] = &beams[i]; //initializing the pointers
}
}
ROS_INFO("Ready");

for (int i = 0, j = 1; j < rep; i++, j++) //initializing the pointers to adjacent beams
{
beams[i].l = &beams[j];
beams[j].r = &beams[i];
}
beams[0].r = &beams[rep];
beams[rep].l = &beams[0];

Kfi = rep / (2 * M_PI); //should be 2pi/rep, but then we would have to divide by it every time - using division only once and then multiplying later on should be somewhat faster (?)
}
/*FÜGGVÉNYEK*/

void starfunc(const int tid, const pcl::PointCloud<pcl::PointXYZI> *cloud) //beam algorithm (filtering, sorting, edge-/roadside detection) - input: beam ID (ordinal position/"which one" by angle), pointcloud
/*Rekurziv, gyors rendező függvény. (1/2)*/
int Detector::partition(std::vector<std::vector<Point3D>>& array3D, int arc,int low, int high)
{
int i = 0, s = beams[tid].p.size(); //loop variables
float c; //temporary variable to simplify things

if (beams[tid].yx) //filtering the points lying outside the area of the beam... (case 1/2, y-direction)
{
while (i < s) //iterating through points in the current sector (instead of a for loop - since 's' is not constant and 'i' needs to be incremented conditionally)
{
c = 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
{
i++; //okay, next one
}
else //if outside the area
{
beams[tid].p.erase(beams[tid].p.begin() + i); //remove point
s--; //the size has shrunk because of the deletion of a point (and its place is taken by the next point, so 'i' does not need to be changed)
}
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]);
}
}
else //same but with x and y swapped (case 2/2, x-direction)
{
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))
{
i++;
}
else
{
beams[tid].p.erase(beams[tid].p.begin() + i);
s--;
}
}
}

std::sort(beams[tid].p.begin(), beams[tid].p.end(), ptcmpr); //sorting points by r-coordinate (radius)

{ //edge detection (edge of the roadside)
if (s > 1) //for less than 2 points it would be pointless
{
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)

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)

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;
slp = slope(ax, ay, bx, by);

if (isnan(slp))
nan++; //Not-a-Number correction
else //calculating (updating) average and deviation
{
avg *= i - nan - 1; //"unpacking" average value (average -> sum, + NaN correction)
avg += slp; //insertion of new element
avg *= 1 / (i - nan); //sum -> average
dev *= i - nan - 1; //Absolute Average Deviation -> sum ("unpacking")
dev += abs(slp - avg); //insertion of new element (absolute difference from average)
dev *= 1 / (i - nan); //sum -> AAD
}
if ( slp > slope_param || //evaluation of the slope -> using a constant + adaptive:
(i > dmin && (slp * slp - avg * avg) * kdev * ((bx - ax) * kdist) > dev) //if sufficient number of points: does the (weighted) "squared difference" from average - corrected with...
) //... the (weighted) distance of adjacent points - exceed the average absolute deviation?
{
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)
}
}
}
}
beams[tid].p.clear(); //evaluation done, the points are no longer needed
}

void starshaped(const pcl::PointCloud<pcl::PointXYZI> &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()")
{
beamp.push_back(&beams[0]); //initializing "360 deg = 0 deg" pointer
int f, s = cloud.size(); //temporary variables
float r, fi; //polar coordinates

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

fi = atan2(cloud.points[i].y, cloud.points[i].x); //angular position of point

if (fi < 0)
fi += 2 * M_PI; //handling negative values (-180...+180 -> 0...360)

f = (int)(fi * Kfi); //which one of the beams (sectors, containing the beam) does it fall into

beamp[f]->p.push_back(polar{i, r, fi}); //adding the point to the 'f'-th beam (still unfiltered)
}
beamp.pop_back(); //removing pointer (to prevent "double free" error)
padkaIDs.clear(); //clearing previous data

for (int i = 0; i < rep; i++) //for every beam...
{
starfunc(i, &cloud); //the heart of the star-shaped method (beam algorithm)
}
std::swap(array3D[arc][i+1],array3D[arc][high]);
return (i + 1);
}
Detector::Detector(ros::NodeHandle* nh){
/*Feliratkozás az adott topicra.*/
sub = nh->subscribe(topicName, 1, &Detector::filtered,this);
/*A szűrt adatok hírdetése.*/
pub_road = nh->advertise<pcl::PCLPointCloud2>("road", 1);
pub_high = nh->advertise<pcl::PCLPointCloud2>("curb", 1);
pub_box = nh->advertise<pcl::PCLPointCloud2>("roi", 1); // ROI - region of interest
pub_pobroad = nh->advertise<pcl::PCLPointCloud2>("road_probably", 1);
pub_marker = nh->advertise<visualization_msgs::MarkerArray>("road_marker", 1);

/*Csaplár László kódjához szükséges.*/
beam_init();

ROS_INFO("Ready");

}
/*FÜGGVÉNYEK*/

/*Rekurziv, gyors rendező függvény. (1/2)*/
int Detector::partition(std::vector<std::vector<Point3D>>& array3D, int arc,int low, int high)
{
float pivot = array3D[arc][high].alpha;
int i = (low - 1);
for (int j = low; j <= high - 1; j++){
if (array3D[arc][j].alpha < pivot){
i++;
std::swap(array3D[arc][i],array3D[arc][j]);
}
}
std::swap(array3D[arc][i+1],array3D[arc][high]);
return (i + 1);
}

/*Rekurziv, gyors rendező függvény. (2/2)*/
/*Rekurziv, gyors rendező függvény. (2/2)*/
void Detector::quickSort(std::vector<std::vector<Point3D>>& array3D, int arc, int low, int high)
{
if (low < high)
Expand Down Expand Up @@ -257,8 +89,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud)
point.y >= min_Y && point.y <= max_Y &&
point.z >= min_Z && point.z <= max_Z &&
point.x + point.y + point.z != 0){

pt=point;
pt = point;
cloud_filtered_Box.push_back(pt);
}
}
Expand All @@ -274,7 +105,7 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud)
/*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);
starshaped(cloud_filtered_Box);
callback(cloud_filtered_Box);
}

/*
Expand Down Expand Up @@ -304,8 +135,8 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud)
/*A 2D tömb feltöltése.*/
for (i = 0; i < piece; i++){
/*--- Az első 4 oszlop feltöltése. ---*/
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));
array2D[i].p=cloud_filtered_Box.points[i];

/*--- Az 5. oszlop feltöltése. ---*/
bracket = abs(array2D[i].p.z) / array2D[i].d;
Expand Down Expand Up @@ -923,8 +754,13 @@ void Detector::filtered(const pcl::PointCloud<pcl::PointXYZI> &cloud)
for (j = 0; j < indexArray[i]; j++)
{
pt = array3D[i][j].p;
if (array3D[i][j].isCurbPoint == 1) cloud_filtered_Road.push_back(pt); /*Az út pontok.*/
else if (array3D[i][j].isCurbPoint == 2) cloud_filtered_High.push_back(pt); /*A magas pontok.*/
/*Az út pontok.*/
if (array3D[i][j].isCurbPoint == 1)
cloud_filtered_Road.push_back(pt);

/*A magas pontok.*/
else if (array3D[i][j].isCurbPoint == 2)
cloud_filtered_High.push_back(pt);
}
}

Expand Down
Loading

0 comments on commit 78eeddd

Please sign in to comment.