2D无序点集曲线重建(沿曲线重新排序)

算法说明

  1. 初始化

    • 从任意点开始(代码中选择第一个点)

    • 找到它的最近邻作为第二个点

  2. 双向扩展

    • 向后扩展:从队尾的点出发,沿着前一点到当前点的方向,在允许角度偏差范围内寻找最近点

    • 向前扩展:从队首的点出发,沿着第二个点到第一个点的方向,在允许角度偏差范围内寻找最近点

  3. 终止条件

    • 当两个方向都无法找到符合条件的点时,算法终止

  4. 关键参数

    • angleThreshold_:控制搜索方向的角度偏差阈值(弧度)

    • 可以通过构造函数调整这个参数来适应不同曲线

 部分关键代码如下:

#pragma once
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include

typedef glm::dvec3 Point3d;
typedef glm::dvec2 Point2d;
typedef glm::dvec2 Vec2d;
typedef glm::dvec3 Vec3d;

class CurvePointSorter {
public:
	// 构造函数,可设置角度偏差阈值(弧度)
	explicit CurvePointSorter(double angleThreshold = M_PI / 3)
		: angleThreshold_(angleThreshold) {
	}

	// 主接口:排序曲线点集
	std::vector sortPoints(const std::vector& points) {
		if (points.size() < 3) return points; // 点数太少直接返回

		std::vector used(points.size(), false);
		std::deque sortedPoints;

		// 1. 从任意点开始(这里选择第一个点)
		size_t firstIdx = 0;
		sortedPoints.push_back(points[firstIdx]);
		used[firstIdx] = true;

		// 2. 找到最近邻作为第二个点
		auto secondIdx = findClosestPoint(points, firstIdx, used);
		if (secondIdx == -1) return points; // 没有找到有效点
		sortedPoints.push_back(points[secondIdx]);
		used[secondIdx] = true;

		// 3. 双向扩展
		bool canExtend = true;
		while (canExtend) {
			canExtend = false;

						// 向后扩展(队尾方向)
			if (sortedPoints.size() > 1) {
				const Point2d& last = sortedPoints.back();
				const Point2d& prev = *(sortedPoints.rbegin() + 1);
				Vec2d dir = last - prev; // 前一点指向当前点的方向

				auto nextIdx = findDirectionalClosest(points, last, dir, used);
				if (nextIdx != -1) {
					sortedPoints.push_back(points[nextIdx]);
					used[nextIdx] = true;
					canExtend = true;
				}
			}

			// 向前扩展(队首方向)
			if (sortedPoints.size() > 1) {
				const Point2d& first = sortedPoints.front();
				const Point2d& second = *(sortedPoints.begin() + 1);
				Vec2d dir = first - second; // 第二点指向第一点的方向

				auto prevIdx = findDirectionalClosest(points, first, dir, used);
				if (prevIdx != -1) {
					sortedPoints.push_front(points[prevIdx]);
					used[prevIdx] = true;
					canExtend = true;
				}
			}
		}

		// 将deque转换为vector返回
		return std::vector(sortedPoints.begin(), sortedPoints.end());
	}

private:
	double angleThreshold_; // 允许的角度偏差(弧度)

		// 找到距离给定点最近的点
	int findClosestPoint(const std::vector& points,
		size_t refIdx,
		const std::vector& used) {
		double minDist = std::numeric_limits::max();
		int closestIdx = -1;

		for (size_t i = 0; i < points.size(); ++i) {
			if (!used[i] && i != refIdx) {
				double dist = glm::distance(points[refIdx], points[i]);
				if (dist < minDist) {
					minDist = dist;
					closestIdx = i;
				}
			}
		}

		return closestIdx;
	}

	// 在给定方向附近找到最近的点
	int findDirectionalClosest(const std::vector& points,
		const Point2d& refPoint,
		const Vec2d& direction,
		const std::vector& used) {
		double minDist = std::numeric_limits::max();
		int closestIdx = -1;

		// 归一化方向向量
		Vec2d normDir = glm::normalize(direction); // { direction.x / dirLen, direction.y / dirLen };

				for (size_t i = 0; i < points.size(); ++i) {
			if (!used[i]) {
				Vec2d currDir = points[i] - refPoint;
				double currDirLen = std::sqrt(currDir.x * currDir.x + currDir.y * currDir.y);

				if (currDirLen > 0) {
					Vec2d normCurrDir = { currDir.x / currDirLen, currDir.y / currDirLen };

					// 计算两个方向向量的夹角(通过点积)
					double cosAngle = normDir.x * normCurrDir.x + normDir.y * normCurrDir.y;
					double angle = std::acos(std::min(1.0, std::max(-1.0, cosAngle)));

					// 检查角度是否在阈值范围内
					if (angle <= angleThreshold_) {
						double dist = glm::distance(refPoint,points[i]);
						if (dist < minDist) {
							minDist = dist;
							closestIdx = i;
						}
					}
				}
			}
		}

		return closestIdx;
	}
};

你可能感兴趣的:(C++,点云,算法,c++)