【C++】RANSAC 鲁棒拟合算法详解与实现

qq_51335499

349人浏览 · 2026-08-27 10:13:33

qq_51335499 · 2026-08-27 10:13:33 发布

【C++】RANSAC 鲁棒拟合算法详解与实现

博客长期更新，本文最近一次更新时间为：2026年8月27日。

一、概述

1、为什么需要 RANSAC

在计算机视觉和机器人领域，我们经常需要从一组观测数据中拟合几何模型（直线、平面、圆等）。最小二乘法是最经典的拟合方法，但它有一个致命缺陷：对异常值（外点）极其敏感。

举个例子：假设你有 100 个点，其中 90 个在一条直线上，10 个是噪声点。最小二乘会把这 10 个外点也纳入计算，导致拟合出的直线严重偏离真实模型。

RANSAC（Random Sample Consensus，随机抽样一致） 就是为了解决这个问题而生的。它是一种鲁棒拟合算法，能够在含有大量外点的数据中，准确估计出模型参数。

方法	对噪声的鲁棒性	适用场景
最小二乘	差 — 外点会严重拉偏结果	数据干净、噪声少
RANSAC	强 — 可承受 50%+ 外点率	含大量异常值的真实数据
Huber 损失	中 — 对小外点鲁棒	噪声较小但有少量离群点

2、核心思想

RANSAC 的核心思想非常朴素：

既然外点会干扰拟合，那我就随机猜一个模型，然后看有多少点"同意"这个模型。猜得次数足够多，总能找到一个让最多点同意的模型。

具体来说：

随机采样：每次随机选取最少数量的点（拟合直线需 2 点，圆需 3 点），用它们计算一个候选模型
内点投票：计算所有点到候选模型的距离，距离小于阈值的点称为"内点"（inlier），否则为"外点"（outlier）
保留最佳：如果当前模型的内点数量超过历史最佳，则更新最佳模型
迭代终止：迭代足够多次后，最佳模型即为最终结果

这个"猜"的过程看似低效，但通过数学推导可以计算出需要多少次迭代才能保证以很高的概率（如 99%）找到正确模型，这个次数通常远小于直觉预期。

3、典型应用场景

RANSAC 在视觉与机器人领域应用极为广泛：

直线检测：从边缘点中拟合直线（焊缝识别、车道线检测）
平面拟合：从点云中提取地面、墙面等平面
圆/圆弧拟合：从点云中检测圆孔、圆柱等特征
图像配准：基于特征点的单应矩阵/基础矩阵估计
点云配准：剔除错误对应点后进行 ICP 精配准
相机位姿估计：PnP 问题中的外点剔除

二、算法原理

1、RANSAC 基本流程

以拟合需要 s 个点的模型为例（直线 s=2，圆 s=3）：

输入：
  - data：观测数据点集
  - threshold：内点距离阈值
  - maxIter：最大迭代次数
  - p：期望置信度（通常 0.99）

输出：
  - bestModel：最佳模型参数
  - bestInliers：内点索引集合

算法：
  1. bestInliers = []
  2. for iter = 1 to maxIter:
  3.     随机选取 s 个不重复的点
  4.     用这 s 个点拟合候选模型 model
  5.     计算所有点到 model 的距离
  6.     inliers = { 距离 < threshold 的点 }
  7.     if |inliers| > |bestInliers|:
  8.         bestInliers = inliers
  9.         bestModel = model
 10.        根据当前内点率更新 maxIter（自适应）
 11. return bestModel, bestInliers

2、自适应迭代次数推导

RANSAC 最精妙的地方在于迭代次数可以通过数学公式自适应调整，而不是盲目迭代。

设：

p 为置信度（我们希望有 99% 的概率至少选到一次全内点样本）
e 为外点率（外点数 / 总点数）
s 为拟合模型所需的最少点数

那么，一次随机采样中 s 个点全是内点的概率为 (1 - e)^s。

一次采样失败（至少有一个外点）的概率为 1 - (1 - e)^s。

k 次采样全部失败的概率为 [1 - (1 - e)^s]^k。

我们要求成功概率至少为 p，即：

1 - [1 - (1 - e)^s]^k >= p

解出 k：

k >= log(1 - p) / log(1 - (1 - e)^s)

这就是 RANSAC 自适应迭代次数的核心公式。

在算法运行过程中，随着我们找到更好的模型（内点率更高），可以用当前最佳内点率来估算外点率 e，从而动态减少所需的迭代次数。

内点率 (1-e)	s=2 (直线)	s=3 (圆)
90%	2 次	4 次
80%	4 次	7 次
70%	7 次	16 次
50%	17 次	52 次
30%	38 次	223 次

注：以上为 p=0.99 时的最小迭代次数，可以看到即使外点率高达 50%，直线拟合也只需约 17 次迭代就能以 99% 的概率找到正确模型。

3、与最小二乘的对比

维度	最小二乘	RANSAC
原理	最小化所有点的残差平方和	寻找内点最多的模型
对外点	非常敏感 — 外点会严重拉偏结果	高度鲁棒 — 可承受 50%+ 外点
计算量	O(n)，一次求解	O(k * n)，k 次迭代
确定性	确定的 — 每次结果相同	随机的 — 结果有概率性波动
输出	唯一的模型参数	模型参数 + 内点/外点划分
适用场景	数据干净、高斯噪声	含大量异常值的真实数据

在实际工程中，最佳实践是 RANSAC + 最小二乘两阶段拟合：

第一阶段（RANSAC）：从含噪数据中筛选出内点，剔除外点
第二阶段（最小二乘）：只用内点进行精化拟合，得到更精确的模型

三、代码实现

1、Fisher-Yates 无重复随机采样

RANSAC 每次迭代需要从 N 个点中随机选取 s 个不重复的点。最简单的做法是用 rand() % N 循环取直到不重复，但这样在 s 接近 N 时效率极低。

Fisher-Yates 洗牌算法是标准解法：在数组中随机交换元素，前 s 个即为不重复的随机样本。

#include <random>
#include <vector>
#include <numeric>   // std::iota
#include <algorithm> // std::swap

// Fisher-Yates 局部洗牌：从 [0, n-1] 中随机选取 num 个不重复索引
// 结果写入 r_idx 数组（调用方需确保空间足够）
inline void getRandomSamples(int n, int num, int* r_idx)
{
    // 静态随机数生成器，只初始化一次
    // 注意：多线程环境下应改为 thread_local 或加锁
    static std::mt19937 gen(42);

    // 创建索引池 [0, 1, 2, ..., n-1]
    std::vector<int> pool(n);
    std::iota(pool.begin(), pool.end(), 0);

    // 局部洗牌：只洗前 num 个位置
    for (int i = 0; i < num; ++i)
    {
        // 从 [i, n-1] 中随机选一个位置与 i 交换
        std::uniform_int_distribution<int> dist(i, n - 1);
        int j = dist(gen);
        std::swap(pool[i], pool[j]);
        r_idx[i] = pool[i];
    }
}

为什么只洗前 num 个？

完整的 Fisher-Yates 需要 O(n) 空间和 O(n) 时间。但在 RANSAC 中，我们只需要 s 个样本（通常 s=2 或 3），而 n 可能成千上万。通过局部洗牌（只交换前 s 次），时间复杂度降为 O(s)，空间仍是 O(n)。

如果 n 非常大（百万级）且 s 很小，可以用哈希集合替代数组池，避免 O(n) 空间开销。

2、RANSAC 2D 直线拟合

二维直线是 RANSAC 最经典的应用场景。直线用一般式 Ax + By + C = 0 表示，归一化后 A² + B² = 1，此时点到直线的距离直接等于 |Ax + By + C|。

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <vector>
#include <cmath>
#include <algorithm>

// RANSAC 2D 直线拟合
// 输入：点集、距离阈值
// 输出：直线参数 (A, B, C) 满足 Ax+By+C=0 且 A²+B²=1，内点索引、外点索引、外点距离
cv::Vec3d ransac2DLineFit(const std::vector<cv::Point2f>& points,
                          std::vector<int>& inliers,
                          std::vector<int>& outliers,
                          std::vector<float>& outDist,
                          float distThreshold)
{
    inliers.clear();
    outliers.clear();
    outDist.clear();

    const int N = static_cast<int>(points.size());
    if (N < 2)
        return cv::Vec3d(0.0, 0.0, 0.0);

    const int maxIter = 1000;
    int       dynamicMaxIter = maxIter;
    int       bestInlierCount = 0;
    int       r_idx[2];

    std::vector<int> bestInliers, bestOutliers;
    std::vector<float> bestOutDist;

    for (int iter = 0; iter < dynamicMaxIter; ++iter)
    {
        // 步骤 1：随机选取 2 个不重复的点
        getRandomSamples(N, 2, r_idx);
        const cv::Point2f& p1 = points[r_idx[0]];
        const cv::Point2f& p2 = points[r_idx[1]];

        // 步骤 2：两点确定一条直线，计算单位方向向量
        cv::Point2f dir = p2 - p1;
        float len = std::hypot(dir.x, dir.y);
        if (len < 1e-6f) continue;   // 两点重合，跳过
        float ux = dir.x / len;
        float uy = dir.y / len;

        // 步骤 3：计算所有点到直线的距离（叉积法）
        // 距离 = |(p - p1) × 方向单位向量|
        std::vector<int> curInliers, curOutliers;
        std::vector<float> curOutDist;
        curInliers.reserve(N);
        curOutliers.reserve(N);
        curOutDist.reserve(N);

        for (int i = 0; i < N; ++i)
        {
            float dx = points[i].x - p1.x;
            float dy = points[i].y - p1.y;
            float cross = ux * dy - uy * dx;  // 2D 叉积 = 有向距离
            float dist = std::abs(cross);

            if (dist < distThreshold)
                curInliers.push_back(i);
            else
            {
                curOutliers.push_back(i);
                curOutDist.push_back(dist);
            }
        }

        // 步骤 4：更新最佳模型
        if (static_cast<int>(curInliers.size()) > bestInlierCount)
        {
            bestInlierCount = static_cast<int>(curInliers.size());
            bestInliers.swap(curInliers);
            bestOutliers.swap(curOutliers);
            bestOutDist.swap(curOutDist);

            // 步骤 5：自适应更新迭代次数
            // k = log(1 - p) / log(1 - (1 - e)^s)
            // 其中 p=0.99, s=2, e = 1 - inlierRatio
            float inlierRatio = std::clamp(
                static_cast<float>(bestInlierCount) / N, 0.01f, 0.9999f);
            float pBadSample = 1.0f - inlierRatio * inlierRatio;
            pBadSample = std::clamp(pBadSample, 1e-6f, 1.0f - 1e-6f);

            int needed = static_cast<int>(
                std::ceil(std::log(1.0f - 0.99f) / std::log(pBadSample)));
            dynamicMaxIter = std::min(needed, maxIter);
        }
    }

    if (bestInlierCount < 2)
        return cv::Vec3d(0.0, 0.0, 0.0);

    // 步骤 6：用内点做最小二乘精化
    std::vector<cv::Point2f> inlierPoints;
    inlierPoints.reserve(bestInliers.size());
    for (int idx : bestInliers)
        inlierPoints.push_back(points[idx]);

    cv::Vec4f line4f;
    cv::fitLine(inlierPoints, line4f, cv::DIST_L2, 0, 0.01, 0.01);
    // line4f = (vx, vy, x0, y0)：方向向量 (vx,vy)，过点 (x0,y0)

    // 转换为一般式 Ax + By + C = 0
    float vx = line4f[0], vy = line4f[1];
    float x0 = line4f[2], y0 = line4f[3];
    float A = vy;
    float B = -vx;
    float C = -(A * x0 + B * y0);

    // 归一化（使 A²+B²=1，便于直接用 Ax+By+C 计算距离）
    float norm = std::hypot(A, B);
    if (norm > 1e-8f)
    {
        A /= norm;
        B /= norm;
        C /= norm;
    }

    inliers = std::move(bestInliers);
    outliers = std::move(bestOutliers);
    outDist = std::move(bestOutDist);

    return cv::Vec3d(A, B, C);
}

关键实现细节：

技术点	说明
叉积求距离	2D 叉积的绝对值等于平行四边形面积，除以底边长即为高（点到直线距离）。方向向量单位化后，叉积值就等于距离
`swap` 替代赋值	用 `swap` 交换 vector，时间复杂度 O(1)，避免大数组拷贝
`std::clamp` 保护	防止 `log(0)` 和除零，内点率限制在 [0.01, 0.9999]
最小二乘精化	RANSAC 选出的模型由随机两点确定，存在误差。用全部内点做最小二乘拟合可显著提高精度

3、RANSAC 3D 直线拟合

3D 直线拟合的原理与 2D 完全相同，只是距离计算从 2D 叉积变成了 3D 叉积：

点 P 到直线（过点 A，方向向量为 v）的距离：
  d = |(P - A) × v| / |v|
当 v 单位化时，d = |(P - A) × v|

#include <opencv2/core.hpp>
#include <vector>
#include <cmath>
#include <algorithm>

// RANSAC 3D 直线拟合
// 输入：点集、距离阈值
// 输出：是否成功，直线参数（6维：方向向量+直线上一点），内点、外点、外点距离
bool ransac3DLineFit(const std::vector<cv::Point3f>& points,
                     std::vector<int>& inliers,
                     std::vector<int>& outliers,
                     std::vector<float>& outDist,
                     std::vector<float>& lineParams,
                     float distThreshold)
{
    if (points.size() < 2)
        return false;

    const int N = static_cast<int>(points.size());
    const int maxIter = 1000;
    int       dynamicMaxIter = maxIter;
    int       bestInlierCount = 0;
    int       r_idx[2];

    std::vector<int> bestInliers, bestOutliers;
    std::vector<float> bestOutDist;

    for (int iter = 0; iter < dynamicMaxIter; ++iter)
    {
        getRandomSamples(N, 2, r_idx);
        cv::Point3f p1 = points[r_idx[0]];
        cv::Point3f p2 = points[r_idx[1]];

        // 计算单位方向向量
        cv::Vec3f dir = p2 - p1;
        float dirLen = cv::norm(dir);
        if (dirLen < 1e-6f) continue;
        dir /= dirLen;

        // 计算所有点到直线的距离：|(p - p1) × dir|
        std::vector<int> curInliers, curOutliers;
        std::vector<float> curOutDist;
        curInliers.reserve(N);
        curOutliers.reserve(N);
        curOutDist.reserve(N);

        for (int i = 0; i < N; ++i)
        {
            cv::Vec3f vec = points[i] - p1;
            float dist = cv::norm(vec.cross(dir));  // 3D 叉积的模 = 距离

            if (dist < distThreshold)
                curInliers.push_back(i);
            else
            {
                curOutliers.push_back(i);
                curOutDist.push_back(dist);
            }
        }

        if (static_cast<int>(curInliers.size()) > bestInlierCount)
        {
            bestInlierCount = static_cast<int>(curInliers.size());
            bestInliers.swap(curInliers);
            bestOutliers.swap(curOutliers);
            bestOutDist.swap(curOutDist);

            // 自适应迭代次数（s=2）
            float inlierRatio = std::clamp(
                static_cast<float>(bestInlierCount) / N, 0.01f, 0.9999f);
            float pBad = 1.0f - inlierRatio * inlierRatio;
            pBad = std::clamp(pBad, 1e-6f, 1.0f - 1e-6f);

            int needed = static_cast<int>(
                std::ceil(std::log(1.0f - 0.99f) / std::log(pBad)));
            dynamicMaxIter = std::min(needed, maxIter);
        }
    }

    if (bestInlierCount < 2)
        return false;

    // 用内点做最小二乘精化
    std::vector<cv::Point3f> inlierPoints;
    inlierPoints.reserve(bestInliers.size());
    for (int idx : bestInliers)
        inlierPoints.push_back(points[idx]);

    cv::Vec6f refinedLine;
    cv::fitLine(inlierPoints, refinedLine, cv::DIST_L2, 0, 0.01, 0.01);

    lineParams = {
        refinedLine[0], refinedLine[1], refinedLine[2],  // 方向向量
        refinedLine[3], refinedLine[4], refinedLine[5]   // 直线上一点
    };

    inliers = std::move(bestInliers);
    outliers = std::move(bestOutliers);
    outDist = std::move(bestOutDist);

    return true;
}

4、三点求空间圆

空间圆拟合需要 3 个点来确定（3 点确定一个圆所在的平面，同时确定圆心和半径）。

数学推导：

三点 A、B、C 确定一个平面，法向量 N = AB × AC
圆心在 AB 的垂直平分面和 AC 的垂直平分面的交线上
圆心也在 ABC 平面内，因此可以在平面内求解

设圆心 O = A + s·AB + t·AC，其中 s、t 为待求参数。

由 |O - A| = |O - B| 得：(O - A)·AB = |AB|² / 2
由 |O - A| = |O - C| 得：(O - A)·AC = |AC|² / 2

代入 O 的表达式，得到关于 s、t 的二元一次方程组：

  2·AB·AB · s + 2·AB·AC · t = |AB|²
  2·AB·AC · s + 2·AC·AC · t = |AC|²

用克莱姆法则求解即可。

#include <opencv2/core.hpp>
#include <cmath>

struct Circle3D
{
    cv::Point3f center;   // 圆心
    cv::Point3f normal;   // 圆平面法向量（单位化）
    float       radius;   // 半径
};

// 三点求空间圆
// 返回值：true 表示求解成功，false 表示三点共线或退化
bool circleFrom3Points(const cv::Point3f& A,
                       const cv::Point3f& B,
                       const cv::Point3f& C,
                       Circle3D& circle)
{
    cv::Vec3f AB = B - A;
    cv::Vec3f AC = C - A;
    cv::Vec3f N  = AB.cross(AC);   // 平面法向量
    float nLen = cv::norm(N);
    if (nLen < 1e-6f) return false; // 三点共线，无法确定圆
    circle.normal = N / nLen;

    // 构建二元一次方程组：
    //   a11*s + a12*t = b1
    //   a12*s + a22*t = b2
    float a11 = 2.0f * AB.dot(AB);
    float a12 = 2.0f * AB.dot(AC);
    float a22 = 2.0f * AC.dot(AC);
    float b1  = AB.dot(AB);   // = |AB|²
    float b2  = AC.dot(AC);   // = |AC|²

    // 克莱姆法则
    float det = a11 * a22 - a12 * a12;
    if (std::abs(det) < 1e-10f * a11 * a22) return false;

    float s = (b1 * a22 - b2 * a12) / det;
    float t = (a11 * b2 - a12 * b1) / det;

    // 圆心 O = A + s*AB + t*AC
    circle.center = cv::Point3f(
        A.x + s * AB[0] + t * AC[0],
        A.y + s * AB[1] + t * AC[1],
        A.z + s * AB[2] + t * AC[2]);

    circle.radius = cv::norm(circle.center - A);
    return true;
}

// 点到空间圆的距离（综合考虑法向偏差与径向偏差）
float pointToCircleDist(const cv::Point3f& P, const Circle3D& c)
{
    cv::Vec3f CP = P - c.center;
    float       planeDist = CP.dot(c.normal);   // 法向距离
    cv::Point3f Pproj = P - planeDist * c.normal; // 投影到圆平面
    float       dr = cv::norm(Pproj - c.center) - c.radius; // 径向偏差
    // 合成距离 = sqrt(法向距离² + 径向偏差²)
    return std::sqrt(planeDist * planeDist + dr * dr);
}

点到圆的距离定义：

空间中一个点到圆的距离有多种定义方式，这里用合成距离：

法向分量：点到圆所在平面的距离
径向分量：点在平面上的投影到圆心的距离与半径之差

两者的平方和开根号即为合成距离。这种定义的好处是同时考虑了平面偏差和半径偏差，对 RANSAC 的内点划分更合理。

5、RANSAC 空间圆拟合

有了三点求圆和点到圆距离的工具函数，RANSAC 空间圆拟合的框架与直线拟合完全一致，只是每次采样 3 个点（s=3）。

#include <opencv2/core.hpp>
#include <vector>
#include <cmath>
#include <algorithm>

// RANSAC 空间圆拟合
// 输入：点集、半径预估（用于过滤不合理候选）、距离阈值
// 输出：是否成功，圆参数 (cx,cy,cz,nx,ny,nz,r)，内点、外点
bool ransac3DCircleFit(const std::vector<cv::Point3f>& points,
                       std::vector<int>& inliers,
                       std::vector<int>& outliers,
                       std::vector<float>& circleParams,
                       float expectedRadius,
                       float distThreshold)
{
    inliers.clear();
    outliers.clear();

    const int N = static_cast<int>(points.size());
    if (N < 3)
        return false;

    const int maxIter = 1000;
    const int minIter = 50;
    int       dynamicMaxIter = maxIter;
    int       r_idx[3];

    std::vector<int> bestInliers;
    Circle3D         bestCircle{};

    for (int iter = 0; iter < dynamicMaxIter; ++iter)
    {
        // 随机选取 3 个不重复的点
        getRandomSamples(N, 3, r_idx);

        Circle3D cand;
        if (!circleFrom3Points(points[r_idx[0]],
                               points[r_idx[1]],
                               points[r_idx[2]], cand))
            continue;

        // 预筛选：候选圆半径必须在合理范围内
        if (cand.radius < 0.1f * expectedRadius ||
            cand.radius > 3.0f * expectedRadius)
            continue;

        // 统计内点
        std::vector<int> curInliers;
        curInliers.reserve(N);
        for (int i = 0; i < N; ++i)
        {
            if (pointToCircleDist(points[i], cand) <= distThreshold)
                curInliers.push_back(i);
        }

        // 更新最佳模型
        if (curInliers.size() > bestInliers.size())
        {
            bestInliers = curInliers;
            bestCircle  = cand;

            // 自适应迭代次数（s=3）
            float inlierRatio = std::clamp(
                static_cast<float>(bestInliers.size()) / N, 0.01f, 0.9999f);
            float p3 = inlierRatio * inlierRatio * inlierRatio;
            float pBadSample = 1.0f - p3;
            pBadSample = std::clamp(pBadSample, 1e-6f, 1.0f - 1e-6f);

            int needed = static_cast<int>(
                std::ceil(std::log(0.01f) / std::log(pBadSample)));
            dynamicMaxIter = std::clamp(needed, minIter, maxIter);
        }
    }

    if (static_cast<int>(bestInliers.size()) < 3)
        return false;

    // ---- 最小二乘精化 ----
    // 构建圆平面的局部坐标系 (e1, e2, n)
    cv::Vec3f n = bestCircle.normal;
    cv::Vec3f ref = (std::abs(n[0]) < 0.9f) ?
        cv::Vec3f(1, 0, 0) : cv::Vec3f(0, 1, 0);
    cv::Vec3f nc = n.cross(ref);
    float ncLen = cv::norm(nc);
    cv::Vec3f e1 = (ncLen > 1e-6f) ? nc / ncLen : cv::Vec3f(1, 0, 0);
    cv::Vec3f e2 = n.cross(e1);

    // 将内点投影到圆平面，以圆心为原点建立 2D 坐标系
    int K = static_cast<int>(bestInliers.size());
    cv::Mat A(K, 3, CV_32F), b(K, 1, CV_32F);
    for (int i = 0; i < K; ++i)
    {
        cv::Vec3f CP = points[bestInliers[i]] - bestCircle.center;
        float u = CP.dot(e1);
        float v = CP.dot(e2);
        A.at<float>(i, 0) = 2.0f * u;
        A.at<float>(i, 1) = 2.0f * v;
        A.at<float>(i, 2) = 1.0f;
        b.at<float>(i, 0) = u * u + v * v;
    }

    // 最小二乘求解圆方程：u² + v² + D*u + E*v + F = 0
    // 整理为：2u·D' + 2v·E' + 1·F' = u² + v²
    cv::Mat sol;
    cv::solve(A, b, sol, cv::DECOMP_SVD);
    float cu = sol.at<float>(0);  // = -D/2
    float cv_ = sol.at<float>(1); // = -E/2
    float c = sol.at<float>(2);   // = F

    float r2 = cu * cu + cv_ * cv_ + c;
    if (r2 <= 0.0f)
        return false;

    // 圆心从 2D 局部坐标映射回 3D
    bestCircle.center += cv::Point3f(cu * e1 + cv_ * e2);
    bestCircle.radius = std::sqrt(r2);

    // 精化后检查半径合法性
    if (bestCircle.radius < 0.1f * expectedRadius ||
        bestCircle.radius > 3.0f * expectedRadius)
        return false;

    // 精化后重新划分内外点
    for (int i = 0; i < N; ++i)
    {
        if (pointToCircleDist(points[i], bestCircle) <= distThreshold)
            inliers.push_back(i);
        else
            outliers.push_back(i);
    }

    circleParams = {
        bestCircle.center.x, bestCircle.center.y, bestCircle.center.z,
        bestCircle.normal.x, bestCircle.normal.y, bestCircle.normal.z,
        bestCircle.radius
    };

    return true;
}

最小二乘精化的原理：

RANSAC 选出的 3 点模型有随机性，精度不够。用所有内点做最小二乘精化可以显著提高精度。

由于空间圆是三维的，直接拟合比较复杂，工程上常用的方法是降维到 2D：

建立圆平面的局部坐标系 (e1, e2, n)
将所有内点投影到圆平面，得到 2D 坐标 (u, v)
在 2D 平面内用最小二乘拟合圆（展开为线性方程求解）
将 2D 圆心映射回 3D 空间

圆方程展开后的线性形式：u² + v² + D·u + E·v + F = 0

整理为 Ax = b 的形式：

A 的每行：[2u, 2v, 1]
x：[D/2, E/2, F]
b 的每行：u² + v²

用 SVD 求解超定方程组即可。

6、最小二乘空间圆精化

如果数据比较干净（外点少），可以直接用最小二乘法拟合空间圆，省去 RANSAC 的迭代开销。完整流程：PCA 求平面 → 投影到 2D → 最小二乘拟合圆 → 映射回 3D。

#include <opencv2/core.hpp>
#include <vector>
#include <cmath>

// 最小二乘空间圆拟合（数据较干净时使用）
// 输入：点集、半径预估（用于合理性检查）
// 输出：圆参数 (cx,cy,cz,nx,ny,nz,r)
bool leastSquare3DCircleFit(const std::vector<cv::Point3f>& points,
                            std::vector<float>& circleParams,
                            float expectedRadius)
{
    const int N = static_cast<int>(points.size());
    if (N < 3)
        return false;

    // -------- 第一步：PCA 拟合平面 --------
    // 计算质心
    cv::Point3f centroid(0.0f, 0.0f, 0.0f);
    for (const auto& p : points)
    {
        centroid.x += p.x;
        centroid.y += p.y;
        centroid.z += p.z;
    }
    centroid.x /= N;
    centroid.y /= N;
    centroid.z /= N;

    // 构建 3x3 协方差矩阵
    cv::Mat cov = cv::Mat::zeros(3, 3, CV_32F);
    for (const auto& p : points)
    {
        cv::Mat d = (cv::Mat_<float>(3, 1) <<
            p.x - centroid.x,
            p.y - centroid.y,
            p.z - centroid.z);
        cov += d * d.t();
    }
    cov /= N;

    // SVD 分解，最小奇异值对应的列向量即为法向量
    cv::Mat w, u, vt;
    cv::SVD::compute(cov, w, u, vt);
    cv::Vec3f normal(u.at<float>(0, 2),
                     u.at<float>(1, 2),
                     u.at<float>(2, 2));

    // -------- 第二步：构建平面局部坐标系 --------
    cv::Vec3f ex, ey;
    cv::Vec3f ref(1.0f, 0.0f, 0.0f);
    if (std::abs(normal.dot(ref)) > 0.9f)
        ref = cv::Vec3f(0.0f, 1.0f, 0.0f);

    ex = normal.cross(ref);
    ex /= cv::norm(ex);
    ey = normal.cross(ex);
    ey /= cv::norm(ey);

    // -------- 第三步：投影到 2D --------
    std::vector<cv::Point2f> pts2d;
    pts2d.reserve(N);
    for (const auto& p : points)
    {
        cv::Vec3f dp = p - centroid;
        pts2d.emplace_back(dp.dot(ex), dp.dot(ey));
    }

    // -------- 第四步：2D 最小二乘圆拟合 --------
    // 圆方程：x² + y² + Dx + Ey + F = 0
    cv::Mat A = cv::Mat::zeros(N, 3, CV_32F);
    cv::Mat b = cv::Mat::zeros(N, 1, CV_32F);

    for (int i = 0; i < N; ++i)
    {
        float x = pts2d[i].x;
        float y = pts2d[i].y;
        A.at<float>(i, 0) = x;
        A.at<float>(i, 1) = y;
        A.at<float>(i, 2) = 1.0f;
        b.at<float>(i, 0) = -(x * x + y * y);
    }

    cv::Mat result;
    if (!cv::solve(A, b, result, cv::DECOMP_SVD))
        return false;

    float D = result.at<float>(0, 0);
    float E = result.at<float>(1, 0);
    float F = result.at<float>(2, 0);

    // 圆心：(-D/2, -E/2)，半径：sqrt(D²/4 + E²/4 - F)
    float cx2d = -D * 0.5f;
    float cy2d = -E * 0.5f;
    float r2 = cx2d * cx2d + cy2d * cy2d - F;

    if (r2 <= 0.0f)
        return false;

    float radius = std::sqrt(r2);

    // 半径合理性检查
    if (radius < 0.2f * expectedRadius || radius > 1.8f * expectedRadius)
        return false;

    // -------- 第五步：2D 圆心映射回 3D --------
    cv::Point3f center3d;
    center3d.x = centroid.x + cx2d * ex[0] + cy2d * ey[0];
    center3d.y = centroid.y + cx2d * ex[1] + cy2d * ey[1];
    center3d.z = centroid.z + cx2d * ex[2] + cy2d * ey[2];

    circleParams = {
        center3d.x, center3d.y, center3d.z,
        normal[0],  normal[1],  normal[2],
        radius
    };

    return true;
}

四、参数调优指南

1、距离阈值 distThreshold

距离阈值是 RANSAC 最重要的参数，决定了内点和外点的划分标准。

设置过小	设置过大
内点率低，需要更多迭代	外点也被当作内点，模型精度下降
可能无法找到有效模型	鲁棒性下降

经验调参方法：

基于数据精度：知道测量精度（如点云精度 ±0.5mm），阈值设为 2~3 倍精度（1.0~1.5mm）
基于先验知识：已知模型大致参数，统计点到模型的距离分布，取合适的分位数
自适应阈值：先粗跑一次 RANSAC，根据内点的平均残差调整阈值，再精跑一次

2、最大迭代次数 maxIter

maxIter 是迭代次数的上限，防止在内点率极低时无限迭代。

外点率	直线 (s=2)	圆 (s=3)	推荐 maxIter
10%	2 次	4 次	100
30%	7 次	16 次	200
50%	17 次	52 次	500
70%	38 次	223 次	1000
90%	114 次	1660 次	2000

由于有自适应迭代机制，maxIter 设大一些不会影响性能（实际迭代次数通常远小于上限），建议默认设 1000。

3、置信度与内点率

置信度 p：通常取 0.99，表示我们希望有 99% 的概率至少选到一次全内点样本
内点率 (1-e)：自适应更新，从当前最佳模型的内点比例估算
初始内点率：算法开始时不知道内点率，所以用 maxIter 作为初始迭代次数

4、不同场景参数参考

场景	distThreshold	maxIter	expectedRadius
2D 边缘点直线拟合（像素级）	1.0~2.0 px	1000	-
3D 点云直线拟合（毫米级）	0.5~2.0 mm	1000	-
3D 点云圆孔检测	0.3~1.0 mm	1000	已知孔径/2
焊缝圆弧特征提取	0.5~1.5 mm	1000	预估圆弧半径

五、进阶技巧

1、预筛选加速

当数据量很大（百万级点云）时，每次迭代遍历所有点计算距离很慢。可以采用预筛选策略：

随机子集法：每次迭代只随机选一小部分点（如 10%）来评估候选模型，只有模型看起来不错时才用全部点验证
空间索引：用 kd-tree 或八叉树加速最近邻搜索，减少距离计算量
提前终止：计算距离时实时统计内点数量，如果已经不可能超过当前最佳，提前终止本次迭代

2、最小二乘精化

RANSAC 的输出是由最少数量的点确定的模型，精度有限。务必加上最小二乘精化步骤：

RANSAC 粗拟合 → 划分内点 → 内点最小二乘精化 → 重新划分内点

精化后的模型精度通常可以提升一个数量级。

对于空间圆等复杂模型，甚至可以做迭代精化：用精化后的模型重新划分内点，再用新内点拟合，反复几次直到收敛。

3、多尺度 RANSAC

当不知道距离阈值应该设多大时，可以用多尺度策略：

先设一个较小的阈值，跑 RANSAC
如果找到的内点太少，逐步增大阈值
每次都用当前最佳模型作为初始值

或者反过来，从大阈值开始，找到模型后逐步收紧阈值，提高精度。

4、并行化思路

RANSAC 的迭代之间是完全独立的，天然适合并行化：

数据并行：将迭代分配给多个线程，每个线程独立执行 RANSAC，最后汇总最佳模型
距离计算并行：单轮迭代中，所有点到模型的距离计算可并行

注意事项：

随机数生成器需要每个线程独立（用 thread_local 替代 static）
最佳模型的更新需要加锁或用原子操作
自适应迭代次数在并行下需要重新设计（因为各线程看不到彼此的最佳结果）

5、常见坑点与解决方案

坑点	现象	解决方案
采样点重合	拟合失败，迭代浪费	加距离检查，重合则跳过本次迭代
退化构型（三点共线）	圆拟合失败	检测三点共线（法向量接近零），跳过
静态随机种子	每次结果完全一样	用 `std::random_device` 替代固定种子
多线程不安全	数据竞争、崩溃	用 `thread_local` 随机数生成器
半径预筛选过严	找不到有效模型	放宽半径范围，比如 0.1~3.0 倍
内点率极低时死循环	迭代次数过多	设置 maxIter 上限保护
SVD 求解失败	最小二乘精化崩溃	检查 A 矩阵秩，点数不足直接返回失败
法向量方向不稳定	法向量每次方向可能相反	统一法向量朝向（如保证 z 分量为正）

六、参考文献

Fischler M A, Bolles R C. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24(6): 381-395.
Hartley R, Zisserman A. Multiple View Geometry in Computer Vision[M]. Cambridge University Press, 2003.
OpenCV Documentation: fitLine function
PCL Documentation: SampleConsensusModel classes
RANSAC - Wikipedia
Fisher-Yates Shuffle - Wikipedia