最小二乘法拟合直线簇交点及Ransac拟合

• 最小二乘法的实现
• Ransac优化

语言环境：Python
直线簇方程： $y=p+v\ast t$
其中 $p$表示直线上一点P的坐标
原理参考 Line–line intersection及 Stack Overflow.

最小二乘法的实现

根据参考链接中的原理给出如下公式：

$x$为拟合结果。
代码如下：

import numpy as np import mathdef Intersection(LineD, PList):l = len(LineD)I = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])q = np.array([0, 0, 0])Msum = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])for j in range(0,l):Lnormal = np.array([-LineD[l]/np.linalg.norm(LineD[l])])p = PList[l][:3]viviT = Lnormal*Lnormal.TM = I - viviTMsum = Msum + Mq = q + np.dot(M,p)q = q.Tlp = np.linalg.lstsq(Msum, q, rcond=None)[0]return lp

其中，输入的LineD为直线方向的List，格式为：[[x1,y1,z1], [x2,y2,z2],....]，P为直线上一点的坐标，格式与LineD相同。输出的lp为三维坐标。

Ransac优化

Ransac原理见链接。
主要实现步骤有：

• 随机抽取num条直线
• 最小二乘法算该组直线交点
• 计算距离交点在阈值t以内的直线个数count
• 重复抽取计算步骤n次，并选择所有结果中count最大时的交点并返回
• 为了使结果鲁棒，对所得结果内符合阈值条件内的直线再次计算交点，并与原结果比较，直到结果不再变化为止

代码如下：

import numpy as np import math import randomdef RansacIntersection(LineD, PList, n, num, t):# LineD is the list of Light Direction# PList is the list of P position# n : times of sampling# num: number of lines during a sampling# t: threshold of distance to examine the number of lines around the point (unit: mm)l = len(LineD)I = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])maxlines = 0reallp = []for i in range(0,n):# pick up num line randomly# calculate the intersection point# calculate the distance between the point and lines# count the number of lines around the pointq = np.array([0, 0, 0])Msum = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])flag = random.sample(range(l), num)for j in range(0,num):flagj = flag[j]Lnormal = np.array([-LineD[flagj]/np.linalg.norm(LineD[flagj])])p = PList[flagj][:3]viviT = Lnormal*Lnormal.TM = I - viviTMsum = Msum + Mq = q + np.dot(M,p)q = q.Tlp = np.linalg.lstsq(Msum, q, rcond=None)[0]count = 0for j in range(0, l):Lnormal = np.array(-LineD[j] / np.linalg.norm(LineD[j]))p = PList[j][:3]P2L = np.array(lp - p)d = np.sqrt(1-(np.dot(Lnormal, P2L)/np.linalg.norm(P2L))**2)*np.linalg.norm(P2L)# print 1-(np.dot(Lnormal, P2L)/np.linalg.norm(P2L))**2# print dif d < t:count = count + 1# print count# print "****"if count > maxlines:maxlines = countreallp = lplp0 = [[],[],[]]lp = reallp# print lp# print lp[0], lp[1], lp[2]while (not((lp0[0] == lp[0]) and (lp0[1] == lp[1]) and (lp0[2] == lp[2]))):count = 0# print "@@@@@@"q = np.array([0, 0, 0])Msum = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])for j in range(0, l):Lnormal = np.array(-LineD[j] / np.linalg.norm(LineD[j]))p = PList[j][:3]P2L = np.array(lp - p)d = np.sqrt(1 - (np.dot(Lnormal, P2L) / np.linalg.norm(P2L)) ** 2) * np.linalg.norm(P2L)if d < t:# print "****"count = count+1Lnormal = np.array([-LineD[j] / np.linalg.norm(LineD[j])])p = PList[j][:3]viviT = Lnormal * Lnormal.TM = I - viviTMsum = Msum + Mq = q + np.dot(M, p)q = q.Tlp0 = lplp = np.linalg.lstsq(Msum, q, rcond=None)[0]# print lpreallp = lpprint (count)# print "%%%%%"return reallp

第一次发博，不足之处请多多指教，如有问题欢迎讨论。

（转载请标明出处）

总结

以上是生活随笔为你收集整理的最小二乘法拟合直线簇交点及Ransac拟合的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。