1. 项目概述为什么需要最远点采样在点云处理、计算机图形学以及三维视觉的很多任务里我们常常会面对一个棘手的问题数据量太大。一个激光雷达扫描的原始点云动辄几十万甚至上百万个点。直接在这些海量点上进行特征提取、分类或者重建计算开销是难以承受的。这就好比你要给一幅超高清的巨幅画作做分析与其盯着每一个像素看不如先选取一些有代表性的“关键像素”来把握整体结构和轮廓。Furthest Point Sampling中文常译为“最远点采样”或“最远点选择”就是解决这个“选取代表性点”问题的经典且高效的算法。它的核心思想非常直观不是随机地、也不是简单地均匀网格化下采样而是迭代地选择距离已选点集最远的点。这样做的目的是用尽可能少的点去最大程度地覆盖原始点云的空间分布保留其几何形状的骨架和边界信息。想象一下你要在一个城市里设立几个消防站目标是让任何一个地方发生火灾时消防车都能尽快到达。你不会把所有消防站都挤在市中心而是会优先把第一个站设在市中心一个初始点然后第二个站设在离市中心最远的郊区第三个站设在离前两个站整体最远的区域以此类推。FPS算法干的就是这个“选址”的活儿只不过场景换成了三维空间中的点。这个算法在PointNet等开创性的点云深度学习网络中扮演了关键角色用于构建层次化特征提取的“感受野”。在点云配准、三维重建、模型简化等领域它也是预处理阶段不可或缺的一步。今天我们就来彻底拆解它的原理并用C从头实现一个清晰、高效的版本。你会发现它背后的数据结构选择比算法逻辑本身更值得玩味。2. 核心原理与算法流程拆解FPS的原理可以用一句话概括贪心地、迭代地选取距离当前已选点集最远的点。但“距离已选点集”如何定义是整个集合吗这里就引出了算法的关键细节。2.1 算法步骤详解假设我们有一个包含N个点的点云我们需要从中采样M个点M N。初始化随机选择一个种子点作为第一个采样点将其加入采样结果集合S。创建一个长度为N的数组distances用来记录原始点云中每个点到当前采样点集S的最近距离。初始化时S中只有第一个点所以distances[i]就是点i到第一个种子点的欧氏距离。迭代采样重复 M-1 次在每一轮迭代中我们的目标是找到那个“最远点”。查找遍历distances数组找到值最大的那个索引。这个索引对应的原始点就是距离当前已选点集S最远的点。因为distances存储的是每个点到S的最近距离其中的最大值就意味着该点是所有未选点中离已选点集“最近距离”最远的即整体最远的点。更新将这个最远点加入采样结果集合S。维护这是算法的核心步骤。由于我们新加入了一个点p_new到集合S那么对于原始点云中的每一个点i它到集合S的最近距离有可能变小因为可能离新加入的点p_new更近。因此我们需要更新distances数组distances[i] min(distances[i], distance(point_i, p_new))这一步确保了distances数组始终维护着每个点到当前采样集S的最近距离。终止当采样结果集合S的大小达到 M 时算法停止。S中的点就是FPS采样结果。2.2 算法复杂度与核心挑战从步骤上我们不难分析出算法的复杂度我们需要进行 M-1 轮迭代。每一轮迭代中我们需要一次O(N)的查找找distances最大值和一次O(N)的更新用新点距离更新所有点的distances。因此总的时间复杂度是O(M * N)。这里就出现了第一个性能瓶颈当需要采样的点数 M 较大或者原始点云 N 很大时这个O(M*N)的复杂度会变得非常可观。例如从10万个点中采1万个点就是百亿次距离计算和比较。更关键的是在每一轮的更新操作中我们需要计算新采样点p_new到所有其他 N 个点的欧氏距离。距离计算本身涉及平方和开方是一个相对耗时的操作。如何高效地管理和更新这个distances数组并快速找到其中的最大值是优化实现的关键。注意很多初学者会误解认为“最远”是到已选点集合的“质心”或“平均位置”最远。这是错误的。FPS中的“距离”是指到集合中最近那个点的距离。这保证了采样点会向空间的边界和空隙扩散而不是围绕一个中心。3. C实现从朴素版本到优化我们将采用自底向上的方式先实现一个最直观的版本再分析其性能痛点并引入优化。3.1 数据结构与基础函数首先我们定义点的基础结构并准备好距离计算函数。#include vector #include cmath #include limits #include random #include algorithm // 定义三维点结构体 struct Point3D { float x, y, z; Point3D(float x_ 0, float y_ 0, float z_ 0) : x(x_), y(y_), z(z_) {} }; // 计算两点间欧氏距离的平方避免开方用于比较 inline float squaredDistance(const Point3D p1, const Point3D p2) { float dx p1.x - p2.x; float dy p1.y - p2.y; float dz p1.z - p2.z; return dx * dx dy * dy dz * dz; } // 计算两点间欧氏距离 inline float distance(const Point3D p1, const Point3D p2) { return std::sqrt(squaredDistance(p1, p2)); }这里有一个重要的优化技巧在比较距离远近时我们完全可以使用距离的平方squaredDistance因为平方运算单调递增不影响大小关系。这省去了大量耗时的std::sqrt操作。只有在需要真实距离值时才调用distance。3.2 朴素实现Brute-Force我们先按照算法描述实现一个最直接的版本。std::vectorint farthestPointSamplingBruteForce( const std::vectorPoint3D points, int num_samples) { int n points.size(); if (num_samples 0 || num_samples n) { return std::vectorint(); } std::vectorint sampled_indices; sampled_indices.reserve(num_samples); // 步骤1: 随机选择第一个点 std::random_device rd; std::mt19937 gen(rd()); std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen); sampled_indices.push_back(first_idx); // 初始化距离数组记录每个点到已选点集的最近距离 std::vectorfloat min_distances(n, std::numeric_limitsfloat::max()); // 用第一个点初始化距离数组 for (int i 0; i n; i) { min_distances[i] squaredDistance(points[i], points[first_idx]); } // 步骤2: 迭代采样剩余点 for (int count 1; count num_samples; count) { // 2.1 查找最远点min_distances中最大值对应的索引 int farthest_idx -1; float max_dist -1.0f; for (int i 0; i n; i) { // 注意这里可以跳过已选中的点但我们的min_distances[已选点]0不影响查找最大值 if (min_distances[i] max_dist) { max_dist min_distances[i]; farthest_idx i; } } if (farthest_idx -1) break; // 理论上不会发生 sampled_indices.push_back(farthest_idx); // 2.2 用新选中的点更新所有点的最近距离 const Point3D new_point points[farthest_idx]; for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_point); if (new_dist min_distances[i]) { min_distances[i] new_dist; } } } return sampled_indices; }这个版本完全遵循了算法流程逻辑清晰。但它有两个明显的性能问题查找最大值是线性扫描O(N)每次都需要遍历整个数组。更新距离是线性扫描O(N)每次都需要计算新点到所有点的距离并比较更新。对于大规模点云这将成为瓶颈。接下来我们针对这两个问题进行优化。3.3 优化实现使用优先队列堆观察发现我们每一轮的核心操作是查找获取min_distances中的最大值。更新修改min_distances中部分元素的值变小。这正是一个典型的“动态更新数据并快速获取最大值”的场景。理想的数据结构是最大堆优先队列。但是标准库的std::priority_queue不支持随机访问和修改已有元素的值。当某个点的min_distance变小时我们需要在堆中更新它的位置。解决方案是使用一个可以“降低键值decrease-key”的堆。我们可以自己实现一个或者使用std::set/std::multiset来模拟。这里我们采用一种在实践中更常见、编码更简单的思路惰性删除法。我们维护一个最大堆堆中的元素是(distance, index)对。当我们更新一个点的距离时我们不直接修改堆中旧的值这很困难而是将新的 (distance, index) 对直接插入堆中。这样堆中对于同一个索引i可能存在多个不同距离的条目。当我们从堆顶弹出最大值时需要检查这个条目中的距离是否与该点当前在min_distances数组中的最新值一致。如果不一致说明这是一个“过时”的条目直接丢弃继续弹出下一个。直到弹出一个“有效”的条目该点即为当前最远点。这种方法避免了复杂的堆内更新操作虽然堆会变大最多包含N M*N个条目但实际远小于此但每次插入和弹出都是O(log N)且常数因子很小。#include queue // for priority_queue #include functional // for greater std::vectorint farthestPointSamplingOptimized( const std::vectorPoint3D points, int num_samples) { int n points.size(); if (num_samples 0 || num_samples n) { return std::vectorint(); } std::vectorint sampled_indices; sampled_indices.reserve(num_samples); // 随机种子 std::random_device rd; std::mt19937 gen(rd()); std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen); sampled_indices.push_back(first_idx); // 初始化距离数组 std::vectorfloat min_distances(n, std::numeric_limitsfloat::max()); for (int i 0; i n; i) { min_distances[i] squaredDistance(points[i], points[first_idx]); } // 定义优先队列的元素类型pair距离, 索引使用最大堆 // 注意默认是最大堆按pair的第一个元素距离比较 using DistIndexPair std::pairfloat, int; std::priority_queueDistIndexPair pq; // 初始化堆将所有点插入 for (int i 0; i n; i) { pq.push(std::make_pair(min_distances[i], i)); } // 迭代采样 while (sampled_indices.size() static_castsize_t(num_samples)) { // 使用惰性删除弹出堆顶元素直到找到一个有效的距离值与当前记录一致 DistIndexPair top; do { if (pq.empty()) { // 理论上不会发生除非所有点都已选中或距离为0 return sampled_indices; } top pq.top(); pq.pop(); } while (std::abs(top.first - min_distances[top.second]) 1e-7f); // 浮点数比较容差 int farthest_idx top.second; // 检查是否重复理论上不会因为选中后其min_distance为0不会再被弹出 sampled_indices.push_back(farthest_idx); // 用新点更新距离 const Point3D new_point points[farthest_idx]; for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_point); if (new_dist min_distances[i]) { min_distances[i] new_dist; // 关键优化将更新后的新距离插入堆而不是修改旧条目 pq.push(std::make_pair(new_dist, i)); } } } return sampled_indices; }优化点解析查找最远点从朴素的O(N)线性扫描优化为O(log N)的堆顶弹出尽管可能有多次弹出但均摊复杂度优秀。更新操作虽然更新min_distances数组仍需O(N)循环但省去了与自身比较的if判断因为我们已经知道新点直接计算新距离即可。更重要的是它将对堆的更新从“修改”变成了“插入”实现了O(log N)的更新登记。惰性删除这是本实现的核心技巧。它巧妙地规避了优先队列无法直接修改元素的限制通过空间换时间并且代码简洁可靠。实操心得在实现惰性删除时浮点数的比较需要特别注意。由于计算精度问题两个理论上相等的浮点数可能并不完全相等。因此我们使用一个极小的容差如1e-7f来判断它们是否“相等”。这是工业级代码中处理浮点数比较的常见做法。3.4 进一步优化距离计算的向量化与提前终止对于超大规模点云即使使用了堆优化O(M*N)的循环体更新距离的双重循环仍然是主要开销。我们可以考虑以下优化SIMD向量化在计算squaredDistance时可以使用编译器自动向量化确保使用-O3 -marchnative编译选项或者显式使用SSE/AVX指令集来同时计算多个点的距离。这对于性能有极致要求的场景是必要的。提前终止在更新min_distances[i]时如果new_dist已经大于等于当前min_distances[i]可以跳过该点后续的计算吗不能因为我们需要计算完整的new_dist才能比较。但是如果我们的点云具有某种空间结构如分布在网格上可能会有更优的算法。不过对于通用的FPS这是不可避免的。并行化更新min_distances的循环是独立的非常适合用OpenMP进行多线程并行。// 使用OpenMP并行化更新循环的示例代码片段 #include omp.h // ... 在更新距离的循环前 ... const Point3D new_point points[farthest_idx]; #pragma omp parallel for for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_point); // 注意这里需要对min_distances[i]的写操作加锁或者使用归约 // 实际上因为每个i是独立的且是赋值操作我们可以使用‘critical’或‘atomic’。 // 但更高效的是先计算再在临界区更新。 if (new_dist min_distances[i]) { #pragma omp critical { // 再次检查防止竞态条件 if (new_dist min_distances[i]) { min_distances[i] new_dist; // 堆的插入操作也需要线程安全可以将待插入元素暂存最后统一插入 // 这里简化处理实际并行化需要更细致的设计 } } } }注意并行化FPS的更新循环需要谨慎处理数据竞争。min_distances数组的更新和pq的插入都不是原子的。一个更稳妥的并行策略是每个线程负责一个点集块计算该块内点到新采样点的距离并记录本线程块内需要更新的信息。所有线程计算完毕后再在一个串行区域统一更新min_distances和插入堆。这属于更高级的优化范畴。4. 算法验证与结果分析实现完成后我们需要验证算法的正确性并直观感受采样效果。4.1 验证方法基础功能验证输入一个简单的小点集如立方体的8个顶点指定采样数M4。观察采样点是否倾向于选取立方体的对角顶点以覆盖最大空间。检查采样点是否不重复。比较朴素版本和优化版本的输出结果是否一致索引顺序可能因距离相等而不同但集合应相同。可视化验证推荐使用诸如PCL (Point Cloud Library)、Open3D等库或者将结果导出为PLY文件用MeshLab、CloudCompare等软件查看。生成一个规则或随机点云应用FPS采样观察采样点是否均匀地散布在原始点云的整个空间范围内而不是聚集在某处。4.2 性能对比实验我们可以设计一个简单的性能测试比较不同实现版本在相同输入下的耗时。#include chrono // ... 其他头文件 ... int main() { // 生成测试点云一个随机点云 std::vectorPoint3D points; int num_points 50000; std::mt19937 rng(42); // 固定种子保证可重复性 std::uniform_real_distributionfloat dist(-10.0f, 10.0f); for (int i 0; i num_points; i) { points.emplace_back(dist(rng), dist(rng), dist(rng)); } int num_samples 5000; // 测试优化版本 auto start std::chrono::high_resolution_clock::now(); auto sampled_indices_opt farthestPointSamplingOptimized(points, num_samples); auto end std::chrono::high_resolution_clock::now(); auto duration_opt std::chrono::duration_caststd::chrono::milliseconds(end - start); std::cout Optimized FPS took duration_opt.count() ms. std::endl; // 测试朴素版本对于大规模点云可能非常慢慎用 // auto start_bf std::chrono::high_resolution_clock::now(); // auto sampled_indices_bf farthestPointSamplingBruteForce(points, num_samples); // auto end_bf std::chrono::high_resolution_clock::now(); // auto duration_bf std::chrono::duration_caststd::chrono::milliseconds(end_bf - start_bf); // std::cout Brute-Force FPS took duration_bf.count() ms. std::endl; std::cout Sampled sampled_indices_opt.size() points. std::endl; return 0; }在我的测试环境中5万点采5千点优化版本通常在几百毫秒到一秒内完成而朴素版本可能需要数十秒甚至分钟级。这个差距随着数据规模增大会急剧拉大。4.3 采样效果分析FPS采样的一个显著特点是其空间覆盖的均匀性。下图定性地展示了其与随机采样、均匀网格下采样的区别 此处用文字描述实际博文可配图随机采样点可能聚集留下大片未覆盖区域。均匀网格采样受限于网格对齐可能丢失细小特征对旋转敏感。FPS采样点与点之间倾向于保持较远距离能更好地捕捉模型的轮廓和 extremities末端对点云的旋转和平移具有不变性。5. 常见问题、陷阱与高级话题5.1 为什么我的FPS结果每次运行不一样这是正常的因为FPS算法的第一个点是随机选择的。第一个种子点的不同会导致后续迭代的“最远点”序列完全不同。这就像消防站选址第一个站设在城东还是城西会完全改变后续站点的布局。如果你需要确定性的结果可以将随机种子固定。// 固定随机种子以获得可重复结果 std::mt19937 gen(12345); // 使用固定种子 std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen);5.2 距离度量可以改变吗当然可以。我们一直使用的是欧氏距离L2范数这是最常见的选择。但你完全可以根据应用场景使用其他距离度量例如曼哈顿距离L1范数|x1-x2| |y1-y2| |z1-z2|切比雪夫距离L∞范数max(|x1-x2|, |y1-y2|, |z1-z2|)特征空间距离如果点除了坐标还有颜色、法向量等特征你可以定义一种结合了几何和特征信息的复合距离。这在一些特定任务中非常有用。只需要修改squaredDistance和distance函数即可。但要注意使用非欧氏距离时“距离平方”的概念可能不存在或不方便需要调整代码逻辑。5.3 算法复杂度能进一步优化吗O(M*N)是下限吗对于通用的、无结构的高维点集FPS的O(M*N)复杂度很难有理论上的突破因为每一轮你确实需要检查所有点来更新距离。但是对于**低维空间如2D3D**的点云我们可以利用空间数据结构进行加速使用KD-Tree或Octree在初始化时构建一个覆盖所有点的KD-Tree。在每一轮中要找到距离当前点集最远的点可以转化为一个“在所有点中找到距离给定点集可能是多个点最近距离最大的点”的问题。这可以通过在KD-Tree上进行一种修改的最近邻搜索来近似或精确求解。更新操作也可以利用树结构进行区域查询避免遍历所有点。一些研究论文和库如FLANN, PCL实现了基于树的近似FPS能显著提升速度尤其是当N很大时。5.4 内存占用优化我们的优化版本使用了O(N)的min_distances数组和一个最大堆。堆在最坏情况下可能包含O(N M*N)个条目但实际中由于很多点的距离不会被更新或者更新次数有限内存占用是可接受的。如果内存极其紧张可以回到朴素版本它只使用O(N)的额外空间min_distances数组。5.5 在深度学习框架中的实现在PyTorch或TensorFlow中FPS通常作为神经网络的一个层来实现。其实现会充分利用GPU的并行计算能力。核心思路是将双重循环转化为矩阵运算。例如计算一个新点到所有点的距离是一个(1, 3)和(N, 3)张量的广播减法、平方和运算非常高效。查找最大值可以使用argmax函数。虽然更新操作仍需一个O(N)的赋值但整个流程可以利用GPU的并行性获得远超CPU串行实现的速率。这也是为什么在PointNet等模型中FPS层虽然计算量大但依然可行的原因。6. 完整可编译示例代码以下是一个整合了优化版本、简单测试和性能计时的完整示例。#include iostream #include vector #include cmath #include limits #include random #include queue #include chrono #include cassert struct Point3D { float x, y, z; Point3D(float x_ 0, float y_ 0, float z_ 0) : x(x_), y(y_), z(z_) {} }; inline float squaredDistance(const Point3D p1, const Point3D p2) { float dx p1.x - p2.x; float dy p1.y - p2.y; float dz p1.z - p2.z; return dx * dx dy * dy dz * dz; } std::vectorint farthestPointSampling(const std::vectorPoint3D points, int num_samples, int seed 42) { int n static_castint(points.size()); if (n 0 || num_samples 0) return {}; num_samples std::min(num_samples, n); std::vectorint indices; indices.reserve(num_samples); // 固定随机种子用于可重复性生产环境可用随机设备 std::mt19937 gen(seed); std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen); indices.push_back(first_idx); std::vectorfloat min_dists(n, std::numeric_limitsfloat::max()); for (int i 0; i n; i) { min_dists[i] squaredDistance(points[i], points[first_idx]); } using Pair std::pairfloat, int; std::priority_queuePair pq; for (int i 0; i n; i) { pq.push({min_dists[i], i}); } while (indices.size() static_castsize_t(num_samples)) { Pair top; do { // 如果堆空理论上不会则中断 if (pq.empty()) { std::cerr Warning: Priority queue emptied early. Sampling may be incomplete.\n; return indices; } top pq.top(); pq.pop(); } while (std::abs(top.first - min_dists[top.second]) 1e-7f); int next_idx top.second; indices.push_back(next_idx); const Point3D new_pt points[next_idx]; // 更新距离并插入新条目到堆 for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_pt); if (new_dist min_dists[i]) { min_dists[i] new_dist; pq.push({new_dist, i}); } } } return indices; } int main() { // 1. 生成测试数据一个简单立方体 内部一些随机点 std::vectorPoint3D cloud; cloud.emplace_back(0, 0, 0); cloud.emplace_back(1, 0, 0); cloud.emplace_back(0, 1, 0); cloud.emplace_back(1, 1, 0); cloud.emplace_back(0, 0, 1); cloud.emplace_back(1, 0, 1); cloud.emplace_back(0, 1, 1); cloud.emplace_back(1, 1, 1); // 立方体8个顶点 std::mt19937 rng(123); std::uniform_real_distributionfloat urd(0.2f, 0.8f); for (int i 0; i 100; i) { cloud.emplace_back(urd(rng), urd(rng), urd(rng)); // 内部100个随机点 } int num_samples 10; // 2. 执行FPS并计时 auto start std::chrono::high_resolution_clock::now(); std::vectorint sampled farthestPointSampling(cloud, num_samples); auto end std::chrono::high_resolution_clock::now(); auto duration std::chrono::duration_caststd::chrono::microseconds(end - start); // 3. 输出结果 std::cout Total points: cloud.size() std::endl; std::cout Sampled sampled.size() points in duration.count() us. std::endl; std::cout Sampled indices: ; for (int idx : sampled) { std::cout idx ; } std::cout std::endl; // 4. 简单验证采样点应不重复 std::sort(sampled.begin(), sampled.end()); auto last std::unique(sampled.begin(), sampled.end()); assert(last sampled.end() Sampled indices contain duplicates!); // 5. 可视化建议 std::cout \nTo visualize, you can output the original and sampled points to a file.\n; std::cout For example, save as a PLY file and open it with MeshLab.\n; // 这里可以添加简单的PLY文件输出代码可选 return 0; }将这段代码保存为fps_demo.cpp使用支持C11及以上版本的编译器编译即可运行。g -stdc11 -O3 fps_demo.cpp -o fps_demo ./fps_demo这个实现平衡了清晰度、正确性和性能是理解和应用FPS算法的良好起点。在实际项目中如果点云规模巨大超过百万你可能需要结合空间索引树KD-Tree, Octree来进一步加速或者直接调用PCL、Open3D等成熟库中经过深度优化的FPS函数。但无论如何理解其背后的原理和优化思路是灵活运用和调试的基石。
1. 项目概述为什么需要最远点采样在点云处理、计算机图形学以及三维视觉的很多任务里我们常常会面对一个棘手的问题数据量太大。一个激光雷达扫描的原始点云动辄几十万甚至上百万个点。直接在这些海量点上进行特征提取、分类或者重建计算开销是难以承受的。这就好比你要给一幅超高清的巨幅画作做分析与其盯着每一个像素看不如先选取一些有代表性的“关键像素”来把握整体结构和轮廓。Furthest Point Sampling中文常译为“最远点采样”或“最远点选择”就是解决这个“选取代表性点”问题的经典且高效的算法。它的核心思想非常直观不是随机地、也不是简单地均匀网格化下采样而是迭代地选择距离已选点集最远的点。这样做的目的是用尽可能少的点去最大程度地覆盖原始点云的空间分布保留其几何形状的骨架和边界信息。想象一下你要在一个城市里设立几个消防站目标是让任何一个地方发生火灾时消防车都能尽快到达。你不会把所有消防站都挤在市中心而是会优先把第一个站设在市中心一个初始点然后第二个站设在离市中心最远的郊区第三个站设在离前两个站整体最远的区域以此类推。FPS算法干的就是这个“选址”的活儿只不过场景换成了三维空间中的点。这个算法在PointNet等开创性的点云深度学习网络中扮演了关键角色用于构建层次化特征提取的“感受野”。在点云配准、三维重建、模型简化等领域它也是预处理阶段不可或缺的一步。今天我们就来彻底拆解它的原理并用C从头实现一个清晰、高效的版本。你会发现它背后的数据结构选择比算法逻辑本身更值得玩味。2. 核心原理与算法流程拆解FPS的原理可以用一句话概括贪心地、迭代地选取距离当前已选点集最远的点。但“距离已选点集”如何定义是整个集合吗这里就引出了算法的关键细节。2.1 算法步骤详解假设我们有一个包含N个点的点云我们需要从中采样M个点M N。初始化随机选择一个种子点作为第一个采样点将其加入采样结果集合S。创建一个长度为N的数组distances用来记录原始点云中每个点到当前采样点集S的最近距离。初始化时S中只有第一个点所以distances[i]就是点i到第一个种子点的欧氏距离。迭代采样重复 M-1 次在每一轮迭代中我们的目标是找到那个“最远点”。查找遍历distances数组找到值最大的那个索引。这个索引对应的原始点就是距离当前已选点集S最远的点。因为distances存储的是每个点到S的最近距离其中的最大值就意味着该点是所有未选点中离已选点集“最近距离”最远的即整体最远的点。更新将这个最远点加入采样结果集合S。维护这是算法的核心步骤。由于我们新加入了一个点p_new到集合S那么对于原始点云中的每一个点i它到集合S的最近距离有可能变小因为可能离新加入的点p_new更近。因此我们需要更新distances数组distances[i] min(distances[i], distance(point_i, p_new))这一步确保了distances数组始终维护着每个点到当前采样集S的最近距离。终止当采样结果集合S的大小达到 M 时算法停止。S中的点就是FPS采样结果。2.2 算法复杂度与核心挑战从步骤上我们不难分析出算法的复杂度我们需要进行 M-1 轮迭代。每一轮迭代中我们需要一次O(N)的查找找distances最大值和一次O(N)的更新用新点距离更新所有点的distances。因此总的时间复杂度是O(M * N)。这里就出现了第一个性能瓶颈当需要采样的点数 M 较大或者原始点云 N 很大时这个O(M*N)的复杂度会变得非常可观。例如从10万个点中采1万个点就是百亿次距离计算和比较。更关键的是在每一轮的更新操作中我们需要计算新采样点p_new到所有其他 N 个点的欧氏距离。距离计算本身涉及平方和开方是一个相对耗时的操作。如何高效地管理和更新这个distances数组并快速找到其中的最大值是优化实现的关键。注意很多初学者会误解认为“最远”是到已选点集合的“质心”或“平均位置”最远。这是错误的。FPS中的“距离”是指到集合中最近那个点的距离。这保证了采样点会向空间的边界和空隙扩散而不是围绕一个中心。3. C实现从朴素版本到优化我们将采用自底向上的方式先实现一个最直观的版本再分析其性能痛点并引入优化。3.1 数据结构与基础函数首先我们定义点的基础结构并准备好距离计算函数。#include vector #include cmath #include limits #include random #include algorithm // 定义三维点结构体 struct Point3D { float x, y, z; Point3D(float x_ 0, float y_ 0, float z_ 0) : x(x_), y(y_), z(z_) {} }; // 计算两点间欧氏距离的平方避免开方用于比较 inline float squaredDistance(const Point3D p1, const Point3D p2) { float dx p1.x - p2.x; float dy p1.y - p2.y; float dz p1.z - p2.z; return dx * dx dy * dy dz * dz; } // 计算两点间欧氏距离 inline float distance(const Point3D p1, const Point3D p2) { return std::sqrt(squaredDistance(p1, p2)); }这里有一个重要的优化技巧在比较距离远近时我们完全可以使用距离的平方squaredDistance因为平方运算单调递增不影响大小关系。这省去了大量耗时的std::sqrt操作。只有在需要真实距离值时才调用distance。3.2 朴素实现Brute-Force我们先按照算法描述实现一个最直接的版本。std::vectorint farthestPointSamplingBruteForce( const std::vectorPoint3D points, int num_samples) { int n points.size(); if (num_samples 0 || num_samples n) { return std::vectorint(); } std::vectorint sampled_indices; sampled_indices.reserve(num_samples); // 步骤1: 随机选择第一个点 std::random_device rd; std::mt19937 gen(rd()); std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen); sampled_indices.push_back(first_idx); // 初始化距离数组记录每个点到已选点集的最近距离 std::vectorfloat min_distances(n, std::numeric_limitsfloat::max()); // 用第一个点初始化距离数组 for (int i 0; i n; i) { min_distances[i] squaredDistance(points[i], points[first_idx]); } // 步骤2: 迭代采样剩余点 for (int count 1; count num_samples; count) { // 2.1 查找最远点min_distances中最大值对应的索引 int farthest_idx -1; float max_dist -1.0f; for (int i 0; i n; i) { // 注意这里可以跳过已选中的点但我们的min_distances[已选点]0不影响查找最大值 if (min_distances[i] max_dist) { max_dist min_distances[i]; farthest_idx i; } } if (farthest_idx -1) break; // 理论上不会发生 sampled_indices.push_back(farthest_idx); // 2.2 用新选中的点更新所有点的最近距离 const Point3D new_point points[farthest_idx]; for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_point); if (new_dist min_distances[i]) { min_distances[i] new_dist; } } } return sampled_indices; }这个版本完全遵循了算法流程逻辑清晰。但它有两个明显的性能问题查找最大值是线性扫描O(N)每次都需要遍历整个数组。更新距离是线性扫描O(N)每次都需要计算新点到所有点的距离并比较更新。对于大规模点云这将成为瓶颈。接下来我们针对这两个问题进行优化。3.3 优化实现使用优先队列堆观察发现我们每一轮的核心操作是查找获取min_distances中的最大值。更新修改min_distances中部分元素的值变小。这正是一个典型的“动态更新数据并快速获取最大值”的场景。理想的数据结构是最大堆优先队列。但是标准库的std::priority_queue不支持随机访问和修改已有元素的值。当某个点的min_distance变小时我们需要在堆中更新它的位置。解决方案是使用一个可以“降低键值decrease-key”的堆。我们可以自己实现一个或者使用std::set/std::multiset来模拟。这里我们采用一种在实践中更常见、编码更简单的思路惰性删除法。我们维护一个最大堆堆中的元素是(distance, index)对。当我们更新一个点的距离时我们不直接修改堆中旧的值这很困难而是将新的 (distance, index) 对直接插入堆中。这样堆中对于同一个索引i可能存在多个不同距离的条目。当我们从堆顶弹出最大值时需要检查这个条目中的距离是否与该点当前在min_distances数组中的最新值一致。如果不一致说明这是一个“过时”的条目直接丢弃继续弹出下一个。直到弹出一个“有效”的条目该点即为当前最远点。这种方法避免了复杂的堆内更新操作虽然堆会变大最多包含N M*N个条目但实际远小于此但每次插入和弹出都是O(log N)且常数因子很小。#include queue // for priority_queue #include functional // for greater std::vectorint farthestPointSamplingOptimized( const std::vectorPoint3D points, int num_samples) { int n points.size(); if (num_samples 0 || num_samples n) { return std::vectorint(); } std::vectorint sampled_indices; sampled_indices.reserve(num_samples); // 随机种子 std::random_device rd; std::mt19937 gen(rd()); std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen); sampled_indices.push_back(first_idx); // 初始化距离数组 std::vectorfloat min_distances(n, std::numeric_limitsfloat::max()); for (int i 0; i n; i) { min_distances[i] squaredDistance(points[i], points[first_idx]); } // 定义优先队列的元素类型pair距离, 索引使用最大堆 // 注意默认是最大堆按pair的第一个元素距离比较 using DistIndexPair std::pairfloat, int; std::priority_queueDistIndexPair pq; // 初始化堆将所有点插入 for (int i 0; i n; i) { pq.push(std::make_pair(min_distances[i], i)); } // 迭代采样 while (sampled_indices.size() static_castsize_t(num_samples)) { // 使用惰性删除弹出堆顶元素直到找到一个有效的距离值与当前记录一致 DistIndexPair top; do { if (pq.empty()) { // 理论上不会发生除非所有点都已选中或距离为0 return sampled_indices; } top pq.top(); pq.pop(); } while (std::abs(top.first - min_distances[top.second]) 1e-7f); // 浮点数比较容差 int farthest_idx top.second; // 检查是否重复理论上不会因为选中后其min_distance为0不会再被弹出 sampled_indices.push_back(farthest_idx); // 用新点更新距离 const Point3D new_point points[farthest_idx]; for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_point); if (new_dist min_distances[i]) { min_distances[i] new_dist; // 关键优化将更新后的新距离插入堆而不是修改旧条目 pq.push(std::make_pair(new_dist, i)); } } } return sampled_indices; }优化点解析查找最远点从朴素的O(N)线性扫描优化为O(log N)的堆顶弹出尽管可能有多次弹出但均摊复杂度优秀。更新操作虽然更新min_distances数组仍需O(N)循环但省去了与自身比较的if判断因为我们已经知道新点直接计算新距离即可。更重要的是它将对堆的更新从“修改”变成了“插入”实现了O(log N)的更新登记。惰性删除这是本实现的核心技巧。它巧妙地规避了优先队列无法直接修改元素的限制通过空间换时间并且代码简洁可靠。实操心得在实现惰性删除时浮点数的比较需要特别注意。由于计算精度问题两个理论上相等的浮点数可能并不完全相等。因此我们使用一个极小的容差如1e-7f来判断它们是否“相等”。这是工业级代码中处理浮点数比较的常见做法。3.4 进一步优化距离计算的向量化与提前终止对于超大规模点云即使使用了堆优化O(M*N)的循环体更新距离的双重循环仍然是主要开销。我们可以考虑以下优化SIMD向量化在计算squaredDistance时可以使用编译器自动向量化确保使用-O3 -marchnative编译选项或者显式使用SSE/AVX指令集来同时计算多个点的距离。这对于性能有极致要求的场景是必要的。提前终止在更新min_distances[i]时如果new_dist已经大于等于当前min_distances[i]可以跳过该点后续的计算吗不能因为我们需要计算完整的new_dist才能比较。但是如果我们的点云具有某种空间结构如分布在网格上可能会有更优的算法。不过对于通用的FPS这是不可避免的。并行化更新min_distances的循环是独立的非常适合用OpenMP进行多线程并行。// 使用OpenMP并行化更新循环的示例代码片段 #include omp.h // ... 在更新距离的循环前 ... const Point3D new_point points[farthest_idx]; #pragma omp parallel for for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_point); // 注意这里需要对min_distances[i]的写操作加锁或者使用归约 // 实际上因为每个i是独立的且是赋值操作我们可以使用‘critical’或‘atomic’。 // 但更高效的是先计算再在临界区更新。 if (new_dist min_distances[i]) { #pragma omp critical { // 再次检查防止竞态条件 if (new_dist min_distances[i]) { min_distances[i] new_dist; // 堆的插入操作也需要线程安全可以将待插入元素暂存最后统一插入 // 这里简化处理实际并行化需要更细致的设计 } } } }注意并行化FPS的更新循环需要谨慎处理数据竞争。min_distances数组的更新和pq的插入都不是原子的。一个更稳妥的并行策略是每个线程负责一个点集块计算该块内点到新采样点的距离并记录本线程块内需要更新的信息。所有线程计算完毕后再在一个串行区域统一更新min_distances和插入堆。这属于更高级的优化范畴。4. 算法验证与结果分析实现完成后我们需要验证算法的正确性并直观感受采样效果。4.1 验证方法基础功能验证输入一个简单的小点集如立方体的8个顶点指定采样数M4。观察采样点是否倾向于选取立方体的对角顶点以覆盖最大空间。检查采样点是否不重复。比较朴素版本和优化版本的输出结果是否一致索引顺序可能因距离相等而不同但集合应相同。可视化验证推荐使用诸如PCL (Point Cloud Library)、Open3D等库或者将结果导出为PLY文件用MeshLab、CloudCompare等软件查看。生成一个规则或随机点云应用FPS采样观察采样点是否均匀地散布在原始点云的整个空间范围内而不是聚集在某处。4.2 性能对比实验我们可以设计一个简单的性能测试比较不同实现版本在相同输入下的耗时。#include chrono // ... 其他头文件 ... int main() { // 生成测试点云一个随机点云 std::vectorPoint3D points; int num_points 50000; std::mt19937 rng(42); // 固定种子保证可重复性 std::uniform_real_distributionfloat dist(-10.0f, 10.0f); for (int i 0; i num_points; i) { points.emplace_back(dist(rng), dist(rng), dist(rng)); } int num_samples 5000; // 测试优化版本 auto start std::chrono::high_resolution_clock::now(); auto sampled_indices_opt farthestPointSamplingOptimized(points, num_samples); auto end std::chrono::high_resolution_clock::now(); auto duration_opt std::chrono::duration_caststd::chrono::milliseconds(end - start); std::cout Optimized FPS took duration_opt.count() ms. std::endl; // 测试朴素版本对于大规模点云可能非常慢慎用 // auto start_bf std::chrono::high_resolution_clock::now(); // auto sampled_indices_bf farthestPointSamplingBruteForce(points, num_samples); // auto end_bf std::chrono::high_resolution_clock::now(); // auto duration_bf std::chrono::duration_caststd::chrono::milliseconds(end_bf - start_bf); // std::cout Brute-Force FPS took duration_bf.count() ms. std::endl; std::cout Sampled sampled_indices_opt.size() points. std::endl; return 0; }在我的测试环境中5万点采5千点优化版本通常在几百毫秒到一秒内完成而朴素版本可能需要数十秒甚至分钟级。这个差距随着数据规模增大会急剧拉大。4.3 采样效果分析FPS采样的一个显著特点是其空间覆盖的均匀性。下图定性地展示了其与随机采样、均匀网格下采样的区别 此处用文字描述实际博文可配图随机采样点可能聚集留下大片未覆盖区域。均匀网格采样受限于网格对齐可能丢失细小特征对旋转敏感。FPS采样点与点之间倾向于保持较远距离能更好地捕捉模型的轮廓和 extremities末端对点云的旋转和平移具有不变性。5. 常见问题、陷阱与高级话题5.1 为什么我的FPS结果每次运行不一样这是正常的因为FPS算法的第一个点是随机选择的。第一个种子点的不同会导致后续迭代的“最远点”序列完全不同。这就像消防站选址第一个站设在城东还是城西会完全改变后续站点的布局。如果你需要确定性的结果可以将随机种子固定。// 固定随机种子以获得可重复结果 std::mt19937 gen(12345); // 使用固定种子 std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen);5.2 距离度量可以改变吗当然可以。我们一直使用的是欧氏距离L2范数这是最常见的选择。但你完全可以根据应用场景使用其他距离度量例如曼哈顿距离L1范数|x1-x2| |y1-y2| |z1-z2|切比雪夫距离L∞范数max(|x1-x2|, |y1-y2|, |z1-z2|)特征空间距离如果点除了坐标还有颜色、法向量等特征你可以定义一种结合了几何和特征信息的复合距离。这在一些特定任务中非常有用。只需要修改squaredDistance和distance函数即可。但要注意使用非欧氏距离时“距离平方”的概念可能不存在或不方便需要调整代码逻辑。5.3 算法复杂度能进一步优化吗O(M*N)是下限吗对于通用的、无结构的高维点集FPS的O(M*N)复杂度很难有理论上的突破因为每一轮你确实需要检查所有点来更新距离。但是对于**低维空间如2D3D**的点云我们可以利用空间数据结构进行加速使用KD-Tree或Octree在初始化时构建一个覆盖所有点的KD-Tree。在每一轮中要找到距离当前点集最远的点可以转化为一个“在所有点中找到距离给定点集可能是多个点最近距离最大的点”的问题。这可以通过在KD-Tree上进行一种修改的最近邻搜索来近似或精确求解。更新操作也可以利用树结构进行区域查询避免遍历所有点。一些研究论文和库如FLANN, PCL实现了基于树的近似FPS能显著提升速度尤其是当N很大时。5.4 内存占用优化我们的优化版本使用了O(N)的min_distances数组和一个最大堆。堆在最坏情况下可能包含O(N M*N)个条目但实际中由于很多点的距离不会被更新或者更新次数有限内存占用是可接受的。如果内存极其紧张可以回到朴素版本它只使用O(N)的额外空间min_distances数组。5.5 在深度学习框架中的实现在PyTorch或TensorFlow中FPS通常作为神经网络的一个层来实现。其实现会充分利用GPU的并行计算能力。核心思路是将双重循环转化为矩阵运算。例如计算一个新点到所有点的距离是一个(1, 3)和(N, 3)张量的广播减法、平方和运算非常高效。查找最大值可以使用argmax函数。虽然更新操作仍需一个O(N)的赋值但整个流程可以利用GPU的并行性获得远超CPU串行实现的速率。这也是为什么在PointNet等模型中FPS层虽然计算量大但依然可行的原因。6. 完整可编译示例代码以下是一个整合了优化版本、简单测试和性能计时的完整示例。#include iostream #include vector #include cmath #include limits #include random #include queue #include chrono #include cassert struct Point3D { float x, y, z; Point3D(float x_ 0, float y_ 0, float z_ 0) : x(x_), y(y_), z(z_) {} }; inline float squaredDistance(const Point3D p1, const Point3D p2) { float dx p1.x - p2.x; float dy p1.y - p2.y; float dz p1.z - p2.z; return dx * dx dy * dy dz * dz; } std::vectorint farthestPointSampling(const std::vectorPoint3D points, int num_samples, int seed 42) { int n static_castint(points.size()); if (n 0 || num_samples 0) return {}; num_samples std::min(num_samples, n); std::vectorint indices; indices.reserve(num_samples); // 固定随机种子用于可重复性生产环境可用随机设备 std::mt19937 gen(seed); std::uniform_int_distribution dis(0, n - 1); int first_idx dis(gen); indices.push_back(first_idx); std::vectorfloat min_dists(n, std::numeric_limitsfloat::max()); for (int i 0; i n; i) { min_dists[i] squaredDistance(points[i], points[first_idx]); } using Pair std::pairfloat, int; std::priority_queuePair pq; for (int i 0; i n; i) { pq.push({min_dists[i], i}); } while (indices.size() static_castsize_t(num_samples)) { Pair top; do { // 如果堆空理论上不会则中断 if (pq.empty()) { std::cerr Warning: Priority queue emptied early. Sampling may be incomplete.\n; return indices; } top pq.top(); pq.pop(); } while (std::abs(top.first - min_dists[top.second]) 1e-7f); int next_idx top.second; indices.push_back(next_idx); const Point3D new_pt points[next_idx]; // 更新距离并插入新条目到堆 for (int i 0; i n; i) { float new_dist squaredDistance(points[i], new_pt); if (new_dist min_dists[i]) { min_dists[i] new_dist; pq.push({new_dist, i}); } } } return indices; } int main() { // 1. 生成测试数据一个简单立方体 内部一些随机点 std::vectorPoint3D cloud; cloud.emplace_back(0, 0, 0); cloud.emplace_back(1, 0, 0); cloud.emplace_back(0, 1, 0); cloud.emplace_back(1, 1, 0); cloud.emplace_back(0, 0, 1); cloud.emplace_back(1, 0, 1); cloud.emplace_back(0, 1, 1); cloud.emplace_back(1, 1, 1); // 立方体8个顶点 std::mt19937 rng(123); std::uniform_real_distributionfloat urd(0.2f, 0.8f); for (int i 0; i 100; i) { cloud.emplace_back(urd(rng), urd(rng), urd(rng)); // 内部100个随机点 } int num_samples 10; // 2. 执行FPS并计时 auto start std::chrono::high_resolution_clock::now(); std::vectorint sampled farthestPointSampling(cloud, num_samples); auto end std::chrono::high_resolution_clock::now(); auto duration std::chrono::duration_caststd::chrono::microseconds(end - start); // 3. 输出结果 std::cout Total points: cloud.size() std::endl; std::cout Sampled sampled.size() points in duration.count() us. std::endl; std::cout Sampled indices: ; for (int idx : sampled) { std::cout idx ; } std::cout std::endl; // 4. 简单验证采样点应不重复 std::sort(sampled.begin(), sampled.end()); auto last std::unique(sampled.begin(), sampled.end()); assert(last sampled.end() Sampled indices contain duplicates!); // 5. 可视化建议 std::cout \nTo visualize, you can output the original and sampled points to a file.\n; std::cout For example, save as a PLY file and open it with MeshLab.\n; // 这里可以添加简单的PLY文件输出代码可选 return 0; }将这段代码保存为fps_demo.cpp使用支持C11及以上版本的编译器编译即可运行。g -stdc11 -O3 fps_demo.cpp -o fps_demo ./fps_demo这个实现平衡了清晰度、正确性和性能是理解和应用FPS算法的良好起点。在实际项目中如果点云规模巨大超过百万你可能需要结合空间索引树KD-Tree, Octree来进一步加速或者直接调用PCL、Open3D等成熟库中经过深度优化的FPS函数。但无论如何理解其背后的原理和优化思路是灵活运用和调试的基石。
学习路径与资源推荐)
