通俗易懂理解RANSAC算法

RANSAC(Random Sample Consensus)，翻译为随机抽样一致算法。

算法思路：从所有观测中随机找到几个尽可能少的点去拟合模型，拟合后依次计算模型和所有观测数据的残差，当残差小于给定的阈值时，就将其判断为内点，大于给定的阈值时，就判断为外点，并统计内点的数量，然后再次随机选取几个点拟合模型迭代。如果本次拟合内点数量大于先前的模型，就将旧模型迭代为新的模型。

做过AMCL算法的同学是不是秒懂了，感觉就是一个低配版的粒子滤波。

直接上代码，以我正在做的项目中的一环进行代码分享（三维平面的拟合）。

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <limits>
#include <random>
#include <vector>

#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
using namespace std;
using namespace cv;
using namespace Eigen;

struct Point3D {
    double x, y, z;
};

double distance(const Point3D& p1, const Point3D& p2) {
    return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2) +
                     std::pow(p1.z - p2.z, 2));
}

Point3D ransacPlaneFitting(const std::vector<Point3D>& points, int max_iterations,
                           double distance_threshold) {
    int n = points.size();
    // 生成随机数
    std::random_device rd;  // 用于随机数引擎获得随机种子
    std::mt19937 gen(rd()); // 以rd()为种子的标准mersenne_twister_engine
    std::uniform_int_distribution<> dis(0, n - 1); // 限制随机数范围

    Point3D best_plane{0, 0, 0};
    int best_inliers = 0;

    // 一直迭代
    for (int i = 0; i < max_iterations; ++i) {
        int idx1 = dis(gen);
        int idx2 = dis(gen);
        int idx3 = dis(gen);

        Point3D p1 = points[idx1];
        Point3D p2 = points[idx2];
        Point3D p3 = points[idx3];

        double a = (p2.y - p1.y) * (p3.z - p1.z) - (p2.z - p1.z) * (p3.y - p1.y);
        double b = (p2.z - p1.z) * (p3.x - p1.x) - (p2.x - p1.x) * (p3.z - p1.z);
        double c = (p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x);
        double d = -a * p1.x - b * p1.y - c * p1.z;

        int inliers = 0;
        double m_dist = std::sqrt(a * a + b * b + c * c);
        for (const auto& point : points) {
            double dist = std::abs(a * point.x + b * point.y + c * point.z + d) / m_dist;
            if (dist < distance_threshold) {
                ++inliers;
            }
        }

        if (inliers > best_inliers) {
            best_inliers = inliers;
            best_plane = {a, b, c};
        }
    }

    return best_plane;
}

int main() {
    int png_size = 4;
    // TODO: 需要输入一个三维点集
    std::vector<Point3D> points;
    int max_iterations = 1000;
    double distance_threshold = 0.05;
    Point3D plane = ransacPlaneFitting(points, max_iterations, distance_threshold);
    // 输出平面的 ABC值
    std::cout << "Best plane: (" << plane.x << ", " << plane.y << ", " << plane.z << ")"

    return 0;
}