extract_ground.cpp

#include <vector>
#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl_msgs/PointIndices.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h> 
#include <pcl/filters/passthrough.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/features/normal_3d.h>
#include <iostream>
#include <array>
#include <pcl/filters/voxel_grid.h>
#include "extract_ground.h"

using namespace std;
using namespace sensor_msgs;
using namespace pcl;

ros::Publisher auxpoint_pub;
ros::Publisher groundcloud_pub;
ros::Publisher cell_pub;
std_msgs::Header header_;

Cell::Cell(){
    minZ = 1000;
    maxZ = -1000;
    size = 0;
    exce_size = 0;
    isGround = false;
    isCover = false;
    isNeighbor = false;
    car = false;
}

const float cell_length = 0.5;
const int numRow = 151;
const int numCol = 101;
const int central_y = 60;
const int central_x = 25;

float rMin = 2.4;
float rMax = 50;
float tHmin ;
float tHmax ;
float tHDiff = 0.1;
float tInnerDiff = 0.1;
float hSeonsor = 2.2;
int row_offset, col_offset;

int car_left ;
int car_right;
int car_up ;
int car_down ;

pcl::ConditionalRemoval<pcl::PointXYZI> condrem;

Eigen::Affine3d getRotationMatrix(Eigen::Vector3d source, Eigen::Vector3d target){
      Eigen::Vector3d rotation_vector = target.cross(source);
      rotation_vector.normalize();
      double theta = acos(source[2]/sqrt( pow(source[0],2)+ pow(source[1],2) + pow(source[2],2)));
      Eigen::Matrix3d rotation = Eigen::AngleAxis<double>(theta, rotation_vector) * Eigen::Scaling(1.0);
      Eigen::Affine3d rot(rotation);
      return rot;
}


void getCellIndexFromPoints(float x, float y, int &row, int &col){
    float x1,y1;
    y1 = round(y/cell_length);
    x1 = round(x/cell_length);
    row = y1 + row_offset;
    col = x1 + col_offset;
}

void getPointFromCellIndex(int row, int col, float &x, float &y){
    row -= row_offset;
    col -= col_offset;
    x = col * cell_length;
    y = row * cell_length;
}



void filterCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<PointXYZI> & filteredCloud,
                    array<array<Cell, numCol>, numRow>& polarData){
    for (int i = 0; i < cloud->size(); i++) {
        float x = cloud->points[i].x;
        float y = cloud->points[i].y;
        float z = cloud->points[i].z;
        float intensity = cloud->points[i].intensity;
        float distance = sqrt(x * x + y * y);
        if( (y > 60 || y < -15) || ( x < -25 || x > 25 ) || (y < 4.5 && y > -4 && x < 2 && x > -3) 
            ) {
            continue; // filter out
        }
        else{
            pcl::PointXYZI o;
            o.x = x;
            o.y = y;
            o.z = z;
            o.intensity = intensity;
            filteredCloud.push_back(o);
            int row, col;
            getCellIndexFromPoints(x, y, row, col);
            if(row < 0 || row >=numRow || col < 0 || col >= numCol){
                continue;
            }  // to prevent segentation fault
            polarData[row][col].updateMinZ(z);
            polarData[row][col].updateMaxZ(z);
            if(polarData[row][col].isThisCover() == false)
                polarData[row][col].updateCover();
    
        }
    }
}



void filter_mid_area_limitation(){
//创建条件限定下的滤波器

    pcl::ConditionAnd<pcl::PointXYZI>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZI>);
    pcl::FieldComparison<pcl::PointXYZI>::ConstPtr cond_1(new pcl::FieldComparison<pcl::PointXYZI>("x",
        pcl::ComparisonOps::GT,-10));
    range_cond->addComparison(cond_1);
    pcl::FieldComparison<pcl::PointXYZI>::ConstPtr cond_2(new pcl::FieldComparison<pcl::PointXYZI>("x",
        pcl::ComparisonOps::LT,10));
    range_cond->addComparison(cond_2);


    pcl::FieldComparison<pcl::PointXYZI>::ConstPtr cond_3(new pcl::FieldComparison<pcl::PointXYZI>("y",
        pcl::ComparisonOps::GT,-20));
    range_cond->addComparison(cond_3);

    pcl::FieldComparison<pcl::PointXYZI>::ConstPtr cond_4(new pcl::FieldComparison<pcl::PointXYZI>("y",
        pcl::ComparisonOps::LT,50));
    range_cond->addComparison(cond_4);

    condrem.setCondition(range_cond);


    //创建滤波器并用条件定义对象初始化
}

void filter_mid_area(pcl::PointCloud<pcl::PointXYZI> ::Ptr cloud_in){
    condrem.setInputCloud(cloud_in);
    condrem.setKeepOrganized(false);
    condrem.filter(*cloud_in);
}

bool estimateGroundPlane(pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr &out_cloud,
                                  const float in_distance_thre)
{
    // 滤除一定高度以上的点
    pcl::PointCloud<pcl::PointXYZI>::Ptr z_filter_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::PointCloud<pcl::PointXYZI>::Ptr z_filter_cloud1 (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::PassThrough<pcl::PointXYZI>pass;     //设置滤波器对象
    pass.setInputCloud(in_cloud);                //设置输入点云
    pass.setFilterFieldName("z");             //设置过滤时所需要点云类型的z字段
    pass.setFilterLimits(tHmin-0.2,tHmax+0.2);           //设置在过滤字段上的范围
    //pass.setFilterLimitsNegative (true);     //设置保留范围内的还是过滤掉范围内的
    pass.filter(*z_filter_cloud);              //执行滤波，保存过滤结果在cloud_filtered


    filter_mid_area(z_filter_cloud);
    sensor_msgs::PointCloud2 ros_cloud;
    pcl::toROSMsg(*z_filter_cloud, ros_cloud);
    ros_cloud.header = header_;
    auxpoint_pub.publish(ros_cloud);


    // 水平面校准
    pcl::SACSegmentation<pcl::PointXYZI> plane_seg;
    pcl::PointIndices::Ptr plane_inliers ( new pcl::PointIndices );
    pcl::ModelCoefficients::Ptr plane_coefficients ( new pcl::ModelCoefficients );
    plane_seg.setOptimizeCoefficients (true);
    plane_seg.setModelType ( pcl::SACMODEL_PLANE );
    plane_seg.setMethodType ( pcl::SAC_RANSAC );
    plane_seg.setDistanceThreshold ( in_distance_thre );
    plane_seg.setInputCloud ( z_filter_cloud );
    plane_seg.segment ( *plane_inliers, *plane_coefficients );
    if (plane_inliers->indices.size () == 0)
    {
        ROS_ERROR ("Could not estimate a planar model for the given dataset.");
        return (-1);
    }
    Eigen::Vector3d xy_plane_normal_vector, floor_plane_normal_vector;
    xy_plane_normal_vector[0] = 0.0;
    xy_plane_normal_vector[1] = 0.0;
    xy_plane_normal_vector[2] = -1.0;

    floor_plane_normal_vector[0] = plane_coefficients->values[0];
    floor_plane_normal_vector[1] = plane_coefficients->values[1];
    floor_plane_normal_vector[2] = plane_coefficients->values[2];

    Eigen::Affine3d rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector);
    pcl::transformPointCloud(*in_cloud, *z_filter_cloud1, rotation);

    pcl::PassThrough<pcl::PointXYZI>pass1;     //设置滤波器对象
    pass1.setInputCloud(z_filter_cloud1);                //设置输入点云
    pass1.setFilterFieldName("z");             //设置过滤时所需要点云类型的z字段
    pass1.setFilterLimits(-3.0, 0.3);           //设置在过滤字段上的范围
    //pass.setFilterLimitsNegative (true);   if(intensity > 15)
            //     polarData[chI][binI].updateMaxZ(hSeonsor);  //设置保留范围内的还是过滤掉范围内的
    pass1.filter(*out_cloud);              //执行滤波，保存过滤结果在cloud_filtered
    



    return true;
}



double gauss(double sigma, double x) {
    double expVal = -1 * (pow(x, 2) / pow(2 * sigma, 2));
    double divider = sqrt(2 * M_PI * pow(sigma, 2));
    return (1 / divider) * exp(expVal);
}

std::vector<double> gaussKernel(int samples, double sigma) {
    std::vector<double> kernel(samples);
    double mean = samples/2;
    double sum = 0.0; // For accumulating the kernel values
    for (int x = 0; x < samples; ++x) {
        kernel[x] = exp( -0.5 * (pow((x-mean)/sigma, 2.0)))/(2 * M_PI * sigma * sigma);
        // Accumulate the kernel values
        sum += kernel[x];
    }

// Normalize the kernelupdateGround
    for (int x = 0; x < samples; ++x){
        kernel[x] /= sum;
    }

    // std::cout << "The kernel contains " << kernel.size() << " entries:";
    for (auto it = kernel.begin(); it != kernel.end(); ++it) {
        // std::cout << ' ' << *it;
    }
    // std::cout << std::endl;
    assert(kernel.size() == samples);

    return kernel;
}
void gaussSmoothen(std::array<Cell, numCol>& values, double sigma, int samples) {
    auto kernel = gaussKernel(samples, sigma);
    int sampleSide = samples / 2;
    unsigned long ubound = values.size();
    // applying gaussian kernel with zero padding
    for (long i = 0; i < ubound; i++) {
        double smoothed = 0;
        for (long j = i - sampleSide; j <= i + sampleSide; j++) {
            if (j >= 0 && j < ubound) {
                int sampleWeightIndex = sampleSide + (j - i);
                smoothed += kernel[sampleWeightIndex] * values[j].getHeight();
            }
        }
        // std::cout << " V: " << values[i].getHeight() << " SM: " << smoothed << std::endl;
        values[i].updateSmoothed(smoothed);
    }
}

// update HDiff with larger value
void computeDiff(array<Cell, numCol>& channelData){
    for(int i = 0; i < channelData.size(); i++){
        // edge case
        if(i == 0){
            float hD = channelData[i].getHeight() - channelData[i+1].getHeight();
            channelData[i].updateHDiff(hD);
        }
        else if(i == channelData.size()-1){
            float hD = channelData[i].getHeight() - channelData[i-1].getHeight();
            channelData[i].updateHDiff(hD);
        }
        // non-edge case
        else{
            float preHD  = channelData[i].getHeight() - channelData[i-1].getHeight();
            float postHD = channelData[i].getHeight() - channelData[i+1].getHeight();
            if(preHD > postHD) channelData[i].updateHDiff(preHD);
            else channelData[i].updateHDiff(postHD);
        }
        float innerDiff = channelData[i].getMaxZ() - channelData[i].getMinZ();
        channelData[i].updateInnerDiff(innerDiff);

//        cout <<channelData[i].getHeight() <<" " <<channelData[i].getHDiff() << endl;
    }
}

void estimateNeighbor( array<array<Cell, numCol>, numRow>& polarData,  int row_center, int col_center){
    for(int row = row_center  ; row <= row_center + 10; row++){
        for(int col = col_center -10 ; col <= col_center + 10; col ++ ){
            if(row < 0 || row >=numRow || col < 0 || col >= numCol)
                continue;
            if((float)((row - row_center) * (row - row_center) + (col - col_center) * (col - col_center) ) > 123 ) 
                continue;

            if(polarData[row][col].isThisCover() && 
                (polarData[row][col].getMaxZ() > -1.5 || polarData[row][col].getInnerDiff() >= tInnerDiff) ){
                int step = abs(row - row_center) + abs(col - col_center);
                float row_slope = (float)(row - row_center) / step;
                float col_slope = (float)(col - col_center) / step;
                // if(col_slope * col_slope < 0.5)
                for(int i = 0; i < step; i++){
                    int row_step = round((float)(row_center + row_slope * i));
                    int col_step = round((float)(col_center + col_slope * i));
                    if(row_step < 0 || row_step >=numRow || col_step < 0 || col_step >= numCol){
                        // cout << "over ";
                        // cout << " estimateNeighbor " << row << " " << col << " " << row_center  << " " << col_center  << " " <<
                        //     row_slope  << " " << col_slope << endl;
                        continue;
                    }
                    polarData[row_step][col_step].updateNeighbor();
                }
            }
        }
    }

}


void FindAdj(array<array<int, numCol>, numRow>& Mapcell, array<array<Cell, numCol>, numRow>& polarData, 
                object_tracking::Road_extract& to_send, int row, int col, int step, int mode){
    if((row < car_up && row > car_down) && (col < car_right && col > car_left )
        || row < 0 || row >=numRow || col >= numCol || col <= 0 ){
            // std::cout <<"in ";
            return;
        }
    int formula1 = row - (-1.0 * (float)col + 80);
    int formula2 = row - (1.0 * (float)col - 20);
    if(mode == 1 && !(formula1 >= 0 && formula2 >= 0 ) ) return;
    if(mode == 2 && !(formula1 >= 0 && formula2 < 0)) return;
    if(mode == 3 && !(formula1 < 0 && formula2 < 0)) return;
    if(mode == 4 && !(formula1 < 0 && formula2 >= 0)) return;
    // if(mode != 1) return;
     if(Mapcell[row][col] > 0 || step > 120){   return;} //  || step > 120

    if((polarData[row][col].isThisCover() == true && polarData[row][col].isThisGround() == false)
         || polarData[row][col].isThisNeighbor()  ){ // 
        Mapcell[row][col] = 1000;
        return ;
    }

        Mapcell[row][col] = 1;
        float cell_x;
        float cell_y;
        getPointFromCellIndex(row, col, cell_x, cell_y);
        to_send.x.push_back(cell_x);
        to_send.y.push_back(cell_y);
        to_send.z.push_back(0);

        if(mode == 1){
            FindAdj(Mapcell, polarData, to_send, row , col + 1, step + 1, 1);
            FindAdj(Mapcell, polarData, to_send, row + 1, col, step + 1, 1);
            FindAdj(Mapcell, polarData, to_send, row , col - 1, step + 1, 1);
        }
        else if (mode == 2){
            FindAdj(Mapcell, polarData, to_send, row , col + 1, step + 1, 2);
            FindAdj(Mapcell, polarData, to_send, row + 1, col, step + 1, 2);
            FindAdj(Mapcell, polarData, to_send, row - 1, col, step + 1, 2);
        }
        else if (mode == 3){
            FindAdj(Mapcell, polarData, to_send, row , col + 1, step + 1, 3);
            FindAdj(Mapcell, polarData, to_send, row - 1, col, step + 1, 3);
            FindAdj(Mapcell, polarData, to_send, row , col - 1, step + 1, 3);
        }
        else if (mode == 4) {
            FindAdj(Mapcell, polarData, to_send, row , col - 1, step + 1, 4);
            FindAdj(Mapcell, polarData, to_send, row + 1, col, step + 1, 4);
            FindAdj(Mapcell, polarData, to_send, row - 1, col, step + 1, 4);
        }
        

        return;
    

}

void applyMedianFilter(array<array<Cell, numCol>, numRow>& polarData){
    // maybe later: consider edge case
    for(int channel = 1; channel < polarData.size()-1; channel++){
        for(int bin = 1; bin < polarData[0].size()-1; bin++){
            if(!polarData[channel][bin].isThisGround()){
                // target cell is non-ground AND surrounded by ground cells
                if(polarData[channel][bin+1].isThisGround()&&
                   polarData[channel][bin-1].isThisGround()&&
                   polarData[channel+1][bin].isThisGround()&&
                   polarData[channel-1][bin].isThisGround()){
                    vector<float> sur{polarData[channel][bin+1].getHeight(),
                                      polarData[channel][bin-1].getHeight(),
                                      polarData[channel+1][bin].getHeight(),
                                      polarData[channel-1][bin].getHeight()};
                    sort(sur.begin(), sur.end());
                    float m1 = sur[1]; float m2 = sur[2];
                    float median = (m1+m2)/2;
                    polarData[channel][bin].updataHeight(median);
                    polarData[channel][bin].updateGround();
                }
            }
        }
    }
}

void cloud_callback(const PointCloud2::ConstPtr& input_cloud){
    ros::Time begin_time = ros::Time::now ();
    pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::PointCloud<pcl::PointXYZI>::Ptr rotation_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::PointCloud<pcl::PointXYZI>::Ptr filter_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::PointCloud<pcl::PointXYZI>::Ptr filter_cloud1 (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr normal_cloud (new pcl::PointCloud<pcl::PointXYZINormal> ());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr extract_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr ground_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    pcl::fromROSMsg(*input_cloud, *laser_cloud);
    header_  = input_cloud->header;
    object_tracking::Road_extract to_send;
    to_send.header = header_;
    to_send.cell_length = cell_length;
    to_send.cell_width = cell_length;
    
    estimateGroundPlane(laser_cloud, rotation_cloud, 0.01);
    sensor_msgs::PointCloud2 ros_cloud;
    pcl::toROSMsg(*rotation_cloud, ros_cloud);
    ros_cloud.header = header_;
    // auxpoint_pub.publish(ros_cloud);
    pcl::VoxelGrid<pcl::PointXYZI> sor;  
    sor.setInputCloud(rotation_cloud);  
    sor.setLeafSize(0.1f, 0.1f, 0.05f);  
    sor.filter(*filter_cloud);  

    array<array<Cell, numCol>, numRow> polarData;
    array<array<int, numCol>, numRow> Mapcell;


    filterCloud(filter_cloud, *filter_cloud1, polarData);

    for (int row = 0; row < numRow; row++){
        for (int col = 0; col < numCol; col ++){
            float zi = polarData[row][col].getMinZ();
            if(zi > tHmin && zi < tHmax){polarData[row][col].updataHeight(zi);}
            else if(zi <= tHmin) {polarData[row][col].updataHeight(tHmin);}
            Mapcell[row][col] = -1 ;
        }

        gaussSmoothen(polarData[row], 1, 3);
        computeDiff(polarData[row]);

        for (int col = 0; col < numCol; col ++){
            if(polarData[row][col].getHeight() < tHmax &&
                    polarData[row][col].getHDiff() < tHDiff && 
                        polarData[row][col].getInnerDiff() < tInnerDiff //&&
                             ){// && polarData[row][col].isThisNeighbor()
                polarData[row][col].updateGround();
                // std::cout <<"update ";
            }
            else if(polarData[row][col].getSmoothed() < tHmax &&
                        polarData[row][col].getHDiff() < tHDiff && 
                            polarData[row][col].getInnerDiff() < tInnerDiff //
                                 ){// && polarData[row][col].isThisNeighbor()
                polarData[row][col].updateGround();
                // std::cout <<"update ";
                
            }
        }


    }

    for(int row_center = 0; row_center < numRow; row_center ++ ){
        for(int col_center = 0; col_center < numCol; col_center ++ ){
            if(polarData[row_center][col_center].isThisCover() && 
                (polarData[row_center][col_center].getMaxZ() > -1.5 || polarData[row_center][col_center].getInnerDiff() >= tInnerDiff) )
                    estimateNeighbor(polarData, row_center, col_center);
        }
    }


    // implement MedianFilter
//    applyMedianFilter(polarData);   
    

    for(int row = car_down; row <= car_up; row++){
        int col = car_right;
        while(!polarData[row][col].isThisCover() && col - car_right<10){
            col++;
        }
        if(polarData[row][col].isThisCover()){
            FindAdj(Mapcell,polarData, to_send,row,col,1, 2);
        }

        col = car_left;
        while(!polarData[row][col].isThisCover() && car_left - col < 10){
            col--;
        }     
        if(polarData[row][col].isThisCover())           
            FindAdj(Mapcell,polarData, to_send,row,col,1,4);

    }

    for(int col = car_left; col <= car_right; col++){
        int row = car_up;
        while(!polarData[row][col].isThisCover() && row - car_up<10){
            row++;
        }
        if(polarData[row][col].isThisCover())
            FindAdj(Mapcell,polarData, to_send,row,col,1,1);
        
        row = car_down;
        while(!polarData[row][col].isThisCover() && car_down - row <10){
            row--;
        }
        if(polarData[row][col].isThisCover())        
            FindAdj(Mapcell,polarData, to_send,row,col,1,3);

    }



    for(int i = 0; i < filter_cloud1->size(); i++) {
        float x = filter_cloud1->points[i].x;
        float y = filter_cloud1->points[i].y;
        float z = filter_cloud1->points[i].z;

        float intensity = filter_cloud1->points[i].intensity;

        pcl::PointXYZRGB o;
        o.x = x;
        o.y = y;
        o.z = z;
        o.r = 0;
        o.g = 255;
        o.b = 0;

        int row, col;
        getCellIndexFromPoints(x, y, row, col);
        if(row < 0 || row >=numRow || col < 0 || col >= numCol) continue;
        
        if (polarData[row][col].isThisGround()) {            
            // float hGround = polarData[row][col].getHGround();
            // if (z < (hGround + 0.25) && Mapcell[row][col] < 1000  && Mapcell[row][col] > 0  ) { //
            //     extract_cloud->push_back(o);
            // } 
            extract_cloud->push_back(o);
        } 
    }

    sensor_msgs::PointCloud2 ros_ground;
    pcl::toROSMsg(*extract_cloud, ros_ground);
    ros_ground.header = header_;
    groundcloud_pub.publish(ros_ground);
    cell_pub.publish(to_send);



    double clustering_time = (ros::Time::now () - begin_time).toSec ();
    ROS_INFO ("%f secs .", clustering_time);  
}


int main (int argc, char **argv){
    ros::init(argc, argv, "Groundcloud_extract");
    ros::NodeHandle nh_("~");
    row_offset = numRow - central_y / cell_length - 1;
    col_offset = numCol - central_x / cell_length - 1;
    car_left = -5 + col_offset;
    car_right =  5 + col_offset;
    car_up = 6 + row_offset;
    car_down =  -6 + row_offset;
    cout << car_down << "  "<< car_up << "  " << car_left << "  " << car_right << "  " << row_offset << "  " << col_offset<<endl;


    ros::Subscriber sub = nh_.subscribe("/velodyne_points", 2, cloud_callback);
    nh_.param<float>("tHmax", tHmax, -2.1);
    nh_.param<float>("tHmin", tHmin, -2.4);
    filter_mid_area_limitation();
    groundcloud_pub = nh_.advertise<sensor_msgs::PointCloud2>("ground_cloud",1);
    auxpoint_pub = nh_.advertise<sensor_msgs::PointCloud2>("aux_points",1);
    cell_pub = nh_.advertise<object_tracking::Road_extract>("ground_cell",1);
    // ros::Rate loop_rate(100);
    // while(ros::ok()){
    //     ros::spinOnce();
    // }
    ros::spin();

}