637 lines
16 KiB
C++
637 lines
16 KiB
C++
#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;
|
||
}
|
||
|
||
//圆最小二乘拟合
|
||
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;
|
||
}
|
||
|
||
#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::Point3f> 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::Point3f& p, const Plane& plane) {
|
||
return (plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D);
|
||
}
|
||
|
||
// 点到平面的距离（绝对值）
|
||
float pointToPlaneDistance(const cv::Point3f& p, const Plane& plane) {
|
||
return fabsf(plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D)
|
||
/ sqrtf(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::Point3f>& 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::Point3f& p1, const cv::Point3f& p2, const cv::Point3f& p3) {
|
||
float v1x = p2.x - p1.x;
|
||
float v1y = p2.y - p1.y;
|
||
float v1z = p2.z - p1.z;
|
||
|
||
float v2x = p3.x - p1.x;
|
||
float v2y = p3.y - p1.y;
|
||
float v2z = p3.z - p1.z;
|
||
|
||
float A = v1y * v2z - v1z * v2y;
|
||
float B = v1z * v2x - v1x * v2z;
|
||
float C = v1x * v2y - v1y * v2x;
|
||
float D = -(A * p1.x + B * p1.y + C * p1.z);
|
||
|
||
float 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::Point3f>& points,
|
||
std::vector<cv::Point3f>& out_inliers,
|
||
float dist_thresh, // 内点距离阈值
|
||
int max_iter, // 最大迭代
|
||
int stop_no_improve // 连续多少次无提升就提前退出
|
||
)
|
||
{
|
||
out_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)
|
||
out_inliers.push_back(p);
|
||
}
|
||
|
||
return best_plane;
|
||
} |