二维点拟合直线ransac c++

需求 ： 用C++实现一堆离散的二维点拟合出来一条直线

理论

直线公式：
$$ax+by+c=0$$

1. 随机选两个点，构造一般式直线：
   $$ax+by+c=0$$

方向向量：
$$d=(x2-x1,y2-y1)$$
法向量：
$$n=(-d_y,d_x)$$

在直线的“一般式方程”中：
ax+by+c=0
这个 (a,b) 实际上就是直线的法向量

也就是
$$\begin{gathered} a=y2-y1 \\ b=x2-x1 \\ c=-ax1-by1 \end{gathered}$$

2. 计算所有点到直线的距离：

$$d_i=\frac{|a{x}_i+b{y}_i+c|}{\sqrt{a^2+b^2}}$$

3. 判断是否为内点（小于阈值）

4. 重复若干次，选出包含最多内点的模型

若：

s 是一次采样所需点数（拟合直线是 2）
p 是样本中内点的比例
k 是需要的置信度（如 0.99）
则最少采样次数
N 应满足：

$$1-(1-p^s{)}^N>=k==>N>=\frac{log(1-k)}{1-P^s}$$
​
这可以根据实际数据调整迭代次数。

Code

//point2ds输入 iterations迭代次数 bestliner输出最佳的点数，besta，bestb，bestc 直线的参数
void testransac2D(std::vector<Eigen::Vector2d> point2ds, int iterations, int& bestliner, int& besta, int& bestb, int& bestc) {
    //生成随机数
    std::random_device rand;
    std::mt19937 gen(rand());
    std::uniform_int_distribution<> distribution(0, point2ds.size() - 1);
    for (int i = 0; i < iterations; i++) {
        int id1 = distribution(gen);
        int id2 = distribution(gen);

        if (id1 == id2) continue;

        Eigen::Vector2d point1 = point2ds[id1];
        Eigen::Vector2d point2 = point2ds[id2];

        //计算直线
        int a = point1.y() - point2.y();
        int b = point1.x() - point2.y();

        int c = -a * point1.x() - b * point1.y();
        double thr = 10.;
        int liner = 0;

        //计算直线距离 算出内点数量最多的那个
        for (auto& point : point2ds) {
            double dis = abs(a * point.x() + b * point.y() + c) / sqrt(a * a + b * b);

            if (dis < thr) {
                liner++;
            }
        }

        if (liner > bestliner) {
            bestliner = liner;
            besta = a;
            bestb = b;
            bestc = c;
        }
    }
}