def diagnostic_process_image(img, camCal): imgFtr = ImageFilters(camCal, debug=True) # Run the functions imgFtr.imageQ(img) imgFtr.applyFilter4() imgFtr.horizonDetect(debug=True) # assemble the screen diagScreen = np.zeros((1080, 1920, 3), dtype=np.uint8) diagScreen[0:540, 0:960] = cv2.resize(imgFtr.diag1.astype(np.uint8), (960,540), interpolation=cv2.INTER_AREA) diagScreen[540:1080, 0:960] = cv2.resize(imgFtr.diag2.astype(np.uint8), (960,540), interpolation=cv2.INTER_AREA) diagScreen[0:540, 960:1920] = cv2.resize(imgFtr.diag3.astype(np.uint8), (960,540), interpolation=cv2.INTER_AREA) diagScreen[540:1080, 960:1920] = cv2.resize(imgFtr.diag4.astype(np.uint8), (960,540), interpolation=cv2.INTER_AREA) font = cv2.FONT_HERSHEY_COMPLEX cv2.putText(diagScreen, imgFtr.skyText, (30, 60), font, 1, (255,255,255), 2) cv2.putText(diagScreen, imgFtr.skyImageQ, (30, 90), font, 1, (255,255,255), 2) cv2.putText(diagScreen, imgFtr.roadImageQ, (30, 120), font, 1, (255,255,255), 2) cv2.putText(diagScreen, 'Road Balance: %f'%(imgFtr.roadbalance), (30, 150), font, 1, (255,255,255), 2) if imgFtr.horizonFound: cv2.putText(diagScreen, 'Road Horizon: %d'%(imgFtr.roadhorizon), (30, 180), font, 1, (255,255,255), 2) else: cv2.putText(diagScreen, 'Road Horizon: NOT FOUND!', (30, 180), font, 1, (255,255,0), 2) return diagScreen
def diagnostic_process_image(img, camCal): imgFtr = ImageFilters(camCal, debug=True) # Run the functions imgFtr.imageQ(img) imgFtr.applyFilter1() filter1 = imgFtr.diag4[:, :, 0] # assemble the screen diagScreen = np.zeros((1080, 1920, 3), dtype=np.uint8) diagScreen[0:540, 0:960] = cv2.resize(imgFtr.diag1.astype(np.uint8), (960, 540), interpolation=cv2.INTER_AREA) diagScreen[540:1080, 0:960] = cv2.resize(imgFtr.diag2.astype(np.uint8), (960, 540), interpolation=cv2.INTER_AREA) diagScreen[0:540, 960:1920] = cv2.resize(imgFtr.diag3.astype(np.uint8), (960, 540), interpolation=cv2.INTER_AREA) imgFtr.applyFilter2() filter2 = imgFtr.diag4[:, :, 0] diag4 = np.dstack((filter1, filter2, filter2)) * 4 diagScreen[540:1080, 960:1920] = cv2.resize(diag4.astype(np.uint8) * 4, (960, 540), interpolation=cv2.INTER_AREA) return diagScreen
class RoadManager: # Initialize lineManager def __init__(self, camCal, keepN=10, debug=False): # for both left and right lines # set debugging self.debug = debug # frameNumber self.curFrame = None # keep last N self.keepN = keepN # our own copy of the camera calibration results self.mtx, self.dist, self.img_size = camCal.get() # normal image size self.x, self.y = self.img_size # mid point self.mid = int(self.y / 2) # create our own projection manager self.projMgr = ProjectionManager(camCal, keepN=keepN, debug=debug) # default left-right lane masking self.maskDelta = 5 # road statistics # the left and right lanes curvature measurement could be misleading:need a threshold to indicate straight road. self.roadStraight = False # radius of curvature of the line in meters self.radiusOfCurvature = None # distance in meters of vehicle center is off from road center self.lineBasePos = None # left lines only # left lane identifier self.left = 1 # ghosting of left lane (for use in trouble spots - i.e: bridge or in harder challenges) self.lastNLEdges = None # left lane line class self.leftLane = Line(self.left, self.x, self.y, self.maskDelta) # left lane stats self.leftLaneLastTop = None # right lines only # right lane identifier self.right = 2 # ghosting of right lane (for use in trouble spots - i.e: bridge or in harder challenges) self.lastNREdges = None # right lane line class self.rightLane = Line(self.right, self.x, self.y, self.maskDelta) # right lane stats self.rightLaneLastTop = None # road overhead and unwarped views self.roadsurface = np.zeros((self.y, self.x, 3), dtype=np.uint8) self.roadunwarped = None # number of points fitted self.leftLanePoints = 0 self.rightLanePoints = 0 # cloudy mode self.cloudyMode = False # pixel offset from direction of travel self.lastLeftRightOffset = 0 # boosting self.boosting = 0.0 # resulting image self.final = None # for debugging only if self.debug: self.diag1 = np.zeros((self.y, self.x, 3), dtype=np.float32) # function to find starting lane line positions # return left and right column positions def find_lane_locations(self, projected_masked_lines): height = projected_masked_lines.shape[0] width = projected_masked_lines.shape[1] lefthistogram = np.sum(projected_masked_lines[int(height / 2):height, 0:int(width / 2)], axis=0).astype( np.float32) righthistogram = np.sum(projected_masked_lines[int(height / 2):height, int(width / 2):width], axis=0).astype( np.float32) leftpos = np.argmax(lefthistogram) rightpos = np.argmax(righthistogram) + int(width / 2) # print("leftpos",leftpos,"rightpos",rightpos) return leftpos, rightpos, rightpos - leftpos def findLanes(self, img): if self.curFrame is None: self.curFrame = 0 else: self.curFrame += 1 self.curImgFtr = ImageFilters(self.projMgr.camCal, debug=True) self.curImgFtr.imageQ(img) # detected cloudy condition! if self.curImgFtr.skyText == 'Sky Condition: cloudy' and self.curFrame == 0: self.cloudyMode = True # choose a default filter based on weather condition # line class can update filter based on what it wants too (different for each lane line). if self.cloudyMode: self.curImgFtr.applyFilter3() self.maskDelta = 20 self.leftLane.setMaskDelta(self.maskDelta) self.rightLane.setMaskDelta(self.maskDelta) elif self.curImgFtr.skyText == 'Sky Condition: clear' or self.curImgFtr.skyText == 'Sky Condition: tree shaded': self.curImgFtr.applyFilter2() elif self.curFrame < 2: self.curImgFtr.applyFilter4() else: self.curImgFtr.applyFilter5() self.curImgFtr.horizonDetect(debug=True) # low confidence? if self.leftLane.confidence < 0.5 or self.rightLane.confidence < 0.5 or self.curFrame < 2: self.projMgr.findInitialRoadCorners(self.curImgFtr) self.initialGradient = self.projMgr.curGradient # Use visibility to lower the FoV # adjust source for perspective projection accordingly if self.curImgFtr.horizonFound: self.roadHorizonGap = self.projMgr.curGradient - self.curImgFtr.roadhorizon newTop = self.curImgFtr.roadhorizon + self.roadHorizonGap self.projMgr.setSrcTop(newTop - self.curImgFtr.visibility, self.curImgFtr.visibility) self.lastNREdges = self.curImgFtr.curRoadEdge self.lastNLEdges = self.curImgFtr.curRoadEdge masked_edges = self.curImgFtr.getProjection() # print("masked_edges: ", masked_edges.shape) masked_edge = masked_edges[:, :, 1] leftpos, rightpos, distance = self.find_lane_locations(masked_edge) self.leftLane.setBasePos(leftpos) self.leftLane.find_lane_lines_points(masked_edge) self.rightLane.setBasePos(rightpos) self.rightLane.find_lane_lines_points(masked_edge) self.leftLane.fitpoly() leftprojection = self.leftLane.applyLineMask(self.curImgFtr.getProjection(self.leftLane.side)) self.leftLane.radius_in_meters(distance) self.leftLane.meters_from_center_of_vehicle(distance) self.rightLane.fitpoly() rightprojection = self.rightLane.applyLineMask(self.curImgFtr.getProjection(self.rightLane.side)) self.rightLane.radius_in_meters(distance) self.rightLane.meters_from_center_of_vehicle(distance) else: # Apply Boosting... # For Challenges ONLY if self.cloudyMode: if self.curImgFtr.skyText == 'Sky Condition: cloudy': self.boosting = 0.4 self.lastNREdges = self.curImgFtr.miximg(self.curImgFtr.curRoadEdge, self.lastNREdges, 1.0, 0.4) else: self.boosting = 1.0 self.lastNREdges = self.curImgFtr.miximg(self.curImgFtr.curRoadEdge, self.lastNREdges, 1.0, 1.0) self.curImgFtr.curRoadEdge = self.lastNREdges elif self.curImgFtr.skyText == 'Sky Condition: surrounded by trees': self.boosting = 0.0 # self.lastNREdges = self.curImgFtr.miximg(self.curImgFtr.curRoadEdge, self.lastNREdges, 1.0, 0.4) # self.curImgFtr.curRoadEdge = self.lastNREdges # project the new frame to a plane for further analysis. self.projMgr.project(self.curImgFtr, self.lastLeftRightOffset) # find approximate left right positions and distance apart masked_edges = self.curImgFtr.getProjection() masked_edge = masked_edges[:, :, 1] # Left Lane Projection setup leftprojection = self.leftLane.applyLineMask(self.curImgFtr.getProjection(self.leftLane.side)) leftPoints = np.nonzero(leftprojection) # leftPoints = cv2.findNonZero(leftprojection) self.leftLane.allX = leftPoints[1] self.leftLane.allY = leftPoints[0] self.leftLane.fitpoly2() # Right Lane Projection setup rightprojection = self.rightLane.applyLineMask(self.curImgFtr.getProjection(self.rightLane.side)) # rightPoints = np.transpose(np.nonzero(rightprojection)) rightPoints = np.nonzero(rightprojection) # rightPoints = cv2.findNonZero(rightprojection) self.rightLane.allX = rightPoints[1] self.rightLane.allY = rightPoints[0] self.rightLane.fitpoly2() # take and calculate some measurements distance = self.rightLane.pixelBasePos - self.leftLane.pixelBasePos self.leftLane.radius_in_meters(distance) self.leftLane.meters_from_center_of_vehicle(distance) self.rightLane.radius_in_meters(distance) self.rightLane.meters_from_center_of_vehicle(distance) leftTop = self.leftLane.getTopPoint() rightTop = self.rightLane.getTopPoint() # Attempt to move up the Lane lines if we missed some predictions if self.leftLaneLastTop is not None and \ self.rightLaneLastTop is not None: # If we are in the harder challenge, our visibility is obscured, # so only do this if we are certain that our visibility is good. # i.e.: not in the harder challenge! if self.curImgFtr.visibility > -30: # if either lines differs by greater than 50 pixel vertically # we need to request the shorter line to go higher. if abs(self.leftLaneLastTop[1] - self.rightLaneLastTop[1]) > 50: if self.leftLaneLastTop[1] > self.rightLaneLastTop[1]: self.leftLane.requestTopY(self.rightLaneLastTop[1]) else: self.rightLane.requestTopY(self.leftLaneLastTop[1]) # if our lane line has fallen to below our threshold, get it to come back up if leftTop is not None and leftTop[1] > self.mid - 100: self.leftLane.requestTopY(leftTop[1] - 10) if leftTop is not None and leftTop[1] > self.leftLaneLastTop[1]: self.leftLane.requestTopY(leftTop[1] - 10) if rightTop is not None and rightTop[1] > self.mid - 100: self.rightLane.requestTopY(rightTop[1] - 10) if rightTop is not None and rightTop[1] > self.rightLaneLastTop[1]: self.rightLane.requestTopY(rightTop[1] - 10) # visibility poor... # harder challenge... need to be less agressive going back up the lane... # let at least 30 frame pass before trying to move forward. elif self.curFrame > 30: # if either lines differs by greater than 50 pixel vertically # we need to request the shorter line to go higher. if abs(self.leftLaneLastTop[1] - self.rightLaneLastTop[1]) > 50: if self.leftLaneLastTop[1] > self.rightLaneLastTop[1] and leftTop is not None: self.leftLane.requestTopY(leftTop[1] - 10) elif rightTop is not None: self.rightLane.requestTopY(rightTop[1] - 10) # if our lane line has fallen to below our threshold, get it to come back up if leftTop is not None and leftTop[1] > self.mid + 100: self.leftLane.requestTopY(leftTop[1] - 10) if leftTop is not None and leftTop[1] > self.leftLaneLastTop[1]: self.leftLane.requestTopY(leftTop[1] - 10) if rightTop is not None and rightTop[1] > self.mid + 100: self.rightLane.requestTopY(rightTop[1] - 10) if rightTop is not None and rightTop[1] > self.rightLaneLastTop[1]: self.rightLane.requestTopY(rightTop[1] - 10) # Experimental # Update location for FoV # if leftTop is not None and rightTop is not None and self.curImgFtr.visibility < -30: # self.lastLeftRightOffset = int((self.x/2)-(leftTop[0]+rightTop[0])/2) # # print("lastLeftRightOffset: ", self.lastLeftRightOffset) # Update Stats and Top points for next frame. self.leftLaneLastTop = self.leftLane.getTopPoint() self.rightLaneLastTop = self.rightLane.getTopPoint() self.leftLanePoints = len(self.leftLane.allX) self.rightLanePoints = len(self.rightLane.allX) # Update road statistics for display self.lineBasePos = (self.leftLane.lineBasePos + self.rightLane.lineBasePos) if self.leftLane.radiusOfCurvature > 0.0 and self.rightLane.radiusOfCurvature > 0.0: self.radiusOfCurvature = (self.leftLane.radiusOfCurvature + self.rightLane.radiusOfCurvature) / 2.0 if self.leftLane.radiusOfCurvature > 3000.0: self.roadStraight = True elif self.rightLane.radiusOfCurvature > 3000.0: self.roadStraight = True else: self.roadStraight = False elif self.leftLane.radiusOfCurvature < 0.0 and self.rightLane.radiusOfCurvature < 0.0: self.radiusOfCurvature = (self.leftLane.radiusOfCurvature + self.rightLane.radiusOfCurvature) / 2.0 if self.leftLane.radiusOfCurvature < -3000.0: self.roadStraight = True elif self.rightLane.radiusOfCurvature < -3000.0: self.roadStraight = True else: self.roadStraight = False else: self.roadStraight = True # Experimental # adjust source for perspective projection accordingly # attempt to dampen bounce # if self.leftLaneLastTop is not None and self.rightLaneLastTop is not None: # x = int((self.x-(self.leftLaneLastTop[0]+self.rightLaneLastTop[0]))/4) # self.projMgr.setSrcTopX(x) # create road mask polygon for reprojection back onto perspective view. roadpoly = np.concatenate((self.rightLane.XYPolyline, self.leftLane.XYPolyline[::-1]), axis=0) roadmask = np.zeros((self.y, self.x), dtype=np.uint8) cv2.fillConvexPoly(roadmask, roadpoly, 64) self.roadsurface[:, :, 0] = self.curImgFtr.miximg(leftprojection, self.leftLane.linemask, 0.5, 0.3) self.roadsurface[:, :, 1] = roadmask self.roadsurface[:, :, 2] = self.curImgFtr.miximg(rightprojection, self.rightLane.linemask, 0.5, 0.3) # unwarp the roadsurface self.roadunwarped = self.projMgr.curUnWarp(self.curImgFtr, self.roadsurface) # create the final image self.final = self.curImgFtr.miximg(self.curImgFtr.curImage, self.roadunwarped, 0.95, 0.75) # draw dots and polyline if self.debug: font = cv2.FONT_HERSHEY_COMPLEX # our own diag screen self.diag1, M = self.projMgr.unwarp_lane(self.curImgFtr.makefull(self.projMgr.diag1), self.projMgr.curSrcRoadCorners, self.projMgr.curDstRoadCorners, self.projMgr.mtx) self.diag1 = np.copy(self.projMgr.diag4) self.leftLane.scatter_plot(self.diag1) self.leftLane.polyline(self.diag1) self.rightLane.scatter_plot(self.diag1) self.rightLane.polyline(self.diag1) cv2.putText(self.diag1, 'Frame: %d' % (self.curFrame), (30, 30), font, 1, (255, 0, 0), 2) self.leftLane.scatter_plot(self.projMgr.diag4) self.leftLane.polyline(self.projMgr.diag4) self.rightLane.scatter_plot(self.projMgr.diag4) self.rightLane.polyline(self.projMgr.diag4) cv2.putText(self.projMgr.diag4, 'Frame: %d' % (self.curFrame), (30, 30), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Left: %d count, %4.1f%% confidence, detected: %r' % ( self.leftLanePoints, self.leftLane.confidence * 100, self.leftLane.detected), (30, 60), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Left: RoC: %fm, DfVC: %fcm' % ( self.leftLane.radiusOfCurvature, self.leftLane.lineBasePos * 100), (30, 90), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Right %d count, %4.1f%% confidence, detected: %r' % ( self.rightLanePoints, self.rightLane.confidence * 100, self.rightLane.detected), (30, 120), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Right RoC: %fm, DfVC: %fcm' % ( self.rightLane.radiusOfCurvature, self.rightLane.lineBasePos * 100), (30, 150), font, 1, (255, 255, 0), 2) if self.boosting > 0.0: cv2.putText(self.projMgr.diag4, 'Boosting @ %f%%' % (self.boosting), (30, 180), font, 1, (128, 128, 192), 2) self.projMgr.diag4 = self.curImgFtr.miximg(self.projMgr.diag4, self.roadsurface, 1.0, 2.0) self.projMgr.diag2 = self.curImgFtr.miximg(self.projMgr.diag2, self.roadunwarped, 1.0, 0.5) self.projMgr.diag1 = self.curImgFtr.miximg(self.projMgr.diag1, self.roadunwarped[self.mid:self.y, :, :], 1.0, 2.0) def drawLaneStats(self, color=(224, 192, 0)): font = cv2.FONT_HERSHEY_COMPLEX if self.roadStraight: cv2.putText(self.final, 'Estimated lane curvature: road nearly straight', (30, 60), font, 1, color, 2) elif self.radiusOfCurvature > 0.0: cv2.putText(self.final, 'Estimated lane curvature: center is %fm to the right' % (self.radiusOfCurvature), (30, 60), font, 1, color, 2) else: cv2.putText(self.final, 'Estimated lane curvature: center is %fm to the left' % (-self.radiusOfCurvature), (30, 60), font, 1, color, 2) if self.lineBasePos < 0.0: cv2.putText(self.final, 'Estimated left of center: %5.2fcm' % (-self.lineBasePos * 100), (30, 90), font, 1, color, 2) elif self.lineBasePos > 0.0: cv2.putText(self.final, 'Estimated right of center: %5.2fcm' % (self.lineBasePos * 100), (30, 90), font, 1, color, 2) else: cv2.putText(self.final, 'Estimated at center of road', (30, 90), font, 1, color, 2)
def findLanes(self, img): if self.curFrame is None: self.curFrame = 0 else: self.curFrame += 1 self.curImgFtr = ImageFilters(self.projMgr.camCal, debug=True) self.curImgFtr.imageQ(img) # detected cloudy condition! if self.curImgFtr.skyText == 'Sky Condition: cloudy' and self.curFrame == 0: self.cloudyMode = True # choose a default filter based on weather condition # line class can update filter based on what it wants too (different for each lane line). if self.cloudyMode: self.curImgFtr.applyFilter3() self.maskDelta = 20 self.leftLane.setMaskDelta(self.maskDelta) self.rightLane.setMaskDelta(self.maskDelta) elif self.curImgFtr.skyText == 'Sky Condition: clear' or self.curImgFtr.skyText == 'Sky Condition: tree shaded': self.curImgFtr.applyFilter2() elif self.curFrame < 2: self.curImgFtr.applyFilter4() else: self.curImgFtr.applyFilter5() self.curImgFtr.horizonDetect(debug=True) # low confidence? if self.leftLane.confidence < 0.5 or self.rightLane.confidence < 0.5 or self.curFrame < 2: self.projMgr.findInitialRoadCorners(self.curImgFtr) self.initialGradient = self.projMgr.curGradient # Use visibility to lower the FoV # adjust source for perspective projection accordingly if self.curImgFtr.horizonFound: self.roadHorizonGap = self.projMgr.curGradient - self.curImgFtr.roadhorizon newTop = self.curImgFtr.roadhorizon + self.roadHorizonGap self.projMgr.setSrcTop(newTop - self.curImgFtr.visibility, self.curImgFtr.visibility) self.lastNREdges = self.curImgFtr.curRoadEdge self.lastNLEdges = self.curImgFtr.curRoadEdge masked_edges = self.curImgFtr.getProjection() # print("masked_edges: ", masked_edges.shape) masked_edge = masked_edges[:, :, 1] leftpos, rightpos, distance = self.find_lane_locations(masked_edge) self.leftLane.setBasePos(leftpos) self.leftLane.find_lane_lines_points(masked_edge) self.rightLane.setBasePos(rightpos) self.rightLane.find_lane_lines_points(masked_edge) self.leftLane.fitpoly() leftprojection = self.leftLane.applyLineMask(self.curImgFtr.getProjection(self.leftLane.side)) self.leftLane.radius_in_meters(distance) self.leftLane.meters_from_center_of_vehicle(distance) self.rightLane.fitpoly() rightprojection = self.rightLane.applyLineMask(self.curImgFtr.getProjection(self.rightLane.side)) self.rightLane.radius_in_meters(distance) self.rightLane.meters_from_center_of_vehicle(distance) else: # Apply Boosting... # For Challenges ONLY if self.cloudyMode: if self.curImgFtr.skyText == 'Sky Condition: cloudy': self.boosting = 0.4 self.lastNREdges = self.curImgFtr.miximg(self.curImgFtr.curRoadEdge, self.lastNREdges, 1.0, 0.4) else: self.boosting = 1.0 self.lastNREdges = self.curImgFtr.miximg(self.curImgFtr.curRoadEdge, self.lastNREdges, 1.0, 1.0) self.curImgFtr.curRoadEdge = self.lastNREdges elif self.curImgFtr.skyText == 'Sky Condition: surrounded by trees': self.boosting = 0.0 # self.lastNREdges = self.curImgFtr.miximg(self.curImgFtr.curRoadEdge, self.lastNREdges, 1.0, 0.4) # self.curImgFtr.curRoadEdge = self.lastNREdges # project the new frame to a plane for further analysis. self.projMgr.project(self.curImgFtr, self.lastLeftRightOffset) # find approximate left right positions and distance apart masked_edges = self.curImgFtr.getProjection() masked_edge = masked_edges[:, :, 1] # Left Lane Projection setup leftprojection = self.leftLane.applyLineMask(self.curImgFtr.getProjection(self.leftLane.side)) leftPoints = np.nonzero(leftprojection) # leftPoints = cv2.findNonZero(leftprojection) self.leftLane.allX = leftPoints[1] self.leftLane.allY = leftPoints[0] self.leftLane.fitpoly2() # Right Lane Projection setup rightprojection = self.rightLane.applyLineMask(self.curImgFtr.getProjection(self.rightLane.side)) # rightPoints = np.transpose(np.nonzero(rightprojection)) rightPoints = np.nonzero(rightprojection) # rightPoints = cv2.findNonZero(rightprojection) self.rightLane.allX = rightPoints[1] self.rightLane.allY = rightPoints[0] self.rightLane.fitpoly2() # take and calculate some measurements distance = self.rightLane.pixelBasePos - self.leftLane.pixelBasePos self.leftLane.radius_in_meters(distance) self.leftLane.meters_from_center_of_vehicle(distance) self.rightLane.radius_in_meters(distance) self.rightLane.meters_from_center_of_vehicle(distance) leftTop = self.leftLane.getTopPoint() rightTop = self.rightLane.getTopPoint() # Attempt to move up the Lane lines if we missed some predictions if self.leftLaneLastTop is not None and \ self.rightLaneLastTop is not None: # If we are in the harder challenge, our visibility is obscured, # so only do this if we are certain that our visibility is good. # i.e.: not in the harder challenge! if self.curImgFtr.visibility > -30: # if either lines differs by greater than 50 pixel vertically # we need to request the shorter line to go higher. if abs(self.leftLaneLastTop[1] - self.rightLaneLastTop[1]) > 50: if self.leftLaneLastTop[1] > self.rightLaneLastTop[1]: self.leftLane.requestTopY(self.rightLaneLastTop[1]) else: self.rightLane.requestTopY(self.leftLaneLastTop[1]) # if our lane line has fallen to below our threshold, get it to come back up if leftTop is not None and leftTop[1] > self.mid - 100: self.leftLane.requestTopY(leftTop[1] - 10) if leftTop is not None and leftTop[1] > self.leftLaneLastTop[1]: self.leftLane.requestTopY(leftTop[1] - 10) if rightTop is not None and rightTop[1] > self.mid - 100: self.rightLane.requestTopY(rightTop[1] - 10) if rightTop is not None and rightTop[1] > self.rightLaneLastTop[1]: self.rightLane.requestTopY(rightTop[1] - 10) # visibility poor... # harder challenge... need to be less agressive going back up the lane... # let at least 30 frame pass before trying to move forward. elif self.curFrame > 30: # if either lines differs by greater than 50 pixel vertically # we need to request the shorter line to go higher. if abs(self.leftLaneLastTop[1] - self.rightLaneLastTop[1]) > 50: if self.leftLaneLastTop[1] > self.rightLaneLastTop[1] and leftTop is not None: self.leftLane.requestTopY(leftTop[1] - 10) elif rightTop is not None: self.rightLane.requestTopY(rightTop[1] - 10) # if our lane line has fallen to below our threshold, get it to come back up if leftTop is not None and leftTop[1] > self.mid + 100: self.leftLane.requestTopY(leftTop[1] - 10) if leftTop is not None and leftTop[1] > self.leftLaneLastTop[1]: self.leftLane.requestTopY(leftTop[1] - 10) if rightTop is not None and rightTop[1] > self.mid + 100: self.rightLane.requestTopY(rightTop[1] - 10) if rightTop is not None and rightTop[1] > self.rightLaneLastTop[1]: self.rightLane.requestTopY(rightTop[1] - 10) # Experimental # Update location for FoV # if leftTop is not None and rightTop is not None and self.curImgFtr.visibility < -30: # self.lastLeftRightOffset = int((self.x/2)-(leftTop[0]+rightTop[0])/2) # # print("lastLeftRightOffset: ", self.lastLeftRightOffset) # Update Stats and Top points for next frame. self.leftLaneLastTop = self.leftLane.getTopPoint() self.rightLaneLastTop = self.rightLane.getTopPoint() self.leftLanePoints = len(self.leftLane.allX) self.rightLanePoints = len(self.rightLane.allX) # Update road statistics for display self.lineBasePos = (self.leftLane.lineBasePos + self.rightLane.lineBasePos) if self.leftLane.radiusOfCurvature > 0.0 and self.rightLane.radiusOfCurvature > 0.0: self.radiusOfCurvature = (self.leftLane.radiusOfCurvature + self.rightLane.radiusOfCurvature) / 2.0 if self.leftLane.radiusOfCurvature > 3000.0: self.roadStraight = True elif self.rightLane.radiusOfCurvature > 3000.0: self.roadStraight = True else: self.roadStraight = False elif self.leftLane.radiusOfCurvature < 0.0 and self.rightLane.radiusOfCurvature < 0.0: self.radiusOfCurvature = (self.leftLane.radiusOfCurvature + self.rightLane.radiusOfCurvature) / 2.0 if self.leftLane.radiusOfCurvature < -3000.0: self.roadStraight = True elif self.rightLane.radiusOfCurvature < -3000.0: self.roadStraight = True else: self.roadStraight = False else: self.roadStraight = True # Experimental # adjust source for perspective projection accordingly # attempt to dampen bounce # if self.leftLaneLastTop is not None and self.rightLaneLastTop is not None: # x = int((self.x-(self.leftLaneLastTop[0]+self.rightLaneLastTop[0]))/4) # self.projMgr.setSrcTopX(x) # create road mask polygon for reprojection back onto perspective view. roadpoly = np.concatenate((self.rightLane.XYPolyline, self.leftLane.XYPolyline[::-1]), axis=0) roadmask = np.zeros((self.y, self.x), dtype=np.uint8) cv2.fillConvexPoly(roadmask, roadpoly, 64) self.roadsurface[:, :, 0] = self.curImgFtr.miximg(leftprojection, self.leftLane.linemask, 0.5, 0.3) self.roadsurface[:, :, 1] = roadmask self.roadsurface[:, :, 2] = self.curImgFtr.miximg(rightprojection, self.rightLane.linemask, 0.5, 0.3) # unwarp the roadsurface self.roadunwarped = self.projMgr.curUnWarp(self.curImgFtr, self.roadsurface) # create the final image self.final = self.curImgFtr.miximg(self.curImgFtr.curImage, self.roadunwarped, 0.95, 0.75) # draw dots and polyline if self.debug: font = cv2.FONT_HERSHEY_COMPLEX # our own diag screen self.diag1, M = self.projMgr.unwarp_lane(self.curImgFtr.makefull(self.projMgr.diag1), self.projMgr.curSrcRoadCorners, self.projMgr.curDstRoadCorners, self.projMgr.mtx) self.diag1 = np.copy(self.projMgr.diag4) self.leftLane.scatter_plot(self.diag1) self.leftLane.polyline(self.diag1) self.rightLane.scatter_plot(self.diag1) self.rightLane.polyline(self.diag1) cv2.putText(self.diag1, 'Frame: %d' % (self.curFrame), (30, 30), font, 1, (255, 0, 0), 2) self.leftLane.scatter_plot(self.projMgr.diag4) self.leftLane.polyline(self.projMgr.diag4) self.rightLane.scatter_plot(self.projMgr.diag4) self.rightLane.polyline(self.projMgr.diag4) cv2.putText(self.projMgr.diag4, 'Frame: %d' % (self.curFrame), (30, 30), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Left: %d count, %4.1f%% confidence, detected: %r' % ( self.leftLanePoints, self.leftLane.confidence * 100, self.leftLane.detected), (30, 60), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Left: RoC: %fm, DfVC: %fcm' % ( self.leftLane.radiusOfCurvature, self.leftLane.lineBasePos * 100), (30, 90), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Right %d count, %4.1f%% confidence, detected: %r' % ( self.rightLanePoints, self.rightLane.confidence * 100, self.rightLane.detected), (30, 120), font, 1, (255, 255, 0), 2) cv2.putText(self.projMgr.diag4, 'Right RoC: %fm, DfVC: %fcm' % ( self.rightLane.radiusOfCurvature, self.rightLane.lineBasePos * 100), (30, 150), font, 1, (255, 255, 0), 2) if self.boosting > 0.0: cv2.putText(self.projMgr.diag4, 'Boosting @ %f%%' % (self.boosting), (30, 180), font, 1, (128, 128, 192), 2) self.projMgr.diag4 = self.curImgFtr.miximg(self.projMgr.diag4, self.roadsurface, 1.0, 2.0) self.projMgr.diag2 = self.curImgFtr.miximg(self.projMgr.diag2, self.roadunwarped, 1.0, 0.5) self.projMgr.diag1 = self.curImgFtr.miximg(self.projMgr.diag1, self.roadunwarped[self.mid:self.y, :, :], 1.0, 2.0)