并行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;
	}

	// 计算 d
	param.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 + 1
	for (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;
}