-
Notifications
You must be signed in to change notification settings - Fork 78
/
Line.py
188 lines (142 loc) · 6.21 KB
/
Line.py
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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
import numpy as np
from LaneFinding import LaneFinding
import helper as aux
class Line:
def __init__(self, lineSpace, historyDepth=5, margin=100, windowSplit=2, winCount=9, searchPortion=1.):
self.lineSpace = lineSpace
self.fits = np.empty((0, 3), float)
self.depth = historyDepth
self.margin = margin
self.windowSplit = windowSplit
self.winCount = winCount
self.searchPortion = searchPortion
def resetFits(self):
"""
removes all previously found fits
:return: void
"""
self.fits = np.empty((0, 3), float)
def reScanWithPrimary(self, src):
"""
Allows to initiate box search with xPrimary obtained from previously found fits.
:param src: binary bird-eye image
:return: fit + fitType + image with search process depicted
fitType used for debugging purposes (may be ignored)
"""
imgH = src.shape[0]
yBottom = imgH - 1
self.fits = self.fits[:len(self.fits) - 1] # Remove newest fit
finder = LaneFinding()
nzX, nzY = self.nz(src=src)
currFit = self.currentFit()
if currFit is not None:
x_primary = aux.funcSpace(yBottom, currFit)
else:
x_primary = None
fit, srcRgb = finder.primarySearchPolyMargin(src=src, lineSpace=self.lineSpace,
winCount=self.winCount, margin=self.margin, minpix=50,
nzX=nzX, nzY=nzY, windowSplit=self.windowSplit,
xPrimary=x_primary)
if fit is not None:
self.addFit(fit)
return self.currentFit(), 'reScan', srcRgb
def addFit(self, fit):
"""
keeps the number of newest fits not exceeding the defined history depth
:param fit: fit to add
:return: void
"""
self.fits = np.vstack((self.fits, np.array(fit)))
self.fits = self.fits[-self.depth:]
def currentFit(self):
"""
Computes current line fit as a weighted average
:return: fit (a, b, c)
"""
weights = self.getWeights()
if weights is not None:
a = np.sum(self.fits[:, 0] * weights) / np.sum(weights)
b = np.sum(self.fits[:, 1] * weights) / np.sum(weights)
c = np.sum(self.fits[:, 2] * weights) / np.sum(weights)
return [a, b, c]
else:
return None
def getWeights(self):
"""
computes weights according to the following logic:
* Slope weights take steepness of curvature (A in polynomial coefficients) and give more weight
to more vertical lines
* Vertex weights are the ```y``` coordinate where parabola turns.
Considered that the lower it is, the higher the weight should be
* Age weights - the younger the fit, the heavier its weight.
Then I compute cumulative weight as a product of all three and normalize it between 0 and 1
:return: average normalized weights
"""
if len(self.fits) > 0:
slope_weights = 1 / abs(self.fits[:, 0])
vertex_weights = -self.fits[:, 1] / (self.fits[:, 0] * 2)
age_weights = 1 / np.flipud(np.linspace(1, len(self.fits), len(self.fits)))
w = slope_weights * vertex_weights * age_weights
normal_w = (w - np.min(w)) / (np.max(w) - np.min(w)) if np.max(w) != np.min(w) else w
return normal_w
else:
return None
@staticmethod
def nz(src, full=False, ratio=1):
"""
Convenience wrapper for numpy.nonzero() function
:param src: source binary image
:param full: flag to determine whether to return all or just nzX and nzY
:param ratio: allows to take partial nonzeros along zeroth axis ('y' in terms of numpy convention)
:return:
"""
imgH = src.shape[0]
vSplit = int(imgH * (1 - ratio))
nonZero = src[vSplit:, :].nonzero()
nzY = np.array(nonZero[0])
nzX = np.array(nonZero[1])
if full:
return nonZero, nzX, nzY
else:
return nzX, nzY
def getFit(self, src):
"""
Performs one of 2 types of search: initial box search or look ahead, depending on available fits
:param src: binary bird-eye
:return: fit + fitType + image with search process depicted
"""
fitType = 'primary'
finder = LaneFinding()
current_fit = self.currentFit()
nzX, nzY = self.nz(src=src)
if current_fit is None:
"""
def primarySearchPolyMargin(self, src, lineSpace, winCount, detectionPointSize, minpix,
nzX, nzY, windowSplit=2, xPrimary=None):
"""
fit, src_rgb = finder.primarySearchPolyMargin(src=src, lineSpace=self.lineSpace,
winCount=self.winCount, margin=self.margin, minpix=50,
nzX=nzX, nzY=nzY, windowSplit=self.windowSplit)
else:
fit, src_rgb = finder.secondarySearch(imgH=src.shape[0], previousFit=current_fit, nzX=nzX, nzY=nzY,
margin=self.margin, src=src, ratio=self.searchPortion,
lineData={'lineSpace': self.lineSpace, 'fits': self.fits})
fitType = 'lookAhead'
if fit is not None:
self.addFit(fit)
return self.currentFit(), fitType, src_rgb
def reScanJustified(self):
"""
Determines whether it makes sense to perform re-scan with box search
:return: bool
"""
if len(self.fits) > 1:
fitParamsFull = self.fits.T
fitParamsNoLast = self.fits[:-1].T
for i in range(len(fitParamsFull)):
stdFull = np.std(fitParamsFull[i])
stdNoLast = np.std(fitParamsNoLast[i])
if (stdNoLast != 0 and stdFull / stdNoLast >= 1.5) or stdNoLast == 0:
return True
return False
return False