点云处理操作

挖掘机点云分析

可以看出在整个环境中，有大量冗杂的点云，且点云极为稠密，不利于后续计算，需要进行一些操作。

直通滤波

void passThrough(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    pcl::PassThrough<pcl::PointXYZ> pass_through_filter; //创建滤波器
    pass_through_filter.setInputCloud(cloud);设置输入点云
    pass_through_filter.setFilterLimitsNegative(true);    //设置取反标志
    pass_through_filter.setFilterFieldName("z"); //选择滤波方向
    pass_through_filter.setFilterLimits(3.0f, 6.0f); //设置滤波距离
    //执行滤波处理
    pass_through_filter.filter(*cloud);
    cout<<"passthrough z after:  "<<cloud->size()<<std::endl;
}

• 对z轴直通滤波
  可以看出后面墙壁的点云基本被消除
• 对y轴直通滤波
  

可以看出地面的点云基本被消除

• 对x轴直通滤波
  
  可以看出桌子的点云基本被消除，从动臂截取了一段
  此时的点云数量为8379。

去除离群点

在通过直通滤波后，依然可以发现有很多离群点，就是仿若飘在空中的点云，需要消除掉这部分点云，以方便后续操作。

void Statistical_remove(pcl::PointCloud<pcl::PointXYZ>::Ptr input_points,pcl::PointCloud<pcl::PointXYZ>::Ptr output_points){
    //gettime Statistical("离群点移除");
    //Statistical.getSTARTtime();
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(input_points);
    sor.setMeanK(50);
    sor.setStddevMulThresh(1.0);
    sor.filter(*output_points);
    cout<<"去除离群点后数量"<<output_points->points.size()<<endl;
    //Statistical.getCosttime();
}

去除离群点后数量8188

可以看出去除了一部分的离群点。

降采样

点云处理前，一般要进行降采样，降采样就是将点云体素化，在不改变整体形状的基础上减少点云密度。

void VoxelGrid(pcl::PointCloud<pcl::PointXYZ>::Ptr input_points,pcl::PointCloud<pcl::PointXYZ>::Ptr out_put){
//    boxdetectPrint.print("before VoxelGrid :",input_points->points.size());
//    gettime VoxelGrid("VoxelGrid",boxdetectPrint, true);
//    VoxelGrid.getSTARTtime();
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(input_points);
    sor.setLeafSize(0.01f,0.01f,0.01f);
    sor.filter(*out_put);
//    VoxelGrid.getCosttime();
//    boxdetectPrint.print("after VoxelGrid :",out_put->points.size());

    cout<<"降采样后的数量: "<<out_put->points.size()<<endl;
//    VoxelGrid.getCosttime();

};

降采样后的数量: 3852

很好的减少了点云数量，却保持了大致的形状。

特征点提取

SIFT特征点提取

在按照特诊点提取教程走时，遇到了错误

sift_keypoint.h:49:16: error: ‘const struct pcl::PointXYZ’ has no member named ‘intensity’

查询StackOverflow后，只有一个回答。

Actually, the problem is that I am using pcl::PointXYZ and there
exists a type pcl::PointXYZRGB. This one has the attribute intensity
(of the color) and now it works, just by changing every “PointType” to
“PointXYZRGB”

Harris关键点提取

void HarrisKeyPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr outcloud){
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal> harris;
    harris.setInputCloud(cloud);
    harris.setNonMaxSupression(true);
    harris.setRadius(0.04f);
    harris.setThreshold(0.02f);
    harris.compute(*cloud_out);
    int size = cloud_out->size();
    cout<<"harris特征点数目"<<size<<std::endl;
    pcl::copyPointCloud(*cloud_out,*outcloud);
}

一共提取出十个特征点。
在图上显示为。
可以看出并没有我们想要的左端点的特征点。

获取最左端点

void getLeft(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr outcloud){
    int minY=INT_MAX;
    int minIndex;
    for(int index=0;index<cloud->size();index++){
        if(cloud->points[index].y<minY){
            minIndex=index;
        }
    }
    cout<<"get the min point"<<minIndex<<endl;
    outcloud->points.push_back(cloud->points[minIndex]);
//    pcl::getMinMax();
}

我们希望得到的是挖掘机的最左端点，利用上述代码实现，主要是对PointCloudpcl::PintXYZ::Ptr cloud 进行遍历，访问并比较y方向最大值，结果选取如下。

可以看到选取的点确实在最左端点，但是由于点云边缘不止一个面，会导致选取的点并不在固定的前表面上。需要进行后续操作。目前的初步想法是截取出铲斗部分，然后做ranscac算法平面拟合，使得铲斗只有前表面部分。

截取并做平面拟合

bool boxDetect::SACSegmentation(pcl::PointCloud<pcl::PointXYZ>& input_points,pcl::PointCloud<pcl::PointXYZ>& output_points){
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> sac;
    if(input_points.size()==0){
        //cerr<<"输入ＳＡＣＳ点数为　０"<<endl;
        return false;
    }
    sac.setOptimizeCoefficients(true);
    sac.setModelType(pcl::SACMODEL_PLANE);
    sac.setMethodType(pcl::SAC_RANSAC);
    sac.setDistanceThreshold(0.01);
    sac.setMaxIterations(50);
    sac.setProbability(0.95);
    sac.setInputCloud(input_points.makeShared());
    sac.segment(*inliers,*coefficients);
    if(inliers->indices.size()==0){
        cerr<<"未提取平面"<<endl;
    }else if(inliers->indices.size()<100){
        return false;
    }else{
//        cout<<"平面点数："<<inliers->indices.size()<<endl;
//        cout<<coefficients->values[0]<<" "
//            <<coefficients->values[1]<<" "
//            <<endl;
    }
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(input_points.makeShared());
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter(output_points);
    //cout<<"提取的得到平面点数"<<output_points.size()<<endl;
    return true;
}

并不好使

想了想用直通滤波可能更合适一些，当然需要先获取面的大致坐标，然后做z方向的滤波，效果不错

之后再取极值点

可以看出目前提取的想要的角点，效果不错。