def average_slope_intercept(lines):
left_lines = [] # (slope, intercept)
left_weights = [] # (length,)
right_lines = [] # (slope, intercept)
right_weights = [] # (length,)
#print ("Line: ", lines)
for line in lines:
for x1, y1, x2, y2 in line:
if x2 == x1:
continue # ignore a vertical line
slope = (y2 - y1) / (x2 - x1)
intercept = y1 - slope * x1
length = np.sqrt((y2 - y1)**2 + (x2 - x1)**2)
#For PID calculations ( steering )
msg = coords()
msg.Dist = slope
msg.X = x2
msg.Y = y2
pub.publish(msg)
if slope < 0: # y is reversed in image
left_lines.append((slope, intercept))
left_weights.append((length))
else:
right_lines.append((slope, intercept))
right_weights.append((length))
# add more weight to longer lines
left_lane = np.dot(left_weights, left_lines) / np.sum(
left_weights) if len(left_weights) > 0 else None
right_lane = np.dot(right_weights, right_lines) / np.sum(
right_weights) if len(right_weights) > 0 else None
print("left_lane", left_lane)
print("right_lane", right_lane)
return left_lane, right_lane # (slope, intercept), (slope, intercept)
def average_slope_intercept(lines):
global publication_topic
left_lines = [] # (slope, intercept)
left_weights = [] # (length,)
right_lines = [] # (slope, intercept)
right_weights = [] # (length,)
#print("---------------------------")
#print ("Lines size: ", len(lines))
#print ("Line: ")
#print (lines)
tam_lines = len(lines) #number of lines
#print ("Line[2]: ", lines[2])
#print ("Array")
#lines_array = np.array(lines)
#print (lines_array)
#print (lines_array[0,1])
#modificado na versao 0.3 do codigo
#i = 0
sum_x1 = 0
sum_x2 = 0
sum_y1 = 0
sum_y2 = 0
for line in lines:
for x1, y1, x2, y2 in line:
sum_x1 = sum_x1 + x1
sum_x2 = sum_x2 + x2
sum_y1 = sum_y1 + y1
sum_y2 = sum_y2 + y2
if x2 == x1:
continue # ignore a vertical line
slope = (y2 - y1) / (x2 - x1)
intercept = y1 - slope * x1
length = np.sqrt((y2 - y1)**2 + (x2 - x1)**2)
x1 = sum_x1 / tam_lines
x2 = sum_x2 / tam_lines
y1 = sum_y1 / tam_lines
y2 = sum_y2 / tam_lines
if (x2 <> x1):
slope = (y2 - y1) / (x2 - x1)
else:
slope = 0
intercept = y1 - slope * x1
length = np.sqrt((y2 - y1)**2 + (x2 - x1)**2)
#For PID calculations ( steering )
msg = coords()
msg.length = length
msg.slope = slope
msg.intercept = intercept
msg.X2 = x2
msg.X1 = x1
msg.dif_X = x2 - x1
msg.Y2 = y2
msg.Y1 = y1
msg.dif_Y = y2 - y1
pub = rospy.Publisher(publication_topic, coords, queue_size=10)
pub.publish(msg)
# add more weight to longer lines
left_lane = np.dot(left_weights, left_lines) / np.sum(
left_weights) if len(left_weights) > 0 else None
right_lane = np.dot(right_weights, right_lines) / np.sum(
right_weights) if len(right_weights) > 0 else None
#print("left_lane", left_lane)
#print("right_lane", right_lane)
return left_lane, right_lane # (slope, intercept), (slope, intercept)