最小二乘法拟合直线簇交点及Ransac拟合最小二乘法的实现Ransac优化

93 阅读 0 评论 62 点赞

我是靠谱客的博主热心帅哥，这篇文章主要介绍最小二乘法拟合直线簇交点及Ransac拟合最小二乘法的实现Ransac优化，现在分享给大家，希望可以做个参考。

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

最小二乘法的实现
Ransac优化

语言环境：Python
直线簇方程：

y = p + v * t

其中

p

表示直线上一点P的坐标
原理参考 Line–line intersection及 Stack Overflow.

最小二乘法的实现

根据参考链接中的原理给出如下公式：
在这里插入图片描述
$x$ 为拟合结果。
代码如下：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
import numpy as np
import math

def 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.T
		M = I - viviT
		Msum = Msum + M
		q = q + np.dot(M,p)
	q = q.T
	lp = 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 random

def 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 = 0
    reallp = []

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 point

q = 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.T
            M = I - viviT
            Msum = Msum + M
            q = q + np.dot(M,p)
        q = q.T
        lp = np.linalg.lstsq(Msum, q, rcond=None)[0]
        count = 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)
            # print 1-(np.dot(Lnormal, P2L)/np.linalg.norm(P2L))**2
            # print d
            if d < t:
                count = count + 1
            # print count
            # print "****"
        if count > maxlines:
            maxlines = count
            reallp = lp

lp0 = [[],[],[]]
    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+1
                Lnormal = np.array([-LineD[j] / np.linalg.norm(LineD[j])])
                p = PList[j][:3]
                viviT = Lnormal * Lnormal.T
                M = I - viviT
                Msum = Msum + M
                q = q + np.dot(M, p)
        q = q.T
        lp0 = lp
        lp = np.linalg.lstsq(Msum, q, rcond=None)[0]
        # print lp

reallp = lp
    print (count)

# print "%%%%%"
    return reallp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
import numpy as np
import math
import random

def 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 = 0
    reallp = []

    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 point

        q = 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.T
            M = I - viviT
            Msum = Msum + M
            q = q + np.dot(M,p)
        q = q.T
        lp = np.linalg.lstsq(Msum, q, rcond=None)[0]
        count = 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)
            # print 1-(np.dot(Lnormal, P2L)/np.linalg.norm(P2L))**2
            # print d
            if d < t:
                count = count + 1
            # print count
            # print "****"
        if count > maxlines:
            maxlines = count
            reallp = lp

    lp0 = [[],[],[]]
    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+1
                Lnormal = np.array([-LineD[j] / np.linalg.norm(LineD[j])])
                p = PList[j][:3]
                viviT = Lnormal * Lnormal.T
                M = I - viviT
                Msum = Msum + M
                q = q + np.dot(M, p)
        q = q.T
        lp0 = lp
        lp = np.linalg.lstsq(Msum, q, rcond=None)[0]
        # print lp

    reallp = lp
    print (count)

    # print "%%%%%"
    return reallp