def compute_center(classes, feat_col, point_cloud):
#Rotate the coordinates in the point cloud as if the camera was looking straight ahead
point_cloud_ = g.rotate_pc(point_cloud)
#Do some morphological operations on the classification, to smooth the edges.
#Since a large part of the classifier works on 64*64 blocks this improves the accuracy
kernel = np.ones((9,9),np.uint8) #(15,15)
classes = classes.astype(np.uint8)
median = cv2.medianBlur(classes, 15)#(31, 31)
opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, kernel)
closing = cv2.morphologyEx(opening, cv2.MORPH_CLOSE, kernel)
#find the contours in the classes image. These contours are pieces of road.
im2, contours, hierarchy = cv2.findContours(closing, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
cont_img = np.array(feat_col)
if len(contours) != 0:
#if any contours were found, find the largest of them
c = max(contours, key = cv2.contourArea)
#lookup the coordinates if the contour points in the point cloud
cont = g.pcl_lookup(c, point_cloud_)
cont = np.int32(cont)
#create an image of the contours in
road_geometry = np.zeros((400, 800, 3), dtype = 'uint8')
cv2.drawContours(road_geometry, [cont], -1, (255,255,255), -1)
scaling_factor = 50 #convert back to milimetres
centering_factor = 20000 #recenter the points
lower_limit_road = 3200 #These is the region to search for the road
upper_limit_road = 3600 #They are also used as handle length, since they define the
#distance from the cart to the point
#This section can be used if non-rectangular regions are desired
#a = np.zeros((400, 800))
#a[lower_limit_road//scaling_factor:upper_limit_road//scaling_factor, ...]=1 #define the region we are looking for the center points of
#b=np.logical_and(road_geometry[..., 0], a).astype('uint8') #mask the image to the region we are looking at
#find contours of the part of the image we want to find the handle in
road_cont = cv2.findContours(road_geometry[lower_limit_road//scaling_factor:upper_limit_road//scaling_factor,:,0].copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
#compute the moments of the contour
M = cv2.moments(road_cont[0])
#find the center point of the contour
if M["m00"] > 1:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
center_point = [cX, cY]
center_point[0] = center_point [0]*scaling_factor - centering_factor
center_point[1] = center_point [1]*scaling_factor + lower_limit_road
return center_point
return [0,0] #if no point was found or the "mass" of the found contour is 0, return 0,0
def show(classes, feat_col, feat_img, point_cloud):
blank_image = np.zeros((np.shape(feat_col)[0], np.shape(feat_col)[1], 3),
np.uint8)
classified = np.dstack((blank_image, classes))
for k in range(0, blank_image.shape[0]):
for l in range(0, blank_image.shape[1]):
if classified[k, l, 3] == 1:
blank_image[k, l] = (255, 255, 255)
elif classified[k, l, 3] == 0:
blank_image[k, l] = (0, 0, 0)
feat_img = cv2.cvtColor(blank_image, cv2.COLOR_BGR2RGB)
kernel = np.ones((9, 9), np.uint8) #(15,15)
classes = classes.astype(np.uint8)
median = cv2.medianBlur(classes, 15) #(31, 31)
opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, kernel)
closing = cv2.morphologyEx(opening, cv2.MORPH_CLOSE, kernel)
im2, contours, hierarchy = cv2.findContours(closing, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cont_img = np.array(feat_col)
if len(contours) != 0:
c = max(contours, key=cv2.contourArea)
blank_img = np.zeros((np.shape(feat_col)[0], np.shape(feat_col)[1], 3),
np.uint8)
cv2.drawContours(blank_image, [c], -1, (0, 0, 255), 10)
median1 = cv2.medianBlur(blank_image, 15) #(31, 31)
opening1 = cv2.morphologyEx(median1, cv2.MORPH_OPEN, kernel)
closing1 = cv2.morphologyEx(opening1, cv2.MORPH_CLOSE, kernel)
#print(c[0][0][1], c.shape, type(c))
#x,y,w,h = cv2.boundingRect(c)
#cv2.rectangle(cont_img,(x,y),(x+w,y+h),(0,255,0),2)
#print(x, y, w, h)
#cropped = cont_img[y:y+h, x:x+w]
#cropped1 = closing[y:y+h, x:x+w]
#Perspective transform
#Perspective matricies both regular and inverse
#Pts = np.float32([[0, 0], [0, h], [w, h], [w, 0]])
#Pts_inv = np.float32([[0, 0], [w-(w/1.5), h], [w-(w/2), h], [w, 0]]) #np.float32([[500, dims[0]], [700, dims[0]], [0, 0], [dims[1], 0]]) (w/1.5) (w/3)
#M =cv2.getPerspectiveTransform(Pts, Pts_inv)
#Minv = cv2.getPerspectiveTransform(Pts, Pts_inv)
#warped_img = cv2.warpPerspective(cropped1, M, (w, h))
point_cloud = np.asarray(point_cloud)
cont = g.pcl_lookup(c, point_cloud)
print(c.dtype)
cont = np.int32(cont)
road_geometry = np.zeros((400, 800, 3), dtype='uint8')
cv2.drawContours(road_geometry, [cont], -1, (255, 255, 255), -1)
center_points = []
for i in range(10):
a = np.zeros((400, 800))
a[i * 40:i * 40 + 40, ...] = 1
b = np.logical_and(road_geometry[..., 0], a).astype('uint8')
#print(road_geometry[0:40,:,0], road_geometry[0:40,:,0].dtype)
#cv2.rectangle(road_geometry,(0, 40*i),(800, 40),(0,255,0),1)
road_cont = cv2.findContours(
road_geometry[i * 40:i * 40 + 40, :, 0].copy(),
cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
M = cv2.moments(road_cont[0])
if M["m00"] > 1:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
center_points.append((cX, cY + i * 40))
for points in center_points:
cv2.circle(road_geometry, points, 5, (255, 0, 0), -1)
svr_points = np.asarray(center_points)
polyapprox = np.polyfit(svr_points[:, 1], svr_points[:, 0], 3)
print(polyapprox)
approximation = np.asarray(
(svr_points[:, 1] * svr_points[:, 1] * polyapprox[0] +
svr_points[:, 1] * polyapprox[1] + polyapprox[2], svr_points[:,
1]),
dtype='uint16').transpose()
print(approximation)
for points in zip(approximation[1:, :], approximation[:-1, :]):
print(points)
cv2.line(road_geometry, (points[0][0], points[0][1]),
(points[1][0], points[1][1]), (0, 255, 0), 3)
#cv2.line(road_geometry, (400, 0), (400, 170), (255, 0 , 125), 3)
#cv2.line(road_geometry, (400, 0), (svr_points[4][0], svr_points[4][1]), (0, 0, 255), 3)
'''
im3, contours1, hierarchy = cv2.findContours(road_geometry, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
M = cv2.moments(contours1[0])
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
'''
# cv2.circle(cont_img, (cX, cY+600), 7, (255, 0, 0), -1)
plt.title('Road Contour')
plt.imshow(closing1)
plt.show()
#plt.subplot(233)
#plt.imshow(warped_img)
#plt.title('Perspective Transformation')
plt.subplot(234)
plt.imshow(road_geometry, origin='lower')
plt.title('Point Cloud Geometry')
plt.subplot(235)
plt.axis('equal')
plt.scatter(svr_points[:, 0],
svr_points[:, 1],
color='darkorange',
label='data')
plt.plot(svr_points[:, 1] * svr_points[:, 1] * polyapprox[0] +
svr_points[:, 1] * polyapprox[1] + polyapprox[2],
svr_points[:, 1],
color='navy',
lw=2,
label='RBF model')
plt.title("Road Model")
cont_img = cv2.cvtColor(cont_img, cv2.COLOR_BGR2RGB)
plt.subplot(232)
plt.imshow(cont_img)
plt.title('Original Image')
plt.subplot(231)
plt.imshow(closing, cmap='gray')
red_patch = mpatches.Patch(color='white', label='road')
green_patch = mpatches.Patch(color='black', label='non-road')
plt.legend(handles=[red_patch, green_patch])
plt.title('Road Extraction')
plt.show()
def show(classes, feat_col, point_cloud):
import matplotlib.pyplot as plt
point_cloud_ = g.rotate_pc(point_cloud)
blank_image = np.zeros((np.shape(feat_col)[0], np.shape(feat_col)[1],3), np.uint8)
classified = np.dstack((blank_image, classes))
for k in range(0, blank_image.shape[0]):
for l in range(0, blank_image.shape[1]):
if classified[k,l,3] == 1:
blank_image[k, l] = (255, 255, 255)
elif classified[k,l,3] == 0:
blank_image[k, l] = (0, 0, 0)
feat_img = cv2.cvtColor(blank_image, cv2.COLOR_BGR2RGB)
kernel = np.ones((9,9),np.uint8) #(15,15)
classes = classes.astype(np.uint8)
median = cv2.medianBlur(classes, 15)#(31, 31)
opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, kernel)
closing = cv2.morphologyEx(opening, cv2.MORPH_CLOSE, kernel)
im2, contours, hierarchy = cv2.findContours(closing, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
cont_img = np.array(feat_col)
if len(contours) != 0:
c = max(contours, key = cv2.contourArea)
#blank_img = np.zeros((np.shape(feat_col)[0], np.shape(feat_col)[1],3), np.uint8)
cv2.drawContours(blank_image, [c], -1, (0,0,255), 10)
median1 = cv2.medianBlur(blank_image, 15)#(31, 31)
opening1 = cv2.morphologyEx(median1, cv2.MORPH_OPEN, kernel)
closing1 = cv2.morphologyEx(opening1, cv2.MORPH_CLOSE, kernel)
cont = g.pcl_lookup(c, point_cloud_)
cont = np.int32(cont)
road_geometry = np.zeros((400, 800, 3), dtype = 'uint8')
cv2.drawContours(road_geometry, [cont], -1, (255,255,255), -1)
center_points = []
center_points_mm = []
scaling_factor = 50 #convert back to milimetres
centering_factor = 20000 #recenter the points
lower_limit_road = 1600
upper_limit_road = 2000
for i in range(10):
a = np.zeros((400, 800))
#a[i*40:i*40+40, ...]=1
cv2.circle(a, (400,0), (i+1)*40, 1, -1)
cv2.circle(a, (400,0), i*40, 0, -1)
cv2.rectangle(road_geometry,(0, 0),(1280, 40*i),(255,255,255),3)
b=np.logical_and(road_geometry[..., 0], a).astype('uint8')
#cv2.rectangle(cont_img,(0, 600),(1280, 400),(255,255,255),6)
road_cont = cv2.findContours(road_geometry[i*40:i*40+40,:,0].copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
M = cv2.moments(road_cont[0])
if M["m00"] > 1:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
center_point = [cX, cY]
center_points.append((cX, cY + i*40))
center_point[0] = center_point [0]*scaling_factor - centering_factor
center_point[1] = center_point [1]*scaling_factor + i*40*scaling_factor
center_points_mm.append(tuple(center_point))
ori_centers = []
for points in center_points_mm:
cv2.circle(road_geometry, points, 5, (255, 0, 0), -1)
ori_center = ori_lookup(point_cloud_, points)
#print(ori_center)
#cv2.circle(cont_img, ori_center, 5, (255, 0, 0), -1)
ori_centers.append(ori_center)
svr_points = np.asarray(center_points)
polyapprox = np.polyfit(svr_points[:, 1], svr_points[:, 0], 2)
approximation = np.asarray((svr_points[:,1]*svr_points[:,1]*polyapprox[0]+ svr_points[:,1]*polyapprox[1]+polyapprox[2], svr_points[:,1]), dtype='uint16').transpose()
#print (approximation)
#for points in zip(approximation[1:, :], approximation[:-1, :]):
#print(points)
#cv2.line(road_geometry, (points[0][0], points[0][1]), (points[1][0], points[1][1]), (0, 255, 0), 3)
#cv2.line(road_geometry, (400, 0), (400, 170), (255, 0 , 125), 3)
#cv2.line(road_geometry, (400, 0), (svr_points[4][0], svr_points[4][1]), (0, 0, 255), 3)
'''
im3, contours1, hierarchy = cv2.findContours(road_geometry, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
M = cv2.moments(contours1[0])
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
'''
# cv2.circle(cont_img, (cX, cY+600), 7, (255, 0, 0), -1)
plt.title('Road Contour')
plt.imshow(closing1)
plt.show()
plt.subplot(233)
plt.imshow(feat_col)
plt.title('Perspective Transformation')
plt.subplot(234)
plt.imshow(road_geometry,origin='lower')
plt.title('Point Cloud Geometry')
plt.subplot(235)
plt.axis('equal')
plt.scatter(svr_points[:,0], svr_points[:, 1], color='darkorange', label='data')
plt.plot(svr_points[:,1]*svr_points[:,1]*polyapprox[0]+ svr_points[:,1]*polyapprox[1]+polyapprox[2], svr_points[:,1], color='navy', lw=2, label='RBF model')
plt.title("Road Model")
cont_img = cv2.cvtColor(cont_img, cv2.COLOR_BGR2RGB)
plt.subplot(232)
plt.imshow(cont_img)
plt.title('Original Image')
plt.subplot(231)
plt.imshow(closing, cmap='gray')
red_patch = mpatches.Patch(color='white', label='road')
green_patch = mpatches.Patch(color='black', label='non-road')
plt.legend(handles=[red_patch, green_patch])
plt.title('Road Extraction')
plt.show()