并行RANSAC平面拟合（C++)

依赖库

1）Eigen

2）GLM

算法大致思路

Step 1：源点云随机采样3个点；

Step 2：3个点拟合平面，统计符合该平面模型的点，为inlier点；

Step 3：判断inlier点比例是否达到阈值，更新最佳模型和最佳模型对应的inlier点集。若inlier比例不到阈值，则继续Step 1；若已满足阈值或达到最大迭代次数，转Step 4；

Step 4：针对最佳模型的inlier点集，使用最小二乘法，重新拟合平面；

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <limits>
#include <algorithm>
#include <Eigen/Dense>
#include <thread>
#include <mutex>
#include <atomic>
#include<glm/glm.hpp>3D 点结构体
//struct Point3D {
//	double x, y, z;
//	Point3D(double x_ = 0, double y_ = 0, double z_ = 0) : x(x_), y(y_), z(z_) {}
//};struct Plane3D
{double a, b, c, d;Plane3D(){a = b = d = 0.0;c = 1.0;}Plane3D(double _a, double _b, double _c, double _d){a = _a;b = _b;c = _c;d = _d;}};typedef glm::dvec3 Point3d;// 平面模型: ax + by + cz + d = 0
struct PlaneModel {//double a, b, c, d;Plane3D param;std::vector<Point3d> inliers;// 从三个点计算平面方程void computeFrom3Points(const Point3d& p1, const Point3d& p2, const Point3d& p3) {// 计算两个向量Point3d v1(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);Point3d v2(p3.x - p1.x, p3.y - p1.y, p3.z - p1.z);// 计算叉积得到法向量 (a,b,c)param.a = v1.y * v2.z - v1.z * v2.y;param.b = v1.z * v2.x - v1.x * v2.z;param.c = v1.x * v2.y - v1.y * v2.x;// 归一化double norm = std::sqrt(param.a * param.a + param.b * param.b + param.c * param.c);if (norm > 0) {param.a /= norm;param.b /= norm;param.c /= norm;}// 计算 dparam.d = -(param.a * p1.x + param.b * p1.y + param.c * p1.z);
}// 使用最小二乘法从内点拟合平面
void refineWithLeastSquares() {if (inliers.size() < 3) return;// 构建矩阵 A 和向量 b (Ax = b)Eigen::MatrixXd A(inliers.size(), 3);Eigen::VectorXd b(inliers.size());// 计算所有点的质心Point3d centroid(0, 0, 0);for (const auto& p : inliers) {centroid.x += p.x;centroid.y += p.y;centroid.z += p.z;}centroid.x /= inliers.size();centroid.y /= inliers.size();centroid.z /= inliers.size();// 构建矩阵for (size_t i = 0; i < inliers.size(); ++i) {A(i, 0) = inliers[i].x - centroid.x;A(i, 1) = inliers[i].y - centroid.y;A(i, 2) = inliers[i].z - centroid.z;}// 使用SVD求解最小二乘问题Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);Eigen::Vector3d normal = svd.matrixV().col(2);// 更新平面参数param.a = normal(0);param.b = normal(1);param.c = normal(2);param.d = -(param.a * centroid.x + param.b * centroid.y + param.c * centroid.z);// 归一化double norm = std::sqrt(param.a * param.a + param.b * param.b + param.c * param.c);if (norm > 0) {param.a /= norm;param.b /= norm;param.c /= norm;param.d /= norm;}}// 计算点到平面的距离double distanceTo(const Point3d& p) const {return std::abs(param.a * p.x + param.b * p.y + param.c * p.z + param.d);}// 评估模型，收集内点int evaluate(const std::vector<Point3d>& points, double distanceThreshold) {inliers.clear();for (const auto& p : points) {if (distanceTo(p) < distanceThreshold) {inliers.push_back(p);}}return inliers.size();}
};// 并行RANSAC工作函数
void ransacWorker(const std::vector<Point3d>& points,int maxIterations,double distanceThreshold,int minInliers,std::atomic<int>& bestInliers,PlaneModel& bestModel,std::mutex& modelMutex,std::atomic<bool>& stopFlag,double inlierRatioThresh,int threadId)
{std::random_device rd;std::mt19937 gen(rd() + threadId); // 每个线程有不同的种子std::uniform_int_distribution<> dis(0, points.size() - 1);PlaneModel localBestModel;int localBestInliers = -1;for (int i = 0; i < maxIterations && !stopFlag; ++i) {// 随机选择3个不重复的点int idx1 = dis(gen);int idx2, idx3;do { idx2 = dis(gen); } while (idx2 == idx1);do { idx3 = dis(gen); } while (idx3 == idx1 || idx3 == idx2);// 计算平面模型PlaneModel model;model.computeFrom3Points(points[idx1], points[idx2], points[idx3]);// 评估模型int inliers = model.evaluate(points, distanceThreshold);// 更新本地最佳模型if (inliers > localBestInliers && inliers >= minInliers) {localBestInliers = inliers;localBestModel = model;// 检查是否需要更新全局最佳模型if (localBestInliers > bestInliers) {std::lock_guard<std::mutex> lock(modelMutex);if (localBestInliers > bestInliers) {bestInliers = localBestInliers;bestModel = localBestModel;// 动态调整: 如果找到足够好的模型，可以提前停止double inlierRatio = static_cast<double>(bestInliers) / points.size();if (inlierRatio > inlierRatioThresh) { // 如果内点比例超过80%，提前停止stopFlag = true;}}}}}
}// 并行RANSAC平面拟合
// inlierRatioThresh=0.9 表示内点占比超过90%，可以提前终止迭代
PlaneModel parallelRansacFitPlane(const std::vector<Point3d>& points,int maxIterations = 1000,double distanceThreshold = 0.01,int minInliers = 0,int numThreads = 4,double inlierRatioThresh = 0.8) 
{if (points.size() < 3) {throw std::runtime_error("At least 3 points are required to fit a plane");}if (minInliers <= 0) {minInliers = static_cast<int>(points.size() * 0.3); // 默认最小内点数为30%}std::atomic<int> bestInliers(-1);PlaneModel bestModel;std::mutex modelMutex;std::atomic<bool> stopFlag(false);// 计算每个线程的迭代次数int iterationsPerThread = maxIterations / numThreads;std::vector<std::thread> threads;for (int i = 0; i < numThreads; ++i) {threads.emplace_back(ransacWorker,std::ref(points),iterationsPerThread,distanceThreshold,minInliers,std::ref(bestInliers),std::ref(bestModel),std::ref(modelMutex),std::ref(stopFlag),inlierRatioThresh,i);}// 等待所有线程完成for (auto& t : threads) {t.join();}// 如果没有找到足够内点的模型，返回第一个模型if (bestInliers == -1) {bestModel.computeFrom3Points(points[0], points[1], points[2]);}// 使用最小二乘法优化最佳模型bestModel.evaluate(points, distanceThreshold); // 找出所有内点bestModel.refineWithLeastSquares();return bestModel;
}// 示例使用
int main() {// 生成一些测试数据 - 平面上的点加一些噪声std::vector<Point3d> points;// 理想平面 z = 2x + 3y + 1for (int i = 0; i < 1000; ++i) {double x = (rand() % 1000) / 100.0;double y = (rand() % 1000) / 100.0;double z = 2 * x + 3 * y + 1;// 添加一些高斯噪声z += (rand() % 100) / 1000.0;points.emplace_back(x, y, z);}// 添加一些噪声点 (离群值)for (int i = 0; i < 200; ++i) {double x = (rand() % 1000) / 100.0;double y = (rand() % 1000) / 100.0;double z = (rand() % 1000) / 10.0;points.emplace_back(x, y, z);}// 使用并行RANSAC拟合平面PlaneModel plane = parallelRansacFitPlane(points, 10000, 0.1, 300, 4, 0.8);std::cout << "拟合平面方程: " << plane.param.a << "x + "<< plane.param.b << "y + " << plane.param.c << "z + "<< plane.param.d << " = 0" << std::endl;std::cout << "内点数量: " << plane.inliers.size()<< "/" << points.size() << std::endl;// 计算拟合误差double totalError = 0;for (const auto& p : plane.inliers) {totalError += plane.distanceTo(p);}std::cout << "平均拟合误差: " << totalError / plane.inliers.size() << std::endl;return 0;
}