def addOffsetStamp(self, leftFit, rightFit, image, origin, color=(255, 255, 255), fontScale=1.0, thickness=1):
"""
Evaluating camera offset and adding it to a given image
:param thickness: line thickness
:param leftFit: left curve polynomial parameters
:param rightFit: right curve polynomial parameters
:param image: image where data being added
:param origin: upper-left corner of the offset stamp
:param color: stamp color
:param fontScale: font scale
:return: void (adds text to passed image)
"""
imgW = image.shape[1]
imgH = image.shape[0]
yBottom = imgH - 1
cameraCenter = imgW / 2
lBottomX = aux.funcSpace(argSpace=yBottom, fitParams=leftFit) - self.imgMarginWidth
rBottomX = aux.funcSpace(argSpace=yBottom, fitParams=rightFit) - self.imgMarginWidth
laneWidth = rBottomX - lBottomX
scaleX = 3.7 / laneWidth
laneCenter = (lBottomX + rBottomX) / 2
offSet = (cameraCenter - laneCenter) * scaleX
aux.putText(img=image, text='Estimated Vehicle Offset: {:.2f} m'.format(offSet),
origin=origin, color=color, scale=fontScale, thickness=thickness)
def secondarySearch(self, imgH, previousFit, nzX, nzY, margin, src, ratio=1., lineData=None):
"""
Look Ahead search, when previous fits exist for guidance
:param imgH: image height
:param previousFit: previous fit used as a guidance
:param nzX: nonZero X
:param nzY: nonZero Y
:param margin: detectionPointSize used for creation of search area
:param src: source bird-eye binary
:param ratio: used to limit search area by height
:param lineData: used for visualization of available fits
:return: fit + image with search process depicted
"""
ySplit = int(imgH * (1 - ratio))
filterKey = nzY >= ySplit
nzY = nzY[filterKey]
nzX = nzX[filterKey]
srcRgb = np.dstack((src, src, src)) * 255
leftFit, rightFit = self.borderFit(imgH=imgH, centralFit=previousFit, margin=margin)
leftX = aux.funcSpace(argSpace=nzY, fitParams=leftFit)
rightX = aux.funcSpace(argSpace=nzY, fitParams=rightFit)
lineInds = ((nzX > leftX) & (nzX < rightX))
fit = LaneFinding.getFit(lineInds, nzX, nzY)
if lineData is not None:
color = [255, 0, 0] if lineData['lineSpace'] == LineSpace.LEFT else [0, 0, 255]
for lineFit in lineData['fits']:
srcRgb = ip.Drawing.addLine(src=srcRgb, fit=lineFit, color=color, thickness=3, stepCount=20)
srcRgb = ip.Drawing.addLine(src=srcRgb, fit=fit, color=[255, 0, 255], thickness=10, stepCount=20)
winImg = np.zeros_like(srcRgb)
y = np.linspace(start=0, stop=imgH - 1, num=imgH)
xl = aux.funcSpace(argSpace=y, fitParams=leftFit)
xr = aux.funcSpace(argSpace=y, fitParams=rightFit)
line_window1 = np.array([np.transpose(np.vstack([xl, y]))])
line_window2 = np.array([np.flipud(np.transpose(np.vstack([xr, y])))])
line_pts = np.hstack((line_window1, line_window2))
cv2.fillPoly(winImg, np.int_([line_pts]), (0, 255, 0))
srcRgb = cv2.addWeighted(srcRgb, 1, winImg, 0.5, 0)
return fit, srcRgb
def addOffsetStamp(self, leftFit, rightFit, image, origin, color=(255, 255, 255), fontScale=1.0, thickness=1):
imgW = image.shape[1]
imgH = image.shape[0]
yBottom = imgH - 1
cameraCenter = imgW / 2
lBottomX = aux.funcSpace(argSpace=yBottom, fitParams=leftFit) - self.imgMarginWidth
rBottomX = aux.funcSpace(argSpace=yBottom, fitParams=rightFit) - self.imgMarginWidth
laneWidth = rBottomX - lBottomX
scaleX = 3.7 / laneWidth
laneCenter = (lBottomX + rBottomX) / 2
offSet = (cameraCenter - laneCenter) * scaleX
aux.putText(img=image, text='Смещение объекта автомобиля: {:.2f} m'.format(offSet),
origin=origin, color=color, scale=fontScale, thickness=thickness)
def addPolygon(srcShape, lFit, rFit, stepCount=10, color=(0, 255, 0)):
"""
Adds polygon to empty image with a shape of a source
:param srcShape: shape of a source image
:param lFit: left 2-nd order polynomial line params
:param rFit: right 2-nd order polynomial line params
:return: image with polygon between lines
:param stepCount: number of steps
:param color:
"""
imgH = srcShape[0]
imgW = srcShape[1]
cL = lFit[2]
cR = rFit[2]
mask = np.zeros(shape=srcShape, dtype=np.uint8)
rOutstand = max(0, (int(math.ceil(cR)) - (imgW - 1)))
if rOutstand > 0:
r_filler = np.zeros((imgH, rOutstand), dtype=np.uint8)
mask = np.hstack((mask, r_filler))
lOutstand = abs(min(0, int(math.floor(cL))))
if lOutstand > 0:
lFiller = np.zeros((imgH, lOutstand), dtype=np.uint8)
mask = np.hstack((lFiller, mask))
outImg = np.dstack((mask, mask, mask))
y = np.linspace(0, imgH - 1, stepCount)
xl = aux.funcSpace(argSpace=y, fitParams=lFit)
xr = aux.funcSpace(argSpace=y, fitParams=rFit)
leftLinePoints = np.array(
[np.transpose(np.vstack([xl + lOutstand, y]))])
rightLinePoints = np.array(
[np.flipud(np.transpose(np.vstack([xr + lOutstand, y])))])
points = np.hstack((leftLinePoints, rightLinePoints))
cv2.fillPoly(img=outImg, pts=np.int_(points), color=color)
return outImg, lOutstand, rOutstand
def borderFit(imgH, centralFit, margin):
"""
Generates a region of interest outstanding to a given detectionPointSize from the center fit
as a perpendicular to the tangent line at each point
:param imgH: image height
:param centralFit: central fit
:param margin:
:return: left and right fits approximating area tangentially equidistant
from central fit at both sides
"""
a = centralFit[0]
b = centralFit[1]
y = np.linspace(start=0, stop=imgH - 1, num=imgH)
x = aux.funcSpace(argSpace=y, fitParams=centralFit)
dy = 2 * a * y + b # Line slope as the derivative
alpha = np.arctan(dy) # Negative for negative slopes
deltaY = abs(margin * np.sin(alpha)) # Negative for negative alphas
deltaX = margin * np.cos(alpha)
xLeft = x - deltaX
xRight = x + deltaX
yLeft = y + deltaY * np.sign(dy)
yRight = y - deltaY * np.sign(dy)
leftFit = np.polyfit(x=yLeft, y=xLeft, deg=2)
rightFit = np.polyfit(x=yRight, y=xRight, deg=2)
return leftFit, rightFit
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 addLine(src, fit, color, stepCount=10, thickness=2):
if fit is not None:
imgH = src.shape[0]
y = np.linspace(0, imgH - 1, stepCount)
x = aux.funcSpace(argSpace=y, fitParams=fit)
for i in range(stepCount - 1):
x1 = int(round(x[i], 0))
y1 = int(round(y[i], 0))
x2 = int(round(x[i + 1], 0))
y2 = int(round(y[i + 1], 0))
pt1 = (x1, y1)
pt2 = (x2, y2)
src = cv2.line(img=src,
pt1=pt1,
pt2=pt2,
color=color,
thickness=thickness)
return src
