def getLaneCurve(img):
# imgCopy = img.copy()
# imgResult = img.copy()
#### STEP 1
imgThres = utils.thresholding(img)
hT, wT, c = img.shape
points = utils.valTrackbars()
imgWarp = utils.warpImg(imgThres, points, wT, hT)
#imgWarpPoints = utils.drawPoints(imgCopy,points)
mid, imgHist = utils.getHistogram(imgThres,
display=True,
minPer=0.5,
region=4)
base, imgHist = utils.getHistogram(imgThres, display=True, minPer=0.9)
cur = base - mid
turn.append(cur)
if len(turn) > avgVal:
turn.pop(0)
curve = int(sum(turn) / len(turn))
cv2.putText(img, str(curve), (wT // 2 - 80, 85), cv2.FONT_HERSHEY_COMPLEX,
2, (255, 0, 255), 3)
imgStacked = utils.stackImages(0.7, ([img, imgHist, imgThres]))
cv2.imshow('ImageStack', imgStacked)
#cv2.imshow("warp", imgWarp)
# cv2.imshow("thres", imgThres)
# cv2.imshow("histogram", imgHist)
return curve
def getLaneCurve(img, display=2):
imgCopy = img.copy()
imgResult = img.copy()
#-- Step1: Threshold each frame
imgThres = utils.thresholding(img)
#-- Step 2: Warp each frame
hT, wT, c = img.shape
points = utils.valTrackbars()
imgWarp = utils.warpImg(imgThres, points, wT, hT)
imgWarpPoints = utils.drawPoints(imgCopy, points)
#-- Step 3: Get midpoint and curve average point to find raw curve value
midPoint, imgHist = utils.getHistogram(imgWarp,
display=True,
minPer=0.5,
region=4)
curveAveragePoint, imgHist = utils.getHistogram(imgWarp,
display=True,
minPer=0.9)
curveRaw = curveAveragePoint - midPoint
#-- Step 4: Find curve value
curveList.append(curveRaw)
if len(curveList) > avgVal:
curveList.pop(0)
curve = int(sum(curveList) / len(curveList))
#-- Display options:
#-- if display not 0, make the component necessary to diplay
#-- if display = 1, show only final result frames with curve value
#-- if display = 2, show the whole process of image processing by stacking different frames
if display != 0:
imgInvWarp = utils.warpImg(imgWarp, points, wT, hT, inv=True)
imgInvWarp = cv2.cvtColor(imgInvWarp, cv2.COLOR_GRAY2BGR)
imgInvWarp[0:hT // 3, 0:wT] = 0, 0, 0
imgLaneColor = np.zeros_like(img)
imgLaneColor[:] = 0, 255, 0
imgLaneColor = cv2.bitwise_and(imgInvWarp, imgLaneColor)
imgResult = cv2.addWeighted(imgResult, 1, imgLaneColor, 1, 0)
midY = 450
cv2.putText(imgResult, str(curve), (wT // 2 - 80, 85),
cv2.FONT_HERSHEY_COMPLEX, 2, (255, 0, 255), 3)
cv2.line(imgResult, (wT // 2, midY), (wT // 2 + (curve * 3), midY),
(255, 0, 255), 5)
cv2.line(imgResult, ((wT // 2 + (curve * 3)), midY - 25),
(wT // 2 + (curve * 3), midY + 25), (0, 255, 0), 5)
for x in range(-30, 30):
w = wT // 20
cv2.line(imgResult, (w * x + int(curve // 50), midY - 10),
(w * x + int(curve // 50), midY + 10), (0, 0, 255), 2)
if display == 2:
imgStacked = utils.stackImages(0.7,
([img, imgWarpPoints, imgWarp],
[imgHist, imgLaneColor, imgResult]))
cv2.imshow('ImageStack', imgStacked)
elif display == 1:
cv2.imshow('Resutlt', imgResult)
def getLaneCurve(img):
imgCopy = img.copy()
h, w, c = img.shape
points = utils.valTrackbars()
imgThres = utils.thresholding(img)
# imgWarp = utils.warpImg(img, points, w, h)
imgWarpThres = utils.warpImg(imgThres, points, w, h)
imgWarpPoints = utils.drawPoints(imgCopy, points)
imgLane = img.copy()
# imgCanny = utils.canny(imgWarp)
basePoint, imgHist = utils.getHistogram(imgWarpThres, display=True)
imgSliding, curves, lanes, ploty = utils.sliding_window(imgWarpThres,
draw_windows=True)
curverad = utils.get_curve(imgLane, curves[0], curves[1])
lane_curve = np.mean([curverad[0], curverad[1]])
imgLane = utils.drawLanes(img,
curves[0],
curves[1],
frameWidth,
frameHeight,
src=points)
# print(round((lane_curve-84.41)/7.5,2))
imgStack = utils.stackImages(
0.6,
([img, imgWarpThres, imgWarpPoints], [imgHist, imgSliding, imgLane]))
cv2.imshow('Stack', imgStack)
return cv2.resize(imgStack, (2 * frameHeight, frameHeight))
def getLaneCurve(img, display):
img = cv2.resize(img, (1280, 720)) #resize the frames to the same size used for calibration
imgCopy = img.copy() #
imgResult = img.copy()
#-- step 1: thresholding frames
imgThres = utils.thresholding(img) #input original image frames to thresholding function in utils module
#-- step 2: warping frames
hT, wT, c = img.shape #get image width and height (channels not used)
points = utils.valTrackbars() #get lane reference points from current trackbar(valTrackbars() is a function we created to place points on four lane edges)
imgWarp = utils.warpImg(imgThres, points, wT,hT) #get warped frame by inputting thresholded frame, lane reference points, width and height
imgWarpPoints = utils.drawPoints(imgCopy, points) #draw
#-- step 3:
midPoint, imgHist = utils.getHistogram(imgWarp, display=True, minPer=0.5, region=4)
curveAveragePoint, imgHist = utils.getHistogram(imgWarp, display=True, minPer=0.9)
curveRaw = curveAveragePoint-midPoint
### step 4
curveList.append(curveRaw)
if len(curveList)>avgVal:
curveList.pop(0)
curve = int(sum(curveList)/len(curveList))
### final step
if display != 0:
imgInvWarp = utils.warpImg(imgWarp, points, wT, hT, inv=True)
imgInvWarp = cv2.cvtColor(imgInvWarp, cv2.COLOR_GRAY2BGR)
imgInvWarp[0:hT // 3, 0:wT] = 0, 0, 0
imgLaneColor = np.zeros_like(img)
imgLaneColor[:] = 0, 255, 0
imgLaneColor = cv2.bitwise_and(imgInvWarp, imgLaneColor)
imgResult = cv2.addWeighted(imgResult, 1, imgLaneColor, 1, 0)
midY = 450
cv2.putText(imgResult, str(curve), (wT // 2 - 80, 85), cv2.FONT_HERSHEY_COMPLEX, 2, (255, 0, 255), 3)
cv2.line(imgResult, (wT // 2, midY), (wT // 2 + (curve * 3), midY), (255, 0, 255), 5)
cv2.line(imgResult, ((wT // 2 + (curve * 3)), midY - 25), (wT // 2 + (curve * 3), midY + 25), (0, 255, 0), 5)
for x in range(-30, 30):
w = wT // 20
cv2.line(imgResult, (w * x + int(curve // 50), midY - 10),
(w * x + int(curve // 50), midY + 10), (0, 0, 255), 2)
#fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
#cv2.putText(imgResult, 'FPS ' + str(int(fps)), (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (230, 50, 50), 3);
if display == 2:
imgStacked = utils.stackImages(0.7, ([img, imgWarpPoints, imgWarp],
[imgHist, imgLaneColor, imgResult]))
cv2.imshow('ImageStack', imgStacked)
elif display == 1:
cv2.imshow('Resutlt', imgResult)
#--- Define Tag
id_to_find = 0 #id of the aruco marker to be found is 0
marker_size = 4 #marker size in centimeters is 4
#--- Get the camera calibration path
calib_path = "/home/pi/mu_code/SeniorDesign/Aruco_pose_est/"
#-- Load camera matrix and camera distortion
camera_matrix = np.load(calib_path+'camera_matrix.npy')
camera_distortion = np.load(calib_path+'camera_distortion.npy')
#--- 180 deg rotation matrix around the x axis
R_flip = np.zeros((3,3), dtype=np.float32)
R_flip[0,0] = 1.0
R_flip[1,1] =-1.0
R_flip[2,2] =-1.0
#--- Define the aruco dictionary
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
#-- Convert in gray scale
gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) #remember, OpenCV stores color images in Blue, Green, Red
#-- Find all the aruco markers in the image
corners, ids, rejected = aruco.detectMarkers(image=gray, dictionary=aruco_dict, parameters=parameters) #, cameraMatrix=camera_matrix, distCoeff=camera_distortion)
#-- If an aruco marker is found check if it is a marker with id 0
if ids is not None and ids[0] == id_to_find:
ret = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix, camera_distortion) #
rvec, tvec = ret[0][0,0,:], ret[1][0,0,:]
distance = tvec[2]
current_velocity = 0 #initial velocity
distance = round(distance, 2)
if distance < 1:
current_velocity_right = 0
current_velocity_left = 0
else if curve < 0:
current_velocity_right = distance * .8
current_velocity_left = (distance * .8) + curve
else if curve > 0:
current_velocity_right = distance * .8 + curve
current_velocity_left = distance * .8
else:
current_velocity_right = distance * .8
current_velocity_left = distance * .8
r.ChangeDutyCycle(current_velocity_right)
l.ChangeDutyCycle(current_velocity_left)
else if (curve < 0):
print('SLOW LEFT WHEEL!!!')
r.ChangeDutyCycle(15)
l.ChangeDutyCycle(12)
else if (curve > 0):
print('SLOW RIGHT WHEEL!!!')
r.ChangeDutyCycle(12)
l.ChangeDutyCycle(15)
else:
r.ChangeDutyCycle(20)
l.ChangeDutyCycle(20)
def getLaneCurve(img, display=2):
imgCopy = img.copy()
imgResult = img.copy()
#### STEP 1
imgThres = utils.thresholding(img)
#### STEP 2
hT, wT, c = img.shape
points = utils.valTrackbars()
imgWarp = utils.warpImg(imgThres, points, wT, hT)
imgWarpPoints = utils.drawPoints(imgCopy, points)
#### STEP 3
middlePoint, imgHist = utils.getHistogram(imgWarp,
display=True,
minPer=0.5,
region=4)
curveAveragePoint, imgHist = utils.getHistogram(imgWarp,
display=True,
minPer=0.9)
curveRaw = curveAveragePoint - middlePoint
#### SETP 4
curveList.append(curveRaw)
if len(curveList) > avgVal:
curveList.pop(0)
curve = int(sum(curveList) / len(curveList))
#### STEP 5
if display != 0:
imgInvWarp = utils.warpImg(imgWarp, points, wT, hT, inv=True)
imgInvWarp = cv2.cvtColor(imgInvWarp, cv2.COLOR_GRAY2BGR)
imgInvWarp[0:hT // 3, 0:wT] = 0, 0, 0
imgLaneColor = np.zeros_like(img)
imgLaneColor[:] = 0, 255, 0
imgLaneColor = cv2.bitwise_and(imgInvWarp, imgLaneColor)
imgResult = cv2.addWeighted(imgResult, 1, imgLaneColor, 1, 0)
midY = 450
cv2.putText(imgResult, str(curve), (wT // 2 - 80, 85),
cv2.FONT_HERSHEY_COMPLEX, 2, (255, 0, 255), 3)
cv2.line(imgResult, (wT // 2, midY), (wT // 2 + (curve * 3), midY),
(255, 0, 255), 5)
cv2.line(imgResult, ((wT // 2 + (curve * 3)), midY - 25),
(wT // 2 + (curve * 3), midY + 25), (0, 255, 0), 5)
for x in range(-30, 30):
w = wT // 20
cv2.line(imgResult, (w * x + int(curve // 50), midY - 10),
(w * x + int(curve // 50), midY + 10), (0, 0, 255), 2)
#fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
#cv2.putText(imgResult, 'FPS ' + str(int(fps)), (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (230, 50, 50), 3);
if display == 2:
imgStacked = utils.stackImages(0.7,
([img, imgWarpPoints, imgWarp],
[imgHist, imgLaneColor, imgResult]))
cv2.imshow('ImageStack', imgStacked)
elif display == 1:
cv2.imshow('Resutlt', imgResult)
#### NORMALIZATION
curve = curve / 100
if curve > 1: curve == 1
if curve < -1: curve == -1
return curve
def getLaneCurve(img, display=2):
#sent an image and get the value of curve
imgCopy = img.copy()
imgResult = img.copy()
######step 1
imgThresh = utils.thresholding(img)
######step 2
hT, wT, c = img.shape
#for points we need trackbar
points = utils.valTrackbars()
imgWarp = utils.warpImg(imgThresh, points, wT, hT)
imgWarpPoints = utils.drawPoints(imgCopy, points)
#############step 3
middlePoint, imgHist = utils.getHistogram(imgWarp,
display=True,
minPer=0.5,
region=4)
curveAveragePoint, imgHist = utils.getHistogram(imgWarp,
display=True,
minPer=0.9)
#print(basePoint - midPoint)
curveRaw = curveAveragePoint - middlePoint
##########step 4 (avrg)
#avrging because want to smooth transition>> noise reduce happens
curveList.append(curveRaw)
if len(curveList) > avrgVal:
curveList.pop(
0
) # maane koyta niye avrg korbo otar beshi batch hoye gele frst er ta baad diye dibe
curve = int(sum(curveList) / len(curveList))
#####Step 5 Display
if display != 0:
imgInvWarp = utils.warpImg(imgWarp, points, wT, hT, inv=True)
imgInvWarp = cv2.cvtColor(imgInvWarp, cv2.COLOR_GRAY2BGR)
imgInvWarp[0:hT // 3, 0:wT] = 0, 0, 0
imgLaneColor = np.zeros_like(img)
imgLaneColor[:] = 0, 255, 0
imgLaneColor = cv2.bitwise_and(imgInvWarp, imgLaneColor)
imgResult = cv2.addWeighted(imgResult, 1, imgLaneColor, 1, 0)
midY = 450
cv2.putText(imgResult, str(curve), (wT // 2 - 80, 85),
cv2.FONT_HERSHEY_COMPLEX, 2, (255, 0, 255), 3)
cv2.line(imgResult, (wT // 2, midY), (wT // 2 + (curve * 3), midY),
(255, 0, 255), 5)
cv2.line(imgResult, ((wT // 2 + (curve * 3)), midY - 25),
(wT // 2 + (curve * 3), midY + 25), (0, 255, 0), 5)
for x in range(-30, 30):
w = wT // 20
cv2.line(imgResult, (w * x + int(curve // 50), midY - 10),
(w * x + int(curve // 50), midY + 10), (0, 0, 255), 2)
#fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
#cv2.putText(imgResult, 'FPS '+str(int(fps)), (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (230,50,50), 3);
if display == 2:
imgStacked = utils.stackImages(0.7,
([img, imgWarpPoints, imgWarp],
[imgHist, imgLaneColor, imgResult]))
cv2.imshow('ImageStack', imgStacked)
elif display == 1:
cv2.imshow('Result', imgResult)
#normalization
curve = curve / 100
if curve > 1: curve = 1
if curve < -1: curve = -1
'''
cv2.imshow('Threshold', imgThresh)
cv2.imshow('Warp', imgWarp)
cv2.imshow('Warp Points', imgWarpPoints)
cv2.imshow('Histogram', imgHist)
'''
return curve
def getLaneCurve(img, display=2):
# create a copy image to display warp points
imgCopy = img.copy()
imgResult = img.copy()
##1 First find the threshold image
imgThres = utils.thresholding(img)
##2 Now find the warped tracking coordinates
hT, wT, c = img.shape
points = utils.valTrackbars()
imgWarp = utils.warpImg(imgThres, points, wT, hT)
imgWarpPoints = utils.drawPoints(imgCopy, points)
##3 We have to find the center of the base which will give us the center line and
# then compare the pixels on both side. By summation of these pixels we are basically
# finding the histogram
middlePoint, imgHist = utils.getHistogram(imgWarp, display=True, minPer=0.5, region=4)
# optimize curve for finding centre point for path incase of straight path but unsymmetrical histogram
curveAveragePoint, imgHist = utils.getHistogram(imgWarp, display=True, minPer=0.9)
# subtract this value from the center to get the curve value.
curveRaw = curveAveragePoint - middlePoint
##4 Averaging the curve value for smooth transitioning
curveList.append(curveRaw)
if len(curveList)>avgVal:
curveList.pop(0)
curve = int(sum(curveList)/len(curveList))
##5 Display
if display != 0:
imgInvWarp = utils.warpImg(imgWarp, points, wT, hT, inv=True)
imgInvWarp = cv2.cvtColor(imgInvWarp, cv2.COLOR_GRAY2BGR)
imgInvWarp[0:hT // 3, 0:wT] = 0, 0, 0
imgLaneColor = np.zeros_like(img)
imgLaneColor[:] = 0, 255, 0
imgLaneColor = cv2.bitwise_and(imgInvWarp, imgLaneColor)
imgResult = cv2.addWeighted(imgResult, 1, imgLaneColor, 1, 0)
midY = 450
cv2.putText(imgResult, str(curve), (wT // 2 - 80, 85), cv2.FONT_HERSHEY_COMPLEX, 2, (255, 0, 255), 3)
cv2.line(imgResult, (wT // 2, midY), (wT // 2 + (curve * 3), midY), (255, 0, 255), 5)
cv2.line(imgResult, ((wT // 2 + (curve * 3)), midY - 25), (wT // 2 + (curve * 3), midY + 25), (0, 255, 0), 5)
for x in range(-30, 30):
w = wT // 20
cv2.line(imgResult, (w * x + int(curve // 50), midY - 10),
(w * x + int(curve // 50), midY + 10), (0, 0, 255), 2)
#fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer);
#cv2.putText(imgResult, 'FPS ' + str(int(fps)), (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (230, 50, 50), 3);
if display == 2:
# stack all the windows together (just for display purposes, no other requirement)
imgStacked = utils.stackImages(0.7, ([img, imgWarpPoints, imgWarp],
[imgHist, imgLaneColor, imgResult]))
cv2.imshow('ImageStack', imgStacked)
elif display == 1:
cv2.imshow('Result', imgResult)
### NORMALIZATION
curve = curve/100
if curve>1: curve = 1
if curve<-1: curve = -1
return curve