PCL之RANSAC实践

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <limits>
#include <iomanip>

struct Point2D
{
    double x;
    double y;
};

struct LineModel
{
    // 直线一般式: ax + by + c = 0
    double a = 0.0;
    double b = 0.0;
    double c = 0.0;
    bool valid = false;
};

struct RansacResult
{
    LineModel bestModel;
    std::vector<int> bestInlierIndices;
};

static const int kNumInliers = 80;
static const int kNumOutliers = 30;
static const double kTrueSlope = 2.0;    // 理想直线方程
static const double kTrueIntercept = 1.0;
static const double kNoiseSigma = 0.15;

static const int kMaxIterations = 200;
static const double kDistanceThreshold = 0.20;
static const unsigned int kRandomSeed = 42;

// ========== 数学工具 ==========

double DistancePointToLine(const Point2D& p, const LineModel& line)
{
    double denom = std::sqrt(line.a * line.a + line.b * line.b);
    if (denom < 1e-12)
    {
        return std::numeric_limits<double>::max();
    }
    return std::fabs(line.a * p.x + line.b * p.y + line.c) / denom;
}

LineModel BuildLineFromTwoPoints(const Point2D& p1, const Point2D& p2)
{
    LineModel line;

    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;

    // 两点过近，认为退化
    if (std::sqrt(dx * dx + dy * dy) < 1e-12)
    {
        line.valid = false;
        return line;
    }

    line.a = p1.y - p2.y;
    line.b = p2.x - p1.x;
    line.c = p1.x * p2.y - p2.x * p1.y;
    line.valid = true;
    return line;
}

// 使用最小二乘拟合 y = kx + b
// 注意：这个版本适合非竖直直线，用来做本实验的精拟合足够了
bool FitLineLeastSquares(const std::vector<Point2D>& points, const std::vector<int>& indices, double& k, double& b)
{
    if (indices.size() < 2)
    {
        return false;
    }

    double sumX = 0.0;
    double sumY = 0.0;
    double sumXX = 0.0;
    double sumXY = 0.0;
    const int n = static_cast<int>(indices.size());

    for (int idx : indices)
    {
        const Point2D& p = points[idx];
        sumX += p.x;
        sumY += p.y;
        sumXX += p.x * p.x;
        sumXY += p.x * p.y;
    }

    double denom = n * sumXX - sumX * sumX;
    if (std::fabs(denom) < 1e-12)
    {
        return false;
    }

    k = (n * sumXY - sumX * sumY) / denom;
    b = (sumY - k * sumX) / n;
    return true;
}

// ========== 数据生成 ==========

std::vector<Point2D> GenerateTestData()
{
    std::vector<Point2D> points;
    points.reserve(kNumInliers + kNumOutliers);

    std::mt19937 rng(kRandomSeed);
    std::uniform_real_distribution<double> xDist(-5.0, 5.0);
    std::normal_distribution<double> noiseDist(0.0, kNoiseSigma);
    std::uniform_real_distribution<double> outlierDist(-8.0, 8.0);

    // 生成内点：落在真实直线附近
    for (int i = 0; i < kNumInliers; ++i)
    {
        double x = xDist(rng);
        double y = kTrueSlope * x + kTrueIntercept + noiseDist(rng);
        points.push_back({x, y});
    }

    // 生成外点：随机散布
    for (int i = 0; i < kNumOutliers; ++i)
    {
        double x = outlierDist(rng);
        double y = outlierDist(rng);
        points.push_back({x, y});
    }

    return points;
}

// ========== RANSAC 主体 ==========

RansacResult RunRansacLine2D(const std::vector<Point2D>& points)
{
    RansacResult result;

    if (points.size() < 2)
    {
        return result;
    }

    std::mt19937 rng(kRandomSeed + 123);
    std::uniform_int_distribution<int> idxDist(0, static_cast<int>(points.size()) - 1);

    int bestInlierCount = -1;

    for (int iter = 0; iter < kMaxIterations; ++iter)
    {
        // 1. 随机取两个不同点
        int idx1 = idxDist(rng);
        int idx2 = idxDist(rng);
        while (idx2 == idx1)
        {
            idx2 = idxDist(rng);
        }

        const Point2D& p1 = points[idx1];
        const Point2D& p2 = points[idx2];

        // 2. 构造候选直线
        LineModel candidate = BuildLineFromTwoPoints(p1, p2);
        if (!candidate.valid)
        {
            continue;
        }

        // 3. 统计内点
        std::vector<int> inliers;
        for (int i = 0; i < static_cast<int>(points.size()); ++i)
        {
            double dist = DistancePointToLine(points[i], candidate);
            if (dist < kDistanceThreshold)
            {
                inliers.push_back(i);
            }
        }

        // 4. 更新最优模型
        if (static_cast<int>(inliers.size()) > bestInlierCount)
        {
            bestInlierCount = static_cast<int>(inliers.size());
            result.bestModel = candidate;
            result.bestInlierIndices = inliers;
        }
    }

    return result;
}

// ========== 主函数 ==========

int main()
{
    std::vector<Point2D> points = GenerateTestData();

    RansacResult ransacResult = RunRansacLine2D(points);

    std::cout << std::fixed << std::setprecision(6);
    std::cout << "总点数: " << points.size() << std::endl;
    std::cout << "RANSAC 最佳内点数: " << ransacResult.bestInlierIndices.size() << std::endl;

    if (ransacResult.bestModel.valid)
    {
        std::cout << "\nRANSAC 粗模型 (ax + by + c = 0):" << std::endl;
        std::cout << "a = " << ransacResult.bestModel.a << std::endl;
        std::cout << "b = " << ransacResult.bestModel.b << std::endl;
        std::cout << "c = " << ransacResult.bestModel.c << std::endl;
    }

    double k = 0.0;
    double b = 0.0;
    bool ok = FitLineLeastSquares(points, ransacResult.bestInlierIndices, k, b);

    if (ok)
    {
        std::cout << "\n基于最佳内点集做最小二乘精拟合:" << std::endl;
        std::cout << "y = " << k << " * x + " << b << std::endl;
    }
    else
    {
        std::cout << "\n精拟合失败。" << std::endl;
    }

    std::cout << "\n真实直线:" << std::endl;
    std::cout << "y = " << kTrueSlope << " * x + " << kTrueIntercept << std::endl;

    return 0;
}