#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#ifdef  __WIN32
#include <corecrt_math_defines.h>
#endif //  __WIN32

#include <cmath>
#include <unordered_map>
#include <Eigen/dense>

void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
{
	//最小二乘拟合直线参数
	double xx_sum = 0;
	double x_sum = 0;
	double y_sum = 0;
	double xy_sum = 0;
	int num = 0;
	for (int i = 0; i < inliers.size(); i++)
	{
		x_sum += inliers[i].x; //x的累加和
		y_sum += inliers[i].y; //y的累加和
		xx_sum += inliers[i].x * inliers[i].x; //x的平方累加和
		xy_sum += inliers[i].x * inliers[i].y; //x，y的累加和
		num++;
	}
	*_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //根据公式求解k
	*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
}

//拟合成通用直线方程ax+by+c=0，包括垂直
void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c)
{
	//判断是否为垂直
	int dataSize = (int)inliers.size();
	if (dataSize < 2)
		return;

	double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
	double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
	std::vector< SVzNL3DPoint> fittingData;
	if (deltaX < deltaY)
	{
		//x=ky+b 拟合
		for (int i = 0; i < dataSize; i++)
		{
			SVzNL3DPoint a_fitPt;
			a_fitPt.x = inliers[i].y;
			a_fitPt.y = inliers[i].x;
			a_fitPt.z = inliers[i].z;
			fittingData.push_back(a_fitPt);
		}
		double k = 0, b = 0;
		lineFitting(fittingData, &k, &b);
		//ax+by+c
		*_a = 1.0;
		*_b = -k;
		*_c = -b;
	}
	else
	{
		//y = kx+b拟合
		double k = 0, b = 0;
		lineFitting(inliers, &k, &b);
		//ax+by+c
		*_a = k;
		*_b = -1;
		*_c = b;
	}
	return;
}

//拟合成通用直线方程ax+by+c=0，包括垂直
void indexingPtLineFitting_abc(std::vector< SWD3DPointPostion>& inliers, double* _a, double* _b, double* _c)
{
	//判断是否为垂直
	int dataSize = (int)inliers.size();
	if (dataSize < 2)
		return;

	double deltaX = abs(inliers[0].point.x - inliers[dataSize - 1].point.x);
	double deltaY = abs(inliers[0].point.y - inliers[dataSize - 1].point.y);
	std::vector< SVzNL3DPoint> fittingData;
	if (deltaX < deltaY)
	{
		//x=ky+b 拟合
		for (int i = 0; i < dataSize; i++)
		{
			SVzNL3DPoint a_fitPt;
			a_fitPt.x = inliers[i].point.y;
			a_fitPt.y = inliers[i].point.x;
			a_fitPt.z = inliers[i].point.z;
			fittingData.push_back(a_fitPt);
		}
		double k = 0, b = 0;
		lineFitting(fittingData, &k, &b);
		//ax+by+c
		*_a = 1.0;
		*_b = -k;
		*_c = -b;
	}
	else
	{
		for (int i = 0; i < dataSize; i++)
		{
			SVzNL3DPoint a_fitPt;
			a_fitPt.x = inliers[i].point.x;
			a_fitPt.y = inliers[i].point.y;
			a_fitPt.z = inliers[i].point.z;
			fittingData.push_back(a_fitPt);
		}
		//y = kx+b拟合
		double k = 0, b = 0;
		lineFitting(fittingData, &k, &b);
		//ax+by+c
		*_a = k;
		*_b = -1;
		*_c = b;
	}
	return;
}

//圆最小二乘拟合
double fitCircleByLeastSquare(
	const std::vector<SVzNL3DPoint>& pointArray,
	SVzNL3DPoint& center,
	double& radius)
{
	int N = pointArray.size();
	if (N < 3) {
		return std::numeric_limits<double>::max();
	}

	double sumX = 0.0;
	double sumY = 0.0;
	double sumX2 = 0.0;
	double sumY2 = 0.0;
	double sumX3 = 0.0;
	double sumY3 = 0.0;
	double sumXY = 0.0;
	double sumXY2 = 0.0;
	double sumX2Y = 0.0;

	for (int pId = 0; pId < N; ++pId) {
		sumX += pointArray[pId].x;
		sumY += pointArray[pId].y;

		double x2 = pointArray[pId].x * pointArray[pId].x;
		double y2 = pointArray[pId].y * pointArray[pId].y;
		sumX2 += x2;
		sumY2 += y2;

		sumX3 += x2 * pointArray[pId].x;
		sumY3 += y2 * pointArray[pId].y;
		sumXY += pointArray[pId].x * pointArray[pId].y;
		sumXY2 += pointArray[pId].x * y2;
		sumX2Y += x2 * pointArray[pId].y;
	}

	double C, D, E, G, H;
	double a, b, c;

	C = N * sumX2 - sumX * sumX;
	D = N * sumXY - sumX * sumY;
	E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
	G = N * sumY2 - sumY * sumY;
	H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;

	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

	center.x = -a / 2.0;
	center.y = -b / 2.0;
	radius = sqrt(a * a + b * b - 4 * c) / 2.0;

	double err = 0.0;
	double e;
	double r2 = radius * radius;
	for (int pId = 0; pId < N; ++pId) {
		e = pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2;
		if (e > err) {
			err = e;
		}
	}
	return err;
}

//圆最小二乘拟合
double fitCircleByLeastSquare_2(
	const std::vector<SVzNL3DPosition>& pointArray,
	SVzNL3DPoint& center,
	double& radius)
{
	int N = pointArray.size();
	if (N < 3) {
		return std::numeric_limits<double>::max();
	}

	double sumX = 0.0;
	double sumY = 0.0;
	double sumX2 = 0.0;
	double sumY2 = 0.0;
	double sumX3 = 0.0;
	double sumY3 = 0.0;
	double sumXY = 0.0;
	double sumXY2 = 0.0;
	double sumX2Y = 0.0;

	for (int pId = 0; pId < N; ++pId) {
		sumX += pointArray[pId].pt3D.x;
		sumY += pointArray[pId].pt3D.y;

		double x2 = pointArray[pId].pt3D.x * pointArray[pId].pt3D.x;
		double y2 = pointArray[pId].pt3D.y * pointArray[pId].pt3D.y;
		sumX2 += x2;
		sumY2 += y2;

		sumX3 += x2 * pointArray[pId].pt3D.x;
		sumY3 += y2 * pointArray[pId].pt3D.y;
		sumXY += pointArray[pId].pt3D.x * pointArray[pId].pt3D.y;
		sumXY2 += pointArray[pId].pt3D.x * y2;
		sumX2Y += x2 * pointArray[pId].pt3D.y;
	}

	double C, D, E, G, H;
	double a, b, c;

	C = N * sumX2 - sumX * sumX;
	D = N * sumXY - sumX * sumY;
	E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
	G = N * sumY2 - sumY * sumY;
	H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;

	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

	center.x = -a / 2.0;
	center.y = -b / 2.0;
	radius = sqrt(a * a + b * b - 4 * c) / 2.0;

	double err = 0.0;
	double e;
	double r2 = radius * radius;
	for (int pId = 0; pId < N; ++pId) {
		e = pow(pointArray[pId].pt3D.x - center.x, 2) + pow(pointArray[pId].pt3D.y - center.y, 2) - r2;
		if (e > err) {
			err = e;
		}
	}
	return err;
}

//圆最小二乘拟合
double fitCircleByLeastSquare_3(
	const std::vector<SVzNL2DPointD>& pointArray,
	SVzNL2DPointD& center,
	double& radius)
{
	int N = pointArray.size();
	if (N < 3) {
		return std::numeric_limits<double>::max();
	}

	double sumX = 0.0;
	double sumY = 0.0;
	double sumX2 = 0.0;
	double sumY2 = 0.0;
	double sumX3 = 0.0;
	double sumY3 = 0.0;
	double sumXY = 0.0;
	double sumXY2 = 0.0;
	double sumX2Y = 0.0;

	for (int pId = 0; pId < N; ++pId) {
		sumX += pointArray[pId].x;
		sumY += pointArray[pId].y;

		double x2 = pointArray[pId].x * pointArray[pId].x;
		double y2 = pointArray[pId].y * pointArray[pId].y;
		sumX2 += x2;
		sumY2 += y2;

		sumX3 += x2 * pointArray[pId].x;
		sumY3 += y2 * pointArray[pId].y;
		sumXY += pointArray[pId].x * pointArray[pId].y;
		sumXY2 += pointArray[pId].x * y2;
		sumX2Y += x2 * pointArray[pId].y;
	}

	double C, D, E, G, H;
	double a, b, c;

	C = N * sumX2 - sumX * sumX;
	D = N * sumXY - sumX * sumY;
	E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
	G = N * sumY2 - sumY * sumY;
	H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;

	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

	center.x = -a / 2.0;
	center.y = -b / 2.0;
	radius = sqrt(a * a + b * b - 4 * c) / 2.0;

	double err = 0.0;
	double e;
	double r2 = radius * radius;
	for (int pId = 0; pId < N; ++pId) {
		e = pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2;
		if (e > err) {
			err = e;
		}
	}
	return err;
}

// 计算点到圆的几何距离（带符号）
double geometricDistance(const SVzNL3DPosition& p, const SVzNL3DPoint& center, const double R) {
	return std::sqrt((p.pt3D.x - center.x) * (p.pt3D.x - center.x) + (p.pt3D.y - center.y) * (p.pt3D.y - center.y)) - R;
}

// ========== 计算几何误差（点到圆的距离） ==========
double calcGeometricError(const SVzNL3DPosition& p, const SWD_Circle& c) {
	double dx = p.pt3D.x - c.cx;
	double dy = p.pt3D.y - c.cy;
	return fabs(sqrt(dx * dx + dy * dy) - c.r);
}

// ========== 计算所有点的误差统计 ==========
void calcErrorStats(const std::vector<SVzNL3DPosition>& points, const SWD_Circle& c,
	std::vector<double>& errors, double& mean, double& stddev) 
{
	int n = points.size();
	errors.resize(n);
	mean = 0;
	for (int i = 0; i < n; ++i) {
		errors[i] = calcGeometricError(points[i], c);
		mean += errors[i];
	}
	mean /= n;

	stddev = 0;
	for (int i = 0; i < n; ++i) {
		double d = errors[i] - mean;
		stddev += d * d;
	}
	stddev = sqrt(stddev / n);
}

// ========== 主函数：迭代剔除离群点 + 最小二乘拟合 ==========
double fitCircleRobust(
	const std::vector<SVzNL3DPosition>& points,
	SVzNL3DPoint& robustCenter,
	double& robustRadius,
	double outlierThreshold,  // 标准差倍数
	int maxIter,
	double minPointsRatio) 
{
	if (points.size() < 3) return 0;

	// 用所有点初始化
	std::vector<SVzNL3DPosition> workingPoints = points;
	SVzNL3DPoint c0;
	double r0;
	double errVal = fitCircleByLeastSquare_2(workingPoints, c0, r0);
	SWD_Circle bestCircle = { c0.x, c0.y, r0 };

	double bestMeanError = std::numeric_limits<double>::max();
	std::vector<SVzNL3DPosition> bestPoints = workingPoints;

	for (int iter = 0; iter < maxIter; ++iter) {
		// 计算当前拟合圆的误差统计
		std::vector<double> errors;
		double mean, stddev;
		calcErrorStats(workingPoints, bestCircle, errors, mean, stddev);

		// 检查收敛：如果平均误差不再显著改善
		if (mean >= bestMeanError * 0.99 && iter > 0) 
			break;
		bestMeanError = mean;

		// 如果标准差太小，说明已经收敛
		if (stddev < 1e-8) break;

		// 剔除离群点：误差 > mean + outlierThreshold * stddev
		double threshold = mean + outlierThreshold * stddev;
		std::vector<SVzNL3DPosition> filteredPoints;
		filteredPoints.reserve(workingPoints.size());

		for (size_t i = 0; i < workingPoints.size(); ++i) {
			if (errors[i] <= threshold) {
				filteredPoints.push_back(workingPoints[i]);
			}
		}

		// 如果剩余点太少，停止剔除
		if (filteredPoints.size() < points.size() * minPointsRatio ||
			filteredPoints.size() < 3) {
			break;
		}

		// 用剩余点重新拟合
		SVzNL3DPoint center;
		double radius;
		errVal = fitCircleByLeastSquare_2(filteredPoints, center, radius);
		SWD_Circle newCircle = { center.x, center.y, radius };

		// 检查新圆是否更优（用平均误差衡量）
		double newMean = 0;
		for (const auto& p : filteredPoints) {
			newMean += calcGeometricError(p, newCircle);
		}
		newMean /= filteredPoints.size();

		if (newMean < bestMeanError) {
			bestCircle = newCircle;
			workingPoints = filteredPoints;
			bestPoints = filteredPoints;
		}
		else {
			// 新圆不如旧圆好，停止迭代
			break;
		}
	}
	robustCenter.x = bestCircle.cx;
	robustCenter.y = bestCircle.cy;
	robustCenter.z = 0;
	robustRadius = bestCircle.r;
	return bestMeanError;
}


#if 0
bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
	double& a, double& b, double& c,
	double& mse, double& max_err)
{
	// 校验点集数量（至少3个点才能拟合抛物线）
	int n = points.size();
	if (n < 3) {
		return false;
	}

	// 初始化各项求和参数
	double sum_x = 0.0, sum_x2 = 0.0, sum_x3 = 0.0, sum_x4 = 0.0;
	double sum_y = 0.0, sum_xy = 0.0, sum_x2y = 0.0;

	// 计算各项求和
	for (const auto& p : points) {
		double x = p.x;
		double y = p.y;
		double x2 = x * x;
		double x3 = x2 * x;
		double x4 = x3 * x;

		sum_x += x;
		sum_x2 += x2;
		sum_x3 += x3;
		sum_x4 += x4;
		sum_y += y;
		sum_xy += x * y;
		sum_x2y += x2 * y;
	}

	// 构建线性方程组 M * [a,b,c]^T = N
	// M矩阵：3x3
	double M[3][3] = {
		{sum_x4, sum_x3, sum_x2},
		{sum_x3, sum_x2, sum_x},
		{sum_x2, sum_x, (double)n}
	};

	// N向量：3x1
	double N[3] = { sum_x2y, sum_xy, sum_y };

	// 高斯消元法求解线性方程组（3元一次方程组）
	// 步骤1：将M转化为上三角矩阵
	for (int i = 0; i < 3; i++) {
		// 选主元（避免除数为0）
		int pivot = i;
		for (int j = i; j < 3; j++) {
			if (fabs(M[j][i]) > fabs(M[pivot][i])) {
				pivot = j;
			}
		}
		// 交换行
		std::swap(M[i], M[pivot]);
		std::swap(N[i], N[pivot]);

		// 归一化主元行
		double div = M[i][i];
		if (fabs(div) < 1e-10) {
			return false;
		}
		for (int j = i; j < 3; j++) {
			M[i][j] /= div;
		}
		N[i] /= div;

		// 消去下方行
		for (int j = i + 1; j < 3; j++) {
			double factor = M[j][i];
			for (int k = i; k < 3; k++) {
				M[j][k] -= factor * M[i][k];
			}
			N[j] -= factor * N[i];
		}
	}

	// 步骤2：回代求解
	c = N[2];
	b = N[1] - M[1][2] * c;
	a = N[0] - M[0][1] * b - M[0][2] * c;

	// 计算拟合误差
	mse = 0.0;
	max_err = 0.0;
	for (const auto& p : points) {
		double y_fit = a * p.x * p.x + b * p.x + c;
		double err = y_fit - p.y;
		double err_abs = fabs(err);
		mse += err * err;
		if (err_abs > max_err) {
			max_err = err_abs;
		}
	}
	mse /= n; // 均方误差

	return true;
}
#endif
//抛物线最小二乘拟合 y=ax^2 + bx + c
bool leastSquareParabolaFitEigen(
	const std::vector<cv::Point2d>& points,
	double& a, double& b, double& c,
	double& mse, double& max_err)
{
	int n = points.size();
	if (n < 3) {
		return false;
	}

	// 构建系数矩阵A和目标向量B
	Eigen::MatrixXd A(n, 3);
	Eigen::VectorXd B(n);
	for (int i = 0; i < n; i++) {
		double x = points[i].x;
		A(i, 0) = x * x;
		A(i, 1) = x;
		A(i, 2) = 1.0;
		B(i) = points[i].y;
	}

	// 最小二乘求解：Ax = B，直接调用Eigen的求解器
	Eigen::Vector3d coeffs = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
	a = coeffs(0);
	b = coeffs(1);
	c = coeffs(2);

	// 计算误差
	mse = 0.0;
	max_err = 0.0;
	for (const auto& p : points) {
		double y_fit = a * p.x * p.x + b * p.x + c;
		double err = y_fit - p.y;
		double err_abs = fabs(err);
		mse += err * err;
		if (err_abs > max_err) {
			max_err = err_abs;
		}
	}
	mse /= n;
	return true;
}

//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
void vzCaculateLaserPlane(std::vector<cv::Point3d>& Points3ds, std::vector<double>& res)
{
	//最小二乘法拟合平面
	//获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反
	//系数矩阵
	cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
	//
	cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
	//结果矩阵
	cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
	double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
	for (int i = 0; i < Points3ds.size(); i++)
	{
		x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
		y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
		xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
		xi += (double)Points3ds[i].x;
		yi += (double)Points3ds[i].y;
		zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
		ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
		zi += (double)Points3ds[i].z;
	}
	A.at<double>(0, 0) = x2;
	A.at<double>(1, 0) = xiyi;
	A.at<double>(2, 0) = xi;
	A.at<double>(0, 1) = xiyi;
	A.at<double>(1, 1) = y2;
	A.at<double>(2, 1) = yi;
	A.at<double>(0, 2) = xi;
	A.at<double>(1, 2) = yi;
	A.at<double>(2, 2) = (double)((int)Points3ds.size());
	B.at<double>(0, 0) = zixi;
	B.at<double>(1, 0) = ziyi;
	B.at<double>(2, 0) = zi;
	//计算平面系数
	X = A.inv() * B;
	//A
	res.push_back(X.at<double>(0, 0));
	//B
	res.push_back(X.at<double>(1, 0));
	//Z的系数为-1
	res.push_back(-1.0);
	//C
	res.push_back(X.at<double>(2, 0));
	return;
}

/**
 * @brief 空间直线最小二乘拟合
 * @param points 输入的三维点集（至少2个点，否则无意义）
 * @param center 输出：拟合直线的质心（基准点P0）
 * @param direction 输出：拟合直线的方向向量v（单位化）
 * @return 拟合是否成功（点集有效返回true）
 */
bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint& center, SVzNL3DPoint& direction)
{
	// 检查点集有效性
	if (points.size() < 2) {
		std::cerr << "Error: 点集数量必须大于等于2！" << std::endl;
		return false;
	}

	int n = points.size();
	Eigen::MatrixXd A(n, 3); // 点集矩阵：每行一个点的(x,y,z)

	// 1. 计算质心（center）
	double cx = 0.0, cy = 0.0, cz = 0.0;
	for (const auto& p : points) {
		cx += p.x;
		cy += p.y;
		cz += p.z;
		A.row(points.size() - n) << p.x, p.y, p.z; // 填充点集矩阵
		n--;
	}
	cx /= points.size();
	cy /= points.size();
	cz /= points.size();
	center = { cx, cy, cz };

	// 2. 构造去中心化的协方差矩阵（3x3）
	// 关键修复：使用RowVector3d（行向量）做rowwise减法，匹配维度
	Eigen::RowVector3d centroid_row(cx, cy, cz);
	Eigen::MatrixXd centered = A.rowwise() - centroid_row; // 维度匹配，无报错

	// 协方差矩阵计算（n-1为无偏估计，工程中也可直接用n）
	Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1);
	// 3. 特征值分解：求协方差矩阵的特征值和特征向量
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
	if (eigensolver.info() != Eigen::Success) {
		std::cerr << "Error: 特征值分解失败！" << std::endl;
		return false;
	}

	// 最大特征值对应的特征向量即为方向向量（Eigen默认按特征值升序排列，取最后一个）
	Eigen::Vector3d dir = eigensolver.eigenvectors().col(2);
	// 单位化方向向量（可选，但工程中通常标准化）
	dir.normalize();

	direction = { dir(0), dir(1), dir(2) };
	return true;
}

// ============================== 工具函数 ==============================
// 点到平面距离（带符号）
double pointToPlaneSignedDist(const cv::Point3d& p, const Plane& plane) {
	return (plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D);
}

// 点到平面的距离（绝对值）
float pointToPlaneDistance(const cv::Point3d& p, const Plane& plane) {
	return fabs(plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D)
		/ sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
}

// 归一化平面（法向量模长=1）
void normalizePlane(Plane& plane) {
	double norm = sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
	if (norm < 1e-6) return;
	plane.A /= norm;
	plane.B /= norm;
	plane.C /= norm;
	plane.D /= norm;
}

// ============================== 鲁棒损失函数 ==============================
// Huber 权重
double huberWeight(double r, double delta) {
	r = fabs(r);
	if (r <= delta) return 1.0;
	else return delta / r;
}

// Tukey 权重（离群点=0，最适合凹坑/强噪声）
double tukeyWeight(double r, double c) {
	r = fabs(r);
	if (r > c) return 0.0;
	double t = 1.0 - (r * r) / (c * c);
	return t * t;
}

// ============================== 鲁棒加权最小二乘平面拟合 ==============================
Plane robustFitPlane(
	const std::vector< cv::Point3d>& points,
	RobustType type,
	double delta,   // 阈值：>此值视为离群点（mm）
	int maxIter,      // 迭代次数
	double convergeThresh  // 收敛阈值（平面变化足够小就停）
) 
{
	int n = points.size();
	if (n < 3) return Plane();

	// 1. 先用普通最小二乘初始化平面
	double cx = 0, cy = 0, cz = 0;
	for (auto& p : points) { cx += p.x; cy += p.y; cz += p.z; }
	cx /= n; cy /= n; cz /= n;

	double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
	for (auto& p : points) {
		double dx = p.x - cx;
		double dy = p.y - cy;
		double dz = p.z - cz;
		xx += dx * dx; xy += dx * dy; xz += dx * dz;
		yy += dy * dy; yz += dy * dz; zz += dz * dz;
	}

	double detX = yy * zz - yz * yz;
	double detY = xx * zz - xz * xz;
	double detZ = xx * yy - xy * xy;
	double maxDet = std::max({ detX, detY, detZ });

	Plane plane;
	if (maxDet == detX) {
		plane.A = 1;
		plane.B = (xy * yz - xz * yy) / detX;
		plane.C = (xz * yz - xy * zz) / detX;
	}
	else if (maxDet == detY) {
		plane.A = (xy * yz - xz * yy) / detY;
		plane.B = 1;
		plane.C = (xz * xy - xx * yz) / detY;
	}
	else {
		plane.A = (xz * yz - xy * zz) / detZ;
		plane.B = (yz * xy - xz * yy) / detZ;
		plane.C = 1;
	}
	plane.D = -(plane.A * cx + plane.B * cy + plane.C * cz);
	normalizePlane(plane);

	// 2. 迭代加权最小二乘 (IRLS)
	std::vector<double> weights(n, 1.0);

	for (int iter = 0; iter < maxIter; iter++) {
		Plane prevPlane = plane;

		double sum_w = 0;
		double swx = 0, swy = 0, swz = 0;
		double swxx = 0, swxy = 0, swxz = 0, swyy = 0, swyz = 0;

		for (int i = 0; i < n; i++) {
			double r = pointToPlaneSignedDist(points[i], plane);
			if (type == HUBER) weights[i] = huberWeight(r, delta);
			else weights[i] = tukeyWeight(r, delta);

			double w = weights[i];
			sum_w += w;
			swx += w * points[i].x;
			swy += w * points[i].y;
			swz += w * points[i].z;
		}

		double mx = swx / sum_w;
		double my = swy / sum_w;
		double mz = swz / sum_w;

		for (int i = 0; i < n; i++) {
			double w = weights[i];
			double dx = points[i].x - mx;
			double dy = points[i].y - my;
			double dz = points[i].z - mz;
			swxx += w * dx * dx;
			swxy += w * dx * dy;
			swxz += w * dx * dz;
			swyy += w * dy * dy;
			swyz += w * dy * dz;
		}

		// 解特征向量
		double detXw = swyy * swyz - swyz * swyz;
		double detYw = swxx * swyz - swxz * swxz;
		double detZw = swxx * swyy - swxy * swxy;
		double maxDw = std::max({ detXw, detYw, detZw });

		if (maxDw == detXw) {
			plane.A = 1;
			plane.B = (swxy * swyz - swxz * swyy) / detXw;
			plane.C = (swxz * swyz - swxy * swyz) / detXw;
		}
		else if (maxDw == detYw) {
			plane.A = (swxy * swyz - swxz * swyy) / detYw;
			plane.B = 1;
			plane.C = (swxz * swxy - swxx * swyz) / detYw;
		}
		else {
			plane.A = (swxz * swyz - swxy * swyz) / detZw;
			plane.B = (swyz * swxy - swxz * swyy) / detZw;
			plane.C = 1;
		}
		plane.D = -(plane.A * mx + plane.B * my + plane.C * mz);
		normalizePlane(plane);

		// ========== 关键：判断是否收敛 ==========
		double da = fabs(plane.A - prevPlane.A);
		double db = fabs(plane.B - prevPlane.B);
		double dc = fabs(plane.C - prevPlane.C);
		double dd = fabs(plane.D - prevPlane.D);

		double maxDiff = std::max({ da, db, dc, dd });

		// 平面几乎不再变化 → 提前终止
		if (maxDiff < convergeThresh) {
			break;
		}
	}
	return plane;
}

// 三点拟合平面
Plane planeFrom3Points(const cv::Point3d& p1, const cv::Point3d& p2, const cv::Point3d& p3) {
	double v1x = p2.x - p1.x;
	double v1y = p2.y - p1.y;
	double v1z = p2.z - p1.z;

	double v2x = p3.x - p1.x;
	double v2y = p3.y - p1.y;
	double v2z = p3.z - p1.z;

	double A = v1y * v2z - v1z * v2y;
	double B = v1z * v2x - v1x * v2z;
	double C = v1x * v2y - v1y * v2x;
	double D = -(A * p1.x + B * p1.y + C * p1.z);

	double norm = sqrtf(A * A + B * B + C * C);
	if (norm > 1e-6) { A /= norm; B /= norm; C /= norm; D /= norm; }
	return Plane(A, B, C, D);
}

// ==============================================
// 带 提前终止 的 RANSAC 平面拟合（工业正式版）
// ==============================================
Plane ransacFitPlane(
	const std::vector<cv::Point3d>& points,
	std::vector<cv::Point3d>& in_inliers,
	float dist_thresh,          // 内点距离阈值
	int max_iter,                // 最大迭代
	int stop_no_improve           // 连续多少次无提升就提前退出
) 
{
	in_inliers.clear();
	int n = points.size();
	if (n < 3) return Plane();

	int best_inlier = 0;
	Plane best_plane;

	srand((unsigned)time(nullptr));

	int no_improve_count = 0;  // 无提升计数

	for (int i = 0; i < max_iter; ++i) {
		// 随机3个不重复点
		int idx[3];
		idx[0] = rand() % n;
		do { idx[1] = rand() % n; } while (idx[1] == idx[0]);
		do { idx[2] = rand() % n; } while (idx[2] == idx[0] || idx[2] == idx[1]);

		Plane plane = planeFrom3Points(
			points[idx[0]],
			points[idx[1]],
			points[idx[2]]
		);

		// 统计内点
		int cnt = 0;
		for (auto& p : points) {
			if (pointToPlaneDistance(p, plane) < dist_thresh) cnt++;
		}

		// 更新最优模型
		if (cnt > best_inlier) {
			best_inlier = cnt;
			best_plane = plane;
			no_improve_count = 0;  // 有提升 → 清零
		}
		else {
			no_improve_count++;
		}

		// ====================== 提前终止条件 ======================
		if (no_improve_count >= stop_no_improve) {
			break;
		}
	}

	// 收集内点
	for (auto& p : points) {
		if (pointToPlaneDistance(p, best_plane) < dist_thresh)
			in_inliers.push_back(p);
	}

	return best_plane;
}

// 输入：3D点云 std::vector<Eigen::Vector3d>
// 输出：axis 轴向单位向量, centroid 点云中心
void computeCylinderAxisFromIncompletePCA(
	const std::vector<SVzNL3DPosition>& src_points,
	SVzNL3DPoint& vec_axis,
	SVzNL3DPoint& vec_centroid)
{
	vec_axis = { 0.0, 0.0, 0.0 };
	vec_centroid = { 0.0, 0.0, 0.0 };

	if (src_points.empty()) {
		return;
	}

	std::vector<Eigen::Vector3d> points;
	for (int i = 0; i < (int)src_points.size(); i++)
	{
		Eigen::Vector3d a_pt = Eigen::Vector3d(src_points[i].pt3D.x, src_points[i].pt3D.y, src_points[i].pt3D.z);
		points.push_back(a_pt);
	}

	Eigen::Vector3d axis, centroid;
	// --------------------------
	// 1. 计算质心 + 中心化
	// --------------------------
	centroid.setZero();
	for (const auto& p : points) centroid += p;
	centroid /= points.size();

	std::vector<Eigen::Vector3d> centered_pts;
	centered_pts.reserve(points.size());
	for (const auto& p : points)
	{
		Eigen::Vector3d a_pt = p - centroid;
		centered_pts.push_back(a_pt);
	}

	// --------------------------
	// 2. 构建 3x3 协方差矩阵
	// --------------------------
	Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
	for (const auto& p : centered_pts) cov += p * p.transpose();
	cov /= centered_pts.size() - 1;

	// --------------------------
	// 3. 特征值分解
	// --------------------------
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);
	Eigen::Vector3d eig_vals = solver.eigenvalues();   // 升序排列
	Eigen::Matrix3d eig_vecs = solver.eigenvectors();

	// --------------------------
	// 关键：残缺圆柱 → 取【最大】特征值对应的向量 = 轴向
	// --------------------------
	axis = eig_vecs.col(2);  // eigenvalues 升序，[2]最大
	axis.normalize();

	vec_axis.x = axis[0];
	vec_axis.y = axis[1];
	vec_axis.z = axis[2];

	vec_centroid.x = centroid[0];
	vec_centroid.y = centroid[1];
	vec_centroid.z = centroid[2];
	return;
}

// 功能：用 PCA 计算平面点云的法向量
// 输入：平面上的 3D 点云
// 输出：normal 法向量（单位化），centroid 平面中心
void computePlaneNormalByPCA(
	const std::vector<SVzNL3DPosition>& src_points,
	SVzNL3DPoint& vec_normal,
	SVzNL3DPoint& vec_centroid)
{
	vec_normal = { 0.0, 0.0, 0.0 };
	vec_centroid = { 0.0, 0.0, 0.0 };

	if (src_points.size() < 3) {
		return;
	}

	std::vector<Eigen::Vector3d> points;
	for (int i = 0; i < (int)src_points.size(); i++)
	{
		Eigen::Vector3d a_pt = Eigen::Vector3d(src_points[i].pt3D.x, src_points[i].pt3D.y, src_points[i].pt3D.z);
		points.push_back(a_pt);
	}

	Eigen::Vector3d normal, centroid;
	// 1. 去均值（中心化）
	centroid.setZero();
	for (const auto& p : points)
		centroid += p;
	centroid /= points.size();

	// 2. 构建协方差矩阵
	Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
	for (const auto& p : points) {
		Eigen::Vector3d dp = p - centroid;
		cov += dp * dp.transpose();
	}
	cov /= points.size() - 1;

	// 3. 特征值分解
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);
	Eigen::Vector3d eig_vals = solver.eigenvalues();   // 升序排列
	Eigen::Matrix3d eig_vecs = solver.eigenvectors();

	// 4. 关键：平面法向量 = 最小特征值对应的向量
	normal = eig_vecs.col(0); // 0 是最小，2 是最大
	normal.normalize();

	vec_normal.x = normal[0];
	vec_normal.y = normal[1];
	vec_normal.z = normal[2];

	vec_centroid.x = centroid[0];
	vec_centroid.y = centroid[1];
	vec_centroid.z = centroid[2];

	return;
}

// 二维PCA，求平面内条纹方向
void pca2D(const std::vector<SVzNL2DPointD>& src_pts, SVzNL2DPointD& axis, SVzNL2DPointD& centroid)
{

	centroid = { 0.0, 0.0 };
	axis = { 0.0, 0.0 };
#if 1
	if (src_pts.size() < 2)
		return;

	std::vector<Eigen::Vector2d> pts;
	for (int i = 0; i < (int)src_pts.size(); i++)
	{
		Eigen::Vector2d a_pt = Eigen::Vector2d(src_pts[i].x, src_pts[i].y);
		pts.push_back(a_pt);
	}

	int n = pts.size();
	Eigen::Vector2d center = Eigen::Vector2d::Zero();
	for (auto& p : pts) center += p;
	center /= n;

	Eigen::Matrix2d cov = Eigen::Matrix2d::Zero();
	for (auto& p : pts)
	{
		Eigen::Vector2d dp = p - center;
		cov += dp * dp.transpose();
	}
	cov /= (n - 1);

	Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(cov);
	// 最大特征向量 = 条纹延伸方向(2D)
	Eigen::Vector2d dir2d = solver.eigenvectors().col(1);

	centroid.x = center[0];
	centroid.y = center[1];
	axis.x = dir2d[0];
	axis.y = dir2d[1];
	return;

#else
	if (src_pts.size() < 3)
		return;

	// 1. 计算质心
	double meanX = 0.0, meanY = 0.0;
	for (const auto& p : src_pts)
	{
		meanX += p.x;
		meanY += p.y;
	}
	double n = (double)src_pts.size();
	meanX /= n;
	meanY /= n;

	// 2. 构造协方差矩阵
	double covXX = 0.0, covXY = 0.0, covYY = 0.0;
	for (const auto& p : src_pts)
	{
		double dx = p.x - meanX;
		double dy = p.y - meanY;
		covXX += dx * dx;
		covXY += dx * dy;
		covYY += dy * dy;
	}
	covXX /= n;
	covXY /= n;
	covYY /= n;

	// 3. 求解2阶协方差矩阵特征值 & 特征向量
	double a = covXX;
	double b = covXY;
	double c = covYY;

	double tr = a + c;
	double det = a * c - b * b;
	double lambda1 = tr * 0.5 + sqrt(tr * tr * 0.25 - det); // 大特征值
	// double lambda2 = tr * 0.5 - sqrt(tr * tr * 0.25 - det);

	// 主特征向量 (条纹方向)
	double dirX, dirY;
	if (fabs(b) > 1e-12)
	{
		dirX = lambda1 - c;
		dirY = b;
	}
	else
	{
		// 对角矩阵，主方向沿X
		dirX = 1.0;
		dirY = 0.0;
	}

	// 单位化
	double len = sqrt(dirX * dirX + dirY * dirY);
	if (len < 1e-12)
		return;
	axis.x = dirX / len;
	axis.y = dirY / len;
	centroid.x = meanX;
	centroid.y = meanY;
	return;
#endif
}