环状点云内插思路分享

大家好啊，今天来聊一下环状点云内插的事儿，介绍一下我的思路。

如下图，有一片环状点云，如果需要向其内部插入点，应该怎么做？

我的思路其实非常简单，

1、抽稀环状点云

2、将抽稀后环状点云排序并构成闭合多边形

3、计算抽稀后的环状点云的包围盒，在包围中随机生成若干点，判断这些点是否在上一步构成的闭合多边形内部

4、输出结果

给大家看一下效果吧，这是抽稀后的环状点云：

这是向其内部插值填充后的结果：

完整代码如下，引用的函数我也补充在下面了：

void fullfill2(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    //1、环状点云抽稀
    cloud = Evenly_Dilute(cloud, 50);
	//2、环状点云排序
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
    sortPointCloudLoop(cloud, cloud2);
	//3、环状点云包围盒计算
    std::vector<float> edge_length;
    pcl::PointXYZ minPoint, maxPoint;
    pcl::getMinMax3D(*cloud2, minPoint, maxPoint);
	//4、生成随机点并验证是否在多边形内
    pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < 10000; i++)
    {
        pcl::PointXYZ randomPoint;
        randomPoint.x = generateRandomDouble(minPoint.x, maxPoint.x);
        randomPoint.y = generateRandomDouble(minPoint.y, maxPoint.y);
        randomPoint.z = 0;
        if (pointcloud_polygonsContain(cloud2, randomPoint))
        {
            result->points.push_back(randomPoint);
        }
    }
    //5、保存结果并退出
    pcl::io::savePCDFile("pIVp.pcd", *cloud);
}

均匀抽稀这块的函数我就省略了，大家自己写吧，不是很困难。抽稀主要是为了方便下一步排序，如果点数过多，存在排序错误的风险。

bool sortPointCloudLoop(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in,
	pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_out) 
{
	// 检查输入点云
	if (cloud_in->empty()) {
		std::cerr << "输入点云为空！" << std::endl;
		return false;
	}

	int n_points = cloud_in->size();
	if (n_points <= 1) {
		*cloud_out = *cloud_in;
		return true;
	}

	// 创建KD树用于近邻搜索
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud_in);

	// 用于存储排序结果的索引
	std::vector<int> sorted_indices;
	sorted_indices.reserve(n_points);

	// 从第一个点开始
	int current_point_idx = 0;
	sorted_indices.push_back(current_point_idx);

	// 用于标记点是否已被访问
	std::vector<bool> visited(n_points, false);
	visited[current_point_idx] = true;

	// 找出所有点的顺序
	for (int i = 0; i < n_points - 1; ++i) {
		std::vector<int> pointIdxNKNSearch(2);
		std::vector<float> pointNKNSquaredDistance(2);

		// 查找当前点的最近邻
		if (kdtree.nearestKSearch(cloud_in->points[current_point_idx], 2, pointIdxNKNSearch, pointNKNSquaredDistance) < 2) {
			std::cerr << "无法找到足够的邻居点！" << std::endl;
			continue;
		}

		// 获取最近的未访问点
		int next_point_idx = -1;

		// 检查第一个最近邻是否已访问
		if (!visited[pointIdxNKNSearch[0]]) {
			next_point_idx = pointIdxNKNSearch[0];
		}
		// 检查第二个最近邻是否已访问
		else if (pointIdxNKNSearch.size() > 1 && !visited[pointIdxNKNSearch[1]]) {
			next_point_idx = pointIdxNKNSearch[1];
		}
		// 如果最近的两个邻居都已访问，查找下一个最近的未访问点
		else {
			float min_dist = std::numeric_limits<float>::max();
			for (int j = 0; j < n_points; ++j) {
				if (!visited[j]) {
					float dist = pcl::squaredEuclideanDistance(cloud_in->points[current_point_idx], cloud_in->points[j]);
					if (dist < min_dist) {
						min_dist = dist;
						next_point_idx = j;
					}
				}
			}
		}

		// 如果找不到下一个点，说明排序已完成
		if (next_point_idx == -1) {
			break;
		}

		// 添加下一个点并标记为已访问
		sorted_indices.push_back(next_point_idx);
		visited[next_point_idx] = true;
		current_point_idx = next_point_idx;
	}

	// 构建排序后的点云
	cloud_out->clear();
	cloud_out->reserve(sorted_indices.size());

	for (int idx : sorted_indices) {
		cloud_out->push_back(cloud_in->points[idx]);
	}

	return true;
}

double generateRandomDouble(double min, double max) 
{
    std::random_device rd; // 随机设备
    std::mt19937 gen(rd()); // 使用Mersenne Twister算法生成随机数
    std::uniform_real_distribution<> dis(min, max); // 定义范围
    return dis(gen); // 生成随机数
}

/// <summary>
/// 判断二维点是否在二维多边形内
/// </summary>
/// <param name="polygon">点云指针表征的多边形区域</param>
/// <param name="point">二维点坐标</param>
/// <returns>包含结果</returns>
bool pointcloud_polygonsContain(const pcl::PointCloud<pcl::PointXYZ>::Ptr& polygon, const pcl::PointXYZ& point)
{
	int n = polygon->size();
	if (n < 3) 
	{
		return false;
	}

	int intersections = 0;
	pcl::PointXYZ p1 = polygon->points[0];
	for (int i = 1; i <= n; ++i) 
	{
		pcl::PointXYZ p2 = polygon->points[i % n];

		if (point.y == p1.y && point.y == p2.y && point.x >= std::min(p1.x, p2.x) && point.x <= std::max(p1.x, p2.x)) 
		{
			return true;
		}

		if (p1.y <= point.y && p2.y > point.y || (p2.y <= point.y && p1.y > point.y)) 
		{
			float vt = (point.y - p1.y) / (p2.y - p1.y);
			if (p1.x + vt * (p2.x - p1.x) < point.x) 
			{
				intersections++;
			}
		}

		p1 = p2;
	}

	return intersections % 2 == 1;
}

最后我想说这个算法效率肯定不是很高，没有经过打磨，但是原理比较简单，大家可以试着对它进行二创，另外以本例来说，在包围盒内洒一万个豆子，能在多边形内的也就五百个，数量上可以根据自己的需求进行更改。