点云内边界提取

点云边界提取，使用concavehull提取凹多边形边界，内边界使用方框滤波来进行提取，方框滤波需要确定边界框的左上角和右下角的坐标，原理为：1. 先求整体点云的最大最小x，y即xmax，ymax,ymin,xmin。再去求取（xmax+xmin）/2 与（ymax+ymin）/2,  那么可以得到，左上角x1 = s所有（ymax+ymin）/2对应点的x值的最小值，y1=所有（xmax+xmin）/2对应点的y值的最小值。同理右下角x2 = s所有（ymax+ymin）/2对应点的x值的最大值，y2=所有（xmax+xmin）/2对应点的y值的最大值。（注意当比较点云数据的时候，需要用小数点后两位去比较）

// 小数点取前两位
double round(double num,int precision)
{
    double factor = pow(10, precision);
    return round(num*factor)/factor;
}

//  凹凸边界
bool Imgdeal::BD_contour(pcl_ptr &CyPort_cloud, pcl_ptr &cloud_in_box)
{
    //------------------------  1、RANSAC拟合平面 ---------------------------
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers1(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.01);
    seg.setInputCloud(CyPort_cloud);
    seg.segment(*inliers1, *coefficients);
    //-----------------------  2、三维点云投影到平面上 ---------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(CyPort_cloud);
    proj.setModelCoefficients(coefficients);
    proj.filter(*cloud_projected);
    std::cout << "投影后点的个数: " << cloud_projected->points.size() << std::endl;
    writer.write("../PointsCloud/cloud_projected.pcd", *cloud_projected);
    //-------------------  3、提取投影平面点云的凹多边形边界  -------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ConcaveHull<pcl::PointXYZ> chull;
    chull.setInputCloud(cloud_projected);
    chull.setAlpha(0.08);
    chull.reconstruct(*cloud_hull);
    std::cout << "凹多边形的点数: " << cloud_hull->points.size() << std::endl;
    writer.write("../PointsCloud/cloud_hull.pcd", *cloud_hull);
    
    //-------------------- 4、区域滤波获取内边缘 ------------------------ 
	std::vector<double> x, y, z;
	for (auto &p : cloud_projected->points)
	{
		x.push_back(p.x);
		y.push_back(p.y);
		z.push_back(p.z);
	}
    // 1、z的最大和最小
	auto minPositionZ = min_element(z.begin(), z.end()); // 最大z值的地址(下标)
	std::vector<double>::iterator iter = find(z.begin(), z.end(), *minPositionZ);
	int index = std::distance(z.begin(), iter); // 获取 [first,last) 范围内包含元素的个数
	double Zmin= *iter;
    auto maxPositionZ = max_element(z.begin(), z.end()); // 最大z值的地址(下标)
	std::vector<double>::iterator iter1 = find(z.begin(), z.end(), *maxPositionZ);
	int index1 = std::distance(z.begin(), iter1); // 获取 [first,last) 范围内包含元素的个数
	double Zmax = *iter1;
    std::cout<<" Zmax:" << Zmax <<" Zmin:" << Zmin <<std::endl;

    // 2、x的最大和最小
    auto minPositionX = min_element(x.begin(), x.end()); // 最大z值的地址(下标)
	std::vector<double>::iterator iter2 = find(x.begin(), x.end(), *minPositionX);
	int index2 = std::distance(x.begin(), iter2); // 获取 [first,last) 范围内包含元素的个数
	double Xmin= *iter2;
    auto maxPositionX = max_element(x.begin(), x.end()); // 最大z值的地址(下标)
	std::vector<double>::iterator iter3 = find(x.begin(), x.end(), *maxPositionX);
	int index3 = std::distance(x.begin(), iter3); // 获取 [first,last) 范围内包含元素的个数
	double Xmax = *iter3;
    std::cout<<" Xmin:" << Xmin <<" Xmax:" << Xmax <<std::endl;

    // 3、y的最大和最小
    auto minPositionY = min_element(y.begin(), y.end()); // 最大z值的地址(下标)
	std::vector<double>::iterator iter4 = find(y.begin(), y.end(), *minPositionY);
	int index4 = std::distance(y.begin(), iter4); // 获取 [first,last) 范围内包含元素的个数
	double Ymin= *iter4;
    auto maxPositionY = max_element(y.begin(), y.end()); // 最大z值的地址(下标)
	std::vector<double>::iterator iter5 = find(y.begin(), y.end(), *maxPositionY);
	int index5 = std::distance(y.begin(), iter5); // 获取 [first,last) 范围内包含元素的个数
	double Ymax = *iter5;
    std::cout<<" Ymin:" << Ymin <<" Ymax:" << Ymax <<std::endl;

    double flag_x = ( Xmin + Xmax)/2;
    double flag_y = ( Ymax + Ymin)/2;
    double x1,x2,y1,y2;
    std::cout<<" round(flag_y,2):" << round(flag_y,2) <<std::endl;

    // 内边界的左上角和右下角的坐标
    // 1、
    std::vector<double> vector_x;
    for (int i = 0; i < cloud_projected->size(); i++)
    {
        if(round(cloud_projected->points[i].y,2) == round(flag_y,2))
        { 
            x1 = cloud_projected->points[i].x;
            vector_x.push_back(x1);
        }
    }
    // for(std::vector<double>::iterator it = vector_x.begin();it !=vector_x.end();it++)
    // {
    //     std::cout<<"vector_x:___"<<*it<<std::endl;
    // }

    double in_xmax = *max_element(vector_x.begin(),vector_x.end());
    double in_xmin = *min_element(vector_x.begin(),vector_x.end());
    std::cout<<"vector_x:in_xmax___"<<in_xmax<<"_in_xmin_"<<in_xmin <<std::endl;

    // 2、
    std::vector<double> vector_y;
    for (int i = 0; i < cloud_projected->size(); i++)
    {
        if(round(cloud_projected->points[i].x,2) == round(flag_x,2))
        { 
            y1 = cloud_projected->points[i].y;
            vector_y.push_back(y1);
        }
    }
    // for(std::vector<double>::iterator it = vector_y.begin();it !=vector_y.end();it++)
    // {
    //     std::cout<<"vector_y:___"<<*it<<std::endl;
    // }
    double in_ymax = *max_element(vector_y.begin(),vector_y.end());
    double in_ymin = *min_element(vector_y.begin(),vector_y.end());
    std::cout<<"vector_x:in_ymax"<<in_ymax<<"_in_ymin_"<<in_ymin <<std::endl;

    //---------------提取缸的边界------------------
    // ------------------------设置方框滤波的范围----------------------------------
	// 前三个值是坐标xyz的范围，第四个值是用来占位置的不用管
    Eigen::Vector4f min_pt = {(float)(in_xmin+0.05),(float)(in_ymin+0.05),(float)Zmin,0};
	Eigen::Vector4f max_pt = {(float)(in_xmax-0.05),(float)(in_ymax-0.05),(float)Zmax,0};
	// -----------------------获取位于方框内的点索引-------------------------------
	pcl::IndicesPtr indexes(new pcl::Indices());
	pcl::getPointsInBox(*cloud_hull, min_pt, max_pt, *indexes);
	// --------------------------取框内和框外点------------------------------------
	pcl::ExtractIndices<pcl::PointXYZ> extr;
	extr.setInputCloud(cloud_hull);  // 设置输入点云
	extr.setIndices(indexes);   // 设置索引
	extr.filter(*cloud_in_box); // 提取对应索引的点云
	cout << "方框内点的个数为：" << cloud_in_box->points.size() << endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out_box(new pcl::PointCloud<pcl::PointXYZ>);
	extr.setNegative(true);    // 提取对应索引之外的点
	extr.filter(*cloud_out_box);
	cout << "方框外点的个数为：" << cloud_out_box->points.size() << endl;
	// ------------------------保存提取结果到本地文件夹---------------------------
    writer.write("../PointsCloud/cloud_in_box.pcd", *cloud_in_box);
    writer.write("../PointsCloud/cloud_out_box.pcd", *cloud_out_box);
    std::cerr << "滤波前有: " << cloud_in_box->size() << " 个点 " << std::endl;

    return true;
}