最小二乘法擬合直線簇交點及Ransac擬合

• 最小二乘法的實現
• Ransac優化

語言環境：Python
直線簇方程： $y=p+v?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拟合的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。