Extraction de la limite intérieure d'un nuage de points

Extraction des limites du nuage de points, utilisez concavehull pour extraire la limite du polygone concave et utilisez le filtrage par boîte pour extraire la limite intérieure. Le filtrage par boîte doit déterminer les coordonnées du coin supérieur gauche et du coin inférieur droit du cadre de délimitation. Le principe est : 1 Trouvez d'abord la valeur maximale du nuage de points global. Les minimum x et y sont xmax, ymax, ymin, xmin. Recherchez ensuite (xmax+xmin)/2 et (ymax+ymin)/2, vous pouvez alors obtenir que le coin supérieur gauche x1 = la valeur x minimale de tous (ymax+ymin)/2 points correspondants dans s, y1=all ( La valeur minimale de la valeur y du point correspondant xmax+xmin)/2. De la même manière, x2 dans le coin inférieur droit = ​​la valeur x maximale de tous (ymax+ymin)/2 points correspondants dans s, y2 = la valeur y maximale de tous (xmax+xmin)/2 points correspondants. (Notez que lorsque vous comparez des données de nuages ​​de points, vous devez utiliser deux décimales pour comparer)

// 小数点取前两位
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;
}