def AssertBlobDistanceCalibrated(self):
'''
@brief Assert that the blob distance is calibrated
Raises exception blob_distance_is_not_calibrated_error_msg if the standard blob distance isn't calibrated.
'''
if not (self.__minDistBetweenBlobs_calibrated):
raise DroneVisionError('blob_distance_is_not_calibrated_error_msg')
def DetectBlobs(self,
frame,
compute_descriptors=False,
ignore_no_blobs_error=False):
'''
@brief Detect blobs (points) in a frame.
@param frame
@param compute_descriptors (Default=False)
@param ignore_no_blobs_error (True/False)
@return keypoints, descriptors (Returns keypoints = blob keypoints as a list, descriptors = blob descriptors as a list
Position and size of blob is found by:
for blob in keypoints:
blob.pt[0] #x
blob.pt[1] #y
blob.size #size )
'''
# Detect blobs. Frame consist only of highlighted points (blobs), so mask is equal to the frame (mask shows all points of interest which is all non-zero values).
keypoints = self.__keypoint_detector.detect(frame, mask=frame)
if compute_descriptors:
self.AssertFeatureDesctriptorAvailable()
keypoints, descriptors = self.__descriptor_detector.compute(
frame, keypoints=keypoints)
else:
descriptors = np.zeros(len(keypoints))
if len(keypoints) == 0 and not (ignore_no_blobs_error):
raise DroneVisionError('no_blobs_error_msg')
return keypoints, descriptors
def AssertFeatureDesctriptorAvailable(self):
'''
@brief Assert that the feature descriptor is available
Raises exception feature_descriptor_not_available_error_msg if it isn't available.
'''
if not (self.__descriptor_available):
raise DroneVisionError(
'feature_descriptor_not_available_error_msg')
def HandleErrorNoEdgeHeadingDetected(self):
'''
@brief Handle exception of no edge heading detected.
Raises warning wit error_msg_no_backup_heading_available message.
Will return backup heading, based in previous headings.
Will raise error_msg_no_backup_heading_available exception if current_heading == None (not available)
@return selected_heading (rho, theta)
'''
blade_heading, tip_heading = self.GetCurrentHeading()
if blade_heading[0] == None:
raise DroneVisionError('error_msg_no_backup_heading_available')
return blade_heading, tip_heading
def LinearLSTriangulation(self,
key_point1,
P1,
key_point2,
P2,
wi1=1.0,
wi2=1.0):
'''
@brief Triangulate points in 3D space.
Harley & Zisserman triangulation.
Inspired by: http://www.morethantechnical.com/2012/01/04/simple-triangulation-with-opencv-from-harley-zisserman-w-code/
@param key_point1 (homogenous image point (u,v,1) - cv2 key point)
@param P1 (camera 1 matrix (projection matrix 3x4))
@param key_point2 (homogenous image point in 2nd camera (u,v,1) - cv2 key point)
@param P2 (camera 2 matrix (projection matrix 3x4))
@param wi1 (weight for camera 1 - used for the iterative method)
@param wi2 (weight for camera 2 - used for the iterative method)
@return X (4x1 matrix as [x,y,z,1]^T)
'''
#build matrix A for homogenous equation system Ax = 0
#assume X = (x,y,z,1), for Linear-LS method,
#which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
A = np.zeros((4, 3))
X = np.zeros((3, 1))
B = np.zeros((4, 1))
# Creating A matrice - each column depends
for i in range(3):
A[0, i] = (key_point1.pt[0] * P1[2, i] - P1[0, i]) / wi1
A[1, i] = (key_point1.pt[1] * P1[2, i] - P1[1, i]) / wi1
A[2, i] = (key_point2.pt[0] * P2[2, i] - P2[0, i]) / wi2
A[3, i] = (key_point2.pt[1] * P2[2, i] - P2[1, i]) / wi2
B[0, 0] = -(key_point1.pt[0] * P1[2, 3] - P1[0, 3]) / wi1
B[1, 0] = -(key_point1.pt[1] * P1[2, 3] - P1[1, 3]) / wi1
B[2, 0] = -(key_point2.pt[0] * P2[2, 3] - P2[0, 3]) / wi2
B[3, 0] = -(key_point2.pt[1] * P2[2, 3] - P2[1, 3]) / wi2
flags = cv2.DECOMP_SVD
retval, X = cv2.solve(A, B, flags=flags)
if not (retval):
raise DroneVisionError('triangulation_error_msg')
return np.vstack([X,
np.ones(1)]) #append row with 1 to create 4x1 vector
def ComputeHeading(self, frame, boundary_hough_lines, draw_possible_edge_headings=False, draw_headings=False, draw_elliptical_error_arrow=False, draw_min_rho=True):
'''
@brief Compute heading according to the boundary_hough_lines.
All headings are measured from image center, meaning returned rho and theta are in regard to the image center.
Set draw_possible_edge_headings=True to return a colored frame showing all edge headings.
Set draw_headings=True to return a colored frame showing the heading.
The function considers the current heading to find the most probable next heading.
Use SetCurrentHeading() to update the current heading, or GetCurrentHeading() to get it.
@param frame (undistorted frame)
@param boundary_hough_lines (See PointDetection.GetBoundaryHoughLines)
boundary_hough_lines = [max_hor_hough_line, min_hor_hough_line, max_vert_hough_line, min_vert_hough_line],
with hough line as (rho, theta)
@param draw_possible_edge_headings (default=False)
@param draw_headings (YELLOW = selected edge heading, ORANGE = selected heading, RED = selected tip/root heading (default=False))
@param draw_elliptical_error_arrow (DARK RED = drawn elliptical error arrow (True/False - default=True))
@param draw_min_rho (Draw minium rho of draw_elliptical_error_arrow == True (True/False - default=True))
@return selected_blade_heading, selected_tip_or_root_heading, tip_or_root_detected, frame
(Return
selected_blade_heading = (rho, theta) (Will be (None, None) if not detected),
selected_tip_or_root_heading = (rho, theta) (Will be (None, None) if not detected),
tip_or_root_detected = True/False
frame
rho = distance to edge, theta = heading direction in radian degrees.
frame (if draw_possible_edge_headings or draw_headings = True, then it is converted to color frame))
'''
try:
edge_heading_returns = self.ComputeEdgeHeading(frame, boundary_hough_lines, draw=draw_possible_edge_headings)
except DroneVisionError, err:
if str(err) == str(DroneVisionError('error_msg_no_heading_angle_detected')):
warnings.simplefilter('always')
warnings.warn(str(err), Warning)
warnings.simplefilter('default')
return self.HandleErrorNoEdgeHeadingDetected()
raise
def ComputeEdgeHeading(self, frame, boundary_hough_lines, draw=False):
'''
@brief Compute heading to the most significant edge based on the boundary hough lines.
Raises self.error_msg_no_heading_angle_detected if no heading angles are detected.
All edge headings are measured from image center, meaning returned rho and theta are in regard to the image center.
The edge heading is perpendicular to the corresponding edge.
Set draw=True to return a colored frame showing the edge headings
The function considers the current edge heading to find the most probable next edge heading.
Use SetCurrentEdgeHeading() to update the current edge heading, or GetCurrentEdgeHeading() to get it.
rho is in range between <0, frame_size>
theta is in range between <0, 2pi>
@param frame (undistorted frame)
@param boundary_hough_lines (See PointDetection.GetBoundaryHoughLines)
boundary_hough_lines = [max_hor_hough_line, min_hor_hough_line, max_vert_hough_line, min_vert_hough_line],
with hough line as (rho, theta)
@param draw (default=False)
if draw == True:
@return selected_hor_edge_heading, selected_vert_edge_heading, possible_hor_edge_headings, possible_vert_edge_headings, frame (frame = updated color frame)
else:
@return selected_hor_edge_heading, selected_vert_edge_heading, possible_hor_edge_headings, possible_vert_edge_headings
(Return
selected_hor_edge_heading = closest horizontal edge heading to previous horizontal edge heading ((None, None, None, None), if there were no detected horizontal edges)
selected_vert_edge_heading = closest vertical edge heading to previous vertical edge heading ((None, None, None, None), if there were no detected vertical edges)
possible_hor_edge_headings = [(rho, theta, max/min, hor/vert),..] in horizontal direction
possible_vert_edge_headings = [(rho, theta, max/min, hor/vert),..] in vertical direction
(rho = distance to edge, theta = heading direction in radian degrees., max/min = True/False according to if it is the max or min boundary line, hor/vert = True/False - True for horizontal, False for vertical)
'''
possible_hor_edge_headings = []
possible_vert_edge_headings = []
# for drawing: max_hor = DARK BLUE, min_hor = LIGHT BLUE, max_vert = PURPLE, min_vert = GREEN
colors = [(0, 0, 255), (0, 255, 255), (204, 0, 204), (0, 255, 0)]
n_boundary_hough_lines = len(boundary_hough_lines)
for i in range(n_boundary_hough_lines):
if i % 2 == 0:
max_line = True
else:
max_line = False
proc_returns = self.ProcessBoundaryHoughLine(
frame, boundary_hough_lines[i], draw=draw, color=colors[i])
edge_line, rho, theta = proc_returns[:3]
if draw:
frame = proc_returns[3]
if edge_line:
if i < n_boundary_hough_lines / 2: # horizontal lines
possible_hor_edge_headings.append(
(rho, theta, max_line, True))
else: # vertical lines
possible_vert_edge_headings.append(
(rho, theta, max_line, False))
if (len(possible_hor_edge_headings) +
len(possible_vert_edge_headings)) == 0:
raise DroneVisionError('error_msg_no_heading_angle_detected')
current_hor_edge_heading, current_vert_edge_heading = self.GetCurrentEdgeHeadings(
)
selected_hor_edge_heading = self.FindMostSignificantEdgeHeading(
possible_hor_edge_headings, current_hor_edge_heading)
selected_vert_edge_heading = self.FindMostSignificantEdgeHeading(
possible_vert_edge_headings, current_vert_edge_heading)
if draw:
return selected_hor_edge_heading, selected_vert_edge_heading, possible_hor_edge_headings, possible_vert_edge_headings, frame
return selected_hor_edge_heading, selected_vert_edge_heading, possible_hor_edge_headings, possible_vert_edge_headings #else
def DetectBoundaryEdges(origin_frame,
bounded_lines,
max_min_lines,
scale_threshold=1.0,
line_perc=2.0 / 3,
filtrate_edge_points=True,
draw=False,
print_hough_positions=False):
'''
@brief Detect all boundary edges, and compute the corresponding line.
Step 1:
For each line in (max_hor_line, min_hor_line, max_vert_line, min_vert_line):
detect if boundary indicates an open space towards the frame edges, indicating end of the blade.
Step 2:
If any max/min boundary line indicates end of blade (detected end of blade region), then
find all blade edge points by using DetectLineEdge, starting from each horizontal/vertical max/min point and towards the frame end point.
Step 3:
Use hough transform to derive the longest and most significant line for each detected blade edge.
@param origin_frame (Original grayscale frame without laser points)
@param bounded_lines (bounded lines from FindLineLimits)
@param max_min_lines (List of max min horizontal and vertical lines, in this order:
- max_hor_line (max_hor_line from FindLineLimits)
- min_hor_line (min_hor_line from FindLineLimits)
- max_vert_line (max_vert_line from FindLineLimits)
- min_vert_line (min_vert_lines from FindLineLimits))
@param scale_threshold (scaling threshold variable for considering one of the limit lines as a possible blade edge -
set between 0 -> 1 (float), where 1 = 100 percent of with or height frame size.
100 percent will mean that all boundary lines are considered as blade edges.)
@param line_perc (Set how many percent of the line to use for detecting the edge. Float between 0 -> 1. (default=2.0/3))
@param filtrate_edge_points (Filtrate detected edge points that deviate outside of standard deviation.)
@param draw (draw hough lines and points (used during testing))
@param print_hough_positions (print hough line positions (rho, theta) (default=False))
@return edgel_frame, hough_lines, edge_points
(Return:
edgel_frame Edge map
hough_lines = [max_hor_hough_line, min_hor_hough_line, max_vert_hough_line, min_vert_hough_line])
Each index as line of (rho, theta)
'''
width, height = GetShape(origin_frame)
edgel_frame = Canny(origin_frame)
hor_edge_region_threshold = width * scale_threshold
vert_edge_region_threshold = height * scale_threshold
max_hor_line = max_min_lines[0]
min_hor_line = max_min_lines[1]
max_vert_line = max_min_lines[2]
min_vert_line = max_min_lines[3]
max_hor_line_edge_points = [[], []] # [x],[y]
max_hor_hough_line = (None, None)
average_line_x_pos = max_hor_line[0][1] + (max_hor_line[1][1] -
max_hor_line[0][1]) / 2
if average_line_x_pos < hor_edge_region_threshold: # Edge detected on the bottom
for vert_line in bounded_lines[
1]: # Check all vertical lines (max points)
len_line = int(
round((vert_line[1][1] - vert_line[0][1]) * line_perc))
max_point = vert_line[1]
y_edge_ind = DetectLineEdge(edgel_frame, False, max_point[0],
max_point[1], width - 1)
max_hor_line_edge_points[0].append(max_point[0])
max_hor_line_edge_points[1].append(y_edge_ind)
if filtrate_edge_points:
max_hor_line_edge_points[0], max_hor_line_edge_points[
1] = FiltrateEdgePoints(max_hor_line_edge_points[0],
max_hor_line_edge_points[1])
max_hor_hough_line = HoughLineEdgePoints(edgel_frame,
max_hor_line_edge_points,
False)
min_hor_line_edge_points = [[], []] # [x],[y]
min_hor_hough_line = (None, None)
average_line_x_pos = min_hor_line[0][1] + (min_hor_line[1][1] -
min_hor_line[0][1]) / 2
if average_line_x_pos > width - hor_edge_region_threshold: # Edge detected on top
for vert_line in bounded_lines[
1]: # Check all vertical lines (min points)
len_line = int(
round((vert_line[1][1] - vert_line[0][1]) * line_perc))
min_point = vert_line[0]
y_edge_ind = DetectLineEdge(edgel_frame, False, min_point[0],
min_point[1], 0)
min_hor_line_edge_points[0].append(min_point[0])
min_hor_line_edge_points[1].append(y_edge_ind)
if filtrate_edge_points:
min_hor_line_edge_points[0], min_hor_line_edge_points[
1] = FiltrateEdgePoints(min_hor_line_edge_points[0],
min_hor_line_edge_points[1])
min_hor_hough_line = HoughLineEdgePoints(edgel_frame,
min_hor_line_edge_points,
False)
max_vert_line_edge_points = [[], []] # [x],[y]
max_vert_hough_line = (None, None)
average_line_y_pos = max_vert_line[0][0] + (max_vert_line[1][0] -
max_vert_line[0][0]) / 2
if average_line_y_pos < vert_edge_region_threshold: # Edge detected to the right
for hor_line in bounded_lines[
0]: # Check all horizontal lines (max points)
len_line = int(round(
(hor_line[1][0] - hor_line[0][0]) * line_perc))
max_point = hor_line[1]
x_edge_ind = DetectLineEdge(edgel_frame, True, max_point[1],
max_point[0], height - 1)
max_vert_line_edge_points[0].append(x_edge_ind)
max_vert_line_edge_points[1].append(max_point[1])
if filtrate_edge_points:
max_vert_line_edge_points[1], max_vert_line_edge_points[
0] = FiltrateEdgePoints(max_vert_line_edge_points[1],
max_vert_line_edge_points[0])
max_vert_hough_line = HoughLineEdgePoints(edgel_frame,
max_vert_line_edge_points,
True)
min_vert_line_edge_points = [[], []] # [x],[y]
min_vert_hough_line = (None, None)
average_line_y_pos = min_vert_line[0][0] + (min_vert_line[1][0] -
min_vert_line[0][0]) / 2
if average_line_y_pos > height - vert_edge_region_threshold: # Edge detected to the left
for hor_line in bounded_lines[
0]: # Check all horizontal lines (min points)
len_line = int(round(
(hor_line[1][0] - hor_line[0][0]) * line_perc))
min_point = hor_line[0]
x_edge_ind = DetectLineEdge(edgel_frame, True, min_point[1],
min_point[0], 0)
min_vert_line_edge_points[0].append(x_edge_ind)
min_vert_line_edge_points[1].append(min_point[1])
if filtrate_edge_points:
min_vert_line_edge_points[1], min_vert_line_edge_points[
0] = FiltrateEdgePoints(min_vert_line_edge_points[1],
min_vert_line_edge_points[0])
min_vert_hough_line = HoughLineEdgePoints(edgel_frame,
min_vert_line_edge_points,
True)
if max_hor_hough_line[0] == None or min_hor_hough_line[
0] == None or max_vert_hough_line[
0] == None or min_vert_hough_line[0] == None:
raise DroneVisionError('detect_boundary_edge_not_found_all_edge_lines')
hough_lines = [
max_hor_hough_line, min_hor_hough_line, max_vert_hough_line,
min_vert_hough_line
]
if draw:
for i in range(len(max_hor_line_edge_points[0])):
if i == 0:
color = (0, 0, 255) #DARK BLUE
edgel_frame = DrawHoughLine(edgel_frame, max_hor_hough_line,
color)
x = max_hor_line_edge_points[0][i]
y = max_hor_line_edge_points[1][i]
cv2.circle(edgel_frame, (x, y), 5, color, -1)
for i in range(len(min_hor_line_edge_points[0])):
if i == 0:
color = (0, 255, 255) #LIGHT BLUE
edgel_frame = DrawHoughLine(edgel_frame, min_hor_hough_line,
color)
x = min_hor_line_edge_points[0][i]
y = min_hor_line_edge_points[1][i]
cv2.circle(edgel_frame, (x, y), 5, color, -1)
for i in range(len(max_vert_line_edge_points[0])):
if i == 0:
color = (204, 0, 204) #PURPLE
edgel_frame = DrawHoughLine(edgel_frame, max_vert_hough_line,
color)
x = max_vert_line_edge_points[0][i]
y = max_vert_line_edge_points[1][i]
cv2.circle(edgel_frame, (x, y), 5, color, -1)
for i in range(len(min_vert_line_edge_points[0])):
if i == 0:
color = (0, 255, 0) #GREEN
edgel_frame = DrawHoughLine(edgel_frame, min_vert_hough_line,
color)
x = min_vert_line_edge_points[0][i]
y = min_vert_line_edge_points[1][i]
cv2.circle(edgel_frame, (x, y), 5, color, -1)
if print_hough_positions:
print 'max_hor_hough_line: ', max_hor_hough_line
print 'min_hor_hough_line: ', min_hor_hough_line
print 'max_vert_hough_line: ', max_vert_hough_line
print 'min_vert_hough_line: ', min_vert_hough_line
# hough_lines = [max_hor_hough_line, min_hor_hough_line, max_vert_hough_line, min_vert_hough_line]
return edgel_frame, hough_lines
def FindLineLimits(frame,
hough_lines,
keypoints,
radi_threshold=None,
radi_threshold_tuning_param=2.0,
draw_hough_matrix=False,
draw_bounded_lines=False,
draw_max_min_lines=False,
draw_arrowed_bounded_lines=True):
'''
@brief Find boundaries for all hough lines according to the point map.
Segments an object based on the point list by limiting the hough lines and detecting the boundary lines.
@param frame (grayscale)
@param hough_lines List of hough lines (rho, theta).
@param keypoints List of detected point positions.
@param radi_threshold Threshold in pixels to search for in vertical and horizontal axis (If none, then it is set to the biggest blob size)
@param radi_threshold_tuning_param Threshold tuning parameter (default=2 when using biggest blob size. < 0.5 is recommended when using distance betwen blobs).
@param draw_hough_matrix Draw hough lines matrix.
@param draw_bounded_lines Draw bounded lines.
@param draw_max_min_lines Draw detected max min lines.
@param draw_arrowed_bounded_lines
@return frame, bounded_lines, max_hor_line, min_hor_line, max_vert_line, min_vert_line
frame - Usually grayscale, but rgb frame if any draw flag == True
bounded_lines - list with vertical and horizontal lines.
bounded_lines[0] = horizontal lines list, bounded_lines[1] = vertical lines list,
where each index describing a line as ((x1,y1), (x2,y2))
max_min_lines - List of max and min horizontal and vertical lines, in this order:
max_hor_line - Bottom boundary line on the horizontal axis (Note: bottom = max index in images) as ((x1,y1), (x2,y2))
min_hor_line - Top boundary line on the horizontal axis (Note: top = min index in images) as ((x1,y1), (x2,y2))
max_vert_line - Right most boundary line on the vertical axis as ((x1,y1), (x2,y2))
min_vert_line - Left most boundary line on the horizontal axis as ((x1,y1), (x2,y2))
'''
x_points = np.zeros(len(keypoints)) #Width
y_points = np.zeros(len(keypoints)) #height
biggest_size = 0.0
for i in range(len(keypoints)):
if keypoints[i].size > biggest_size:
biggest_size = keypoints[i].size
x_points[i] = keypoints[i].pt[0]
y_points[i] = keypoints[i].pt[1]
# Tuning variable - search along vertical or horizontal axis is limited to this radius.
if radi_threshold == None:
radi_threshold = biggest_size
radi_threshold *= radi_threshold_tuning_param
min_vert_points = [[], []]
max_vert_points = [[], []]
min_hor_points = [[], []]
max_hor_points = [[], []]
bounded_lines = [[], []]
width, height = GetShape(frame)
size = math.sqrt(math.pow(width, 2.0) + math.pow(height, 2.0))
max_vert_line_y_pos = -size * 10 # Set invalid 'large' values to begin with
min_vert_line_y_pos = size * 10
max_hor_line_x_pos = -size * 10
min_hor_line_x_pos = size * 10
if draw_hough_matrix:
frame = DrawHoughLines(frame, hough_lines)
for rho, theta in hough_lines:
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
if round(np.rad2deg(a)) == 0: # Horizontal lines - search y_points
y_m_above = np.argwhere(y_points >= y0 - radi_threshold).T[0]
y_m_below = np.argwhere(y_points < y0 + radi_threshold).T[0]
y_m_ab_b = np.in1d(y_m_above, y_m_below)
y_m_be_b = np.in1d(y_m_below, y_m_above)
y_m_ab_in = np.argwhere(y_m_ab_b == True).T[0]
y_m_be_in = np.argwhere(y_m_be_b == True).T[0]
y_m_temp = np.zeros(len(y_m_ab_in) + len(y_m_be_in))
len_ab = len(y_m_ab_in)
for i in range(len(y_m_temp)):
if i < len_ab:
y_m_temp[i] = y_m_above[y_m_ab_in[i]]
else:
y_m_temp[i] = y_m_below[y_m_be_in[i - len_ab]]
y_m = np.unique(y_m_temp)
if len(y_m) < 2: # Ignore lines of only a single point.
continue
y_dist = {}
for y in y_m:
y = int(y)
y_dist[x_points[y]] = y_points[y]
sorted_y = sorted(y_dist.items(), key=operator.itemgetter(0))
min_point = sorted_y[0]
max_point = sorted_y[-1:][0]
xp = np.zeros(len(y_dist))
fp = np.zeros(len(y_dist))
i = 0
for x, y in sorted_y:
xp[i] = x
fp[i] = y
i += 1
y_inter = np.interp([min_point[0], max_point[0]], xp, fp)
x1 = int(round(min_point[0]))
y1 = int(round(y_inter[0]))
x2 = int(round(max_point[0]))
y2 = int(round(y_inter[1]))
min_hor_points[0].append(min_point[0]) #x
min_hor_points[1].append(y_inter[0]) #y
max_hor_points[0].append(max_point[0]) #x
max_hor_points[1].append(y_inter[1]) #y
average_line_x_pos = y_inter[0] + (y_inter[1] - y_inter[0]) / 2
if average_line_x_pos > max_hor_line_x_pos:
max_hor_line_x_pos = average_line_x_pos
max_hor_line = ((x1, y1), (x2, y2))
if average_line_x_pos < min_hor_line_x_pos:
min_hor_line_x_pos = average_line_x_pos
min_hor_line = ((x1, y1), (x2, y2))
bounded_lines[0].append(((x1, y1), (x2, y2)))
elif round(np.rad2deg(b)) == 0: # vertical lines - search x_points
x_m_above = np.argwhere(x_points >= x0 - radi_threshold).T[0]
x_m_below = np.argwhere(x_points < x0 + radi_threshold).T[0]
x_m_ab_b = np.in1d(x_m_above, x_m_below)
x_m_be_b = np.in1d(x_m_below, x_m_above)
x_m_ab_in = np.argwhere(x_m_ab_b == True).T[0]
x_m_be_in = np.argwhere(x_m_be_b == True).T[0]
x_m_temp = np.zeros(len(x_m_ab_in) + len(x_m_be_in))
len_ab = len(x_m_ab_in)
for i in range(len(x_m_temp)):
if i < len_ab:
x_m_temp[i] = x_m_above[x_m_ab_in[i]]
else:
x_m_temp[i] = x_m_below[x_m_be_in[i - len_ab]]
x_m = np.unique(x_m_temp)
if len(x_m) < 2: # Ignore lines of only a single point.
continue
x_dist = {}
for x in x_m:
x = int(x)
x_dist[y_points[x]] = x_points[x]
sorted_x = sorted(x_dist.items(), key=operator.itemgetter(0))
min_point = sorted_x[0]
max_point = sorted_x[-1:][0]
xp = np.zeros(len(x_dist))
fp = np.zeros(len(x_dist))
i = 0
for x, y in sorted_x:
xp[i] = x
fp[i] = y
i += 1
x_inter = np.interp([min_point[1], max_point[1]], xp, fp)
x1 = int(round(x_inter[0]))
y1 = int(round(min_point[0]))
x2 = int(round(x_inter[1]))
y2 = int(round(max_point[0]))
min_vert_points[0].append(x_inter[0]) #x
min_vert_points[1].append(min_point[0]) #y
max_vert_points[0].append(x_inter[1]) #x
max_vert_points[1].append(max_point[0]) #y
average_line_y_pos = x_inter[0] + (x_inter[1] - x_inter[0]) / 2
if average_line_y_pos > max_vert_line_y_pos:
max_vert_line_y_pos = average_line_y_pos
max_vert_line = ((x1, y1), (x2, y2))
if average_line_y_pos < min_vert_line_y_pos:
min_vert_line_y_pos = average_line_y_pos
min_vert_line = ((x1, y1), (x2, y2))
bounded_lines[1].append(((x1, y1), (x2, y2)))
else:
raise DroneVisionError('find_line_limits_unexpected_angle')
if draw_bounded_lines:
color = (0, 255, 255)
line_thick = 2
if draw_arrowed_bounded_lines:
tipLength = 0.05
cv2.arrowedLine(frame, (x1, y1), (x2, y2),
color,
line_thick,
tipLength=tipLength)
cv2.arrowedLine(frame, (x2, y2), (x1, y1),
color,
line_thick,
tipLength=tipLength)
else:
cv2.line(frame, (x1, y1), (x2, y2), color, line_thick)
try:
max_min_lines = [
max_hor_line, min_hor_line, max_vert_line, min_vert_line
]
except:
raise DroneVisionError('find_line_limits_no_hor_or_vert_found')
if draw_max_min_lines and len(hough_lines) > 0:
cv2.line(frame, max_hor_line[0], max_hor_line[1], (255, 0, 255), 4)
cv2.line(frame, min_hor_line[0], min_hor_line[1], (255, 0, 255), 4)
cv2.line(frame, max_vert_line[0], max_vert_line[1], (255, 0, 255), 4)
cv2.line(frame, min_vert_line[0], min_vert_line[1], (255, 0, 255), 4)
return frame, bounded_lines, max_min_lines