二维点拟合直线ransac c++
需求 ： 用C++实现一堆离散的二维点拟合出来一条直线
理论
直线公式：
a x + b y + c = 0 ax+by+c=0 ax+by+c=0
- 随机选两个点，构造一般式直线：
a x + b y + c = 0 ax+by+c=0 ax+by+c=0
方向向量：
d = ( x 2 − x 1 , y 2 − y 1 ) d = (x2-x1,y2-y1) d=(x2−x1,y2−y1)
法向量：
n = ( − d y , d x ) n = (-d_y,d_x) n=(−dy,dx)
在直线的“一般式方程”中：
ax+by+c=0
这个 (a,b) 实际上就是直线的法向量
也就是
a = y 2 − y 1 b = x 2 − x 1 c = − a x 1 − b y 1 a = y2-y1 \\ b= x2-x1\\c = -ax1-by1 a=y2−y1b=x2−x1c=−ax1−by1
- 计算所有点到直线的距离：
d i = ∣ a x i + b y i + c ∣ a 2 + b 2 d_i = \frac{|ax_i + by_i + c|}{\sqrt{a^2+b^2}} di=a2+b2∣axi+byi+c∣
-
判断是否为内点（小于阈值）
-
重复若干次，选出包含最多内点的模型
若：
s 是一次采样所需点数（拟合直线是 2）
p 是样本中内点的比例
k 是需要的置信度（如 0.99）
则最少采样次数
N 应满足：
1 − ( 1 − p s ) N > = k = = > N > = l o g ( 1 − k ) 1 − P s 1 - ( 1 - p^{s} ) ^N >= k ==> N>= \frac{log(1-k)}{1-P^s} 1−(1−ps)N>=k==>N>=1−Pslog(1−k)
这可以根据实际数据调整迭代次数。
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;}}
}