def DetectCorners(frame, keypoints=[], kernel_size=5, kernel_step_size=1, k=0.04, thresh=1000): ''' @brief Detect corners using the Harris corner method. Inspired by: https://github.com/hughesj919/HarrisCorner/blob/master/Corners.py @param frame (grayscale) @param keypoints (list of cv2 keypoints. The frame will be reset if any keypoints are given. (default=empty list)) @param kernel_size (It is the size of neighbourhood considered for corner detection (default=5)) @param kernel_step_size (default=1) @param k (Harris corner constant - usually between 0.04 - 0.06 (default=0.04)) @param thresh (Response threshold (default=1000)) @return corners (list of corners given as cv2 keypoints. Get position of keypoint as kp.pt (x,y position) and response of corner as kp.response) ''' corners = [] offset = int(kernel_size//2) kernel_step_size = int(round(kernel_step_size)) len_kp = len(keypoints) if len_kp > 0: # Find corners from keypoints. corners_frame = np.zeros(GetShape(frame), dtype=np.uint8) for i in range(len_kp): corners_frame[int(round(keypoints[i].pt[1])), int(round(keypoints[i].pt[0]))] = keypoints[i].size else: # Find corners in given frame. corners_frame = frame #Find x and y derivatives dy, dx = np.gradient(corners_frame) Ixx = dx**2 Ixy = dy*dx Iyy = dy**2 height, width = GetShape(corners_frame) for y in range(offset, height-offset, kernel_step_size): for x in range(offset, width-offset, kernel_step_size): #Calculate sum of squares windowIxx = Ixx[y-offset:y+offset+1, x-offset:x+offset+1] windowIxy = Ixy[y-offset:y+offset+1, x-offset:x+offset+1] windowIyy = Iyy[y-offset:y+offset+1, x-offset:x+offset+1] Sxx = windowIxx.sum() Sxy = windowIxy.sum() Syy = windowIyy.sum() #Find determinant and trace, use to get corner response det = (Sxx * Syy) - (Sxy**2) trace = Sxx + Syy r = det - k*(trace**2) #If corner response is over threshold, add to corner list if r > thresh: corners.append(cv2.KeyPoint(x=x, y=y, _size=r/thresh, _response=r)) return corners
def GetContentRequestPointList(self, content, error=False):
'''
@brief Get unpacked/packet content for point list request.
@param content
@param error (True/False)
if master:
@return keypoints, descriptors, valid (Return: keypoints = point keys data, descriptors = point description data, valid = True/False)
else: (slave)
@return content
'''
keypoints = None
descriptors = None
und_shape = None
if self.__master_or_slave: # True = master
valid = content['valid']
error = content['error']
if valid:
und_shape = content['und_shape']
keypoints = content['keypoints']
descriptors = content['descriptors']
keypoints = list_to_keypoints(keypoints)
descriptors = np.array(descriptors)
return und_shape, keypoints, descriptors, valid, error
else:
valid = False
if not(isinstance(content, type(None))):
valid = True
original_frame, original_sl_frame, frame_un, delta_frame, keypoints, descriptors = content
und_shape = GetShape(frame_un)
keypoints = keypoints_to_list(keypoints)
descriptors = descriptors.tolist()
content = {'und_shape': und_shape, 'keypoints': keypoints, 'descriptors': descriptors, 'valid': valid, 'error': error}
return content
def test_PyQtGraphImageMultipleImages(self):
'''
@brief Test PyQtImage real-time plot with multiple images
'''
from src.DroneVision.DroneVision_src.hardware.imageTools import GetImage
from src.DroneVision.DroneVision_src.imgProcessing.frameTools.frameTools import CheckColor, GetShape
import time, cv2, timeit
from getpass import getpass
for folder, left_frames, right_frames, actual_distances, baselines, use_set in self.GetFrameSets():
if use_set:
self.pqImage = self.PyQtImage.PyQtImage(initialize=True, title='Test real-time multiple plot')
timer = timeit.default_timer()
reset = True
for i in range(len(left_frames)):
frame_l = GetImage(folder+left_frames[i][0])
frame_l_sl = GetImage(folder+left_frames[i][1])
frame_r = GetImage(folder+right_frames[i][0])
frame_r_sl = GetImage(folder+right_frames[i][1])
frame_l = CheckColor(frame_l)
widht, height = GetShape(frame_l)
cv2.line(frame_l, (0,0), (height,widht), (255,255,0), 10)
touple_frames = [('frame_l', frame_l), ('frame_l_sl', frame_l_sl), ('frame_r', frame_r), ('frame_r_sl', frame_r_sl)]
self.pqImage(touple_frames, rotate=True)
if not(self.CheckAllTests()):
getpass('Press enter to continue to next frame..')
if reset:
self.pqImage(reset=True)
reset = False
else:
reset = True
print 'Delay for processing {0} image sets was: {1} sec'.format(len(left_frames), timeit.default_timer()-timer)
self.pqImage.ClosePQWindow()
def DetectCornersCV2(frame, keypoints=[], kernel_size=5, k_sobel_size=3, k=0.04, thresh=0.8): ''' @brief Detect corner using the Harris corner detector as provided by the opencv library. @param frame (grayscale) @param keypoints (list of cv2 keypoints. The frame will be reset if any keypoints are given. (default=empty list)) @param kernel_size (It is the size of neighbourhood considered for corner detection (default=5)) @param k_sobel_size (Aperture parameter of Sobel derivative used. (default=3)) @param k (Harris corner constant - usually between 0.04 - 0.06 (default=0.04)) @param thresh (Threshold value relative to the maximum corner response. (default=0.01)) @return corners (list of corners given as cv2 keypoints. Get position of keypoint as kp.pt (x,y position) and response of corner as kp.response) ''' len_kp = len(keypoints) if len_kp > 0: # Find corners from keypoints. corners_frame = np.zeros(GetShape(frame), dtype=np.float32) for i in range(len_kp): corners_frame[int(round(keypoints[i].pt[1])), int(round(keypoints[i].pt[0]))] = keypoints[i].size else: # Find corners in given frame. corners_frame = np.float32(frame) corners_frame = cv2.cornerHarris(corners_frame, kernel_size, k_sobel_size, k) #result is eroded for marking the corners corners_frame = cv2.erode(corners_frame, np.ones((3,3), dtype=np.uint8)) # Threshold for an optimal value, it may vary depending on the image. kp_positions = np.flip(np.argwhere(corners_frame > thresh*corners_frame.max()), 1) n_kps = len(kp_positions) corners = [cv2.KeyPoint()]*n_kps for i in range(n_kps): corners[i].pt = tuple(kp_positions[i]) corners[i].size = corners_frame[kp_positions[i,1], kp_positions[i,0]] corners[i].response = corners_frame[kp_positions[i,1], kp_positions[i,0]] return corners
def PrintHoughAccumulator(accumulator, id_m, rhos):
'''
@brief Print Hough transform accumulator in matplotlib figure.
@param accumulator
'''
acc_shape = GetShape(accumulator)
ylabs = np.deg2rad(np.arange(-0.0, 180.0, 1))
acc_map = np.zeros((acc_shape[0], len(ylabs)), dtype=np.uint16)
id_acc = np.argwhere(accumulator > 0)
for i in range(len(id_acc)):
idx = id_acc[i][0] * acc_shape[1] + id_acc[i][1]
rho = rhos[idx / acc_shape[1]]
acc_map[int(rho), idx % acc_shape[1]] += 1
acc_map = cv2.cvtColor(acc_map, cv2.COLOR_GRAY2BGR)
for i in range(len(id_m)):
idx = id_m[i][0] * acc_shape[1] + id_m[i][1]
rho = rhos[idx / acc_shape[1]]
cv2.circle(acc_map, (int(rho), idx % acc_shape[1]), 1, (0, 0, 255), -1)
acc_map = CheckGrayScale(acc_map)
touple_frame_plot = []
touple_frame_plot.append(('Hough line accumulator', acc_map.T))
MatplotShow(touple_frame_plot, 'Hough_line_accumulator')
def NormalizeEdgeHeading(self, frame, rho, theta):
'''
@brief Normalize the edge heading (rho, theta) according to image center.
See theory about the hough transform.
Short: rho is originally considered from top left image corner,
we want it from image center.
theta is ranged from 0 -> pi, with rho as positive or negative,
we want theta to be ranged from 0-> 2pi and rho as positive.
From image center:
if rho < 0:
theta += pi
rho = abs(rho), where rho is distance to line from image center.
@param frame
@param rho
@param theta
@param draw (draw center hough line points. (default=False))
@param color (draw color if draw=True (default=(255,255,0)))
@return rho, theta (normalized)
'''
if rho < 0:
theta += np.pi
rho *= -1.0
width, height = GetShape(frame)
x0, y0 = height / 2.0, width / 2.0
x1, y1, x2, y2 = self.GetHoughLineSegment(frame, rho, theta)
theta = self.GetAngleOfLineBetweenTwoPoints(x1, y1, x2, y2, x0, y0)
rho = self.GetDistanceToLine(x1, y1, x2, y2, x0, y0)
return rho, theta
def DrawErrorArrow(self,
frame,
heading,
draw_min_rho=True,
rho_min_diag_perc=1 / 4.0,
color=(255, 0, 0),
min_rho_color=(153, 0, 0),
line_thick=5,
draw_arrow=True):
'''
@brief Draw error line/arrow from point on line to image edge.
@param frame
@param heading (rho, theta, max/min, hor/vert)
@param draw_min_rho (True/False)
@param rho_min_diag_perc (default=1/4.0)
@param color (default=(255,0,0) (RED))
@param min_rho_color (default=(153,0,0) (DARK RED))
@param line_thick (default=5)
@param draw line as arrow
@return frame (color)
'''
rho, theta, max_line, hor_or_vert = heading[:4]
if not (rho == None or theta == None):
frame = CheckColor(frame)
I_l, I_w = GetShape(frame)
x1 = int(round(I_w / 2.0 + np.cos(theta) * rho))
y1 = int(round(I_l / 2.0 + np.sin(theta) * rho))
flip_rot = 0.0
if not (self.VerifyEdgeQuadrant(heading)):
flip_rot = np.pi # Edge is in the wrong quadrant, so find rho_f in the correct direction.
I_cos = I_w * np.cos(theta + flip_rot)
I_sin = I_l * np.sin(theta + flip_rot)
diag_rot_l = 0.5 * np.sqrt(I_cos * I_cos + I_sin * I_sin)
x2 = int(round(I_w / 2.0 + np.cos(theta + flip_rot) * diag_rot_l))
y2 = int(round(I_l / 2.0 + np.sin(theta + flip_rot) * diag_rot_l))
x_rho_min = int(
round(I_w / 2.0 + np.cos(theta + flip_rot) * diag_rot_l *
(1.0 - rho_min_diag_perc)))
y_rho_min = int(
round(I_l / 2.0 + np.sin(theta + flip_rot) * diag_rot_l *
(1.0 - rho_min_diag_perc)))
if draw_arrow:
cv2.arrowedLine(frame, (x1, y1), (x2, y2),
color,
line_thick,
tipLength=0.1)
if draw_min_rho:
cv2.arrowedLine(frame, (x1, y1), (x_rho_min, y_rho_min),
min_rho_color,
line_thick,
tipLength=0.1)
else:
cv2.line(frame, (x1, y1), (x2, y2), color, line_thick)
if draw_min_rho:
cv2.line(frame, (x1, y1), (x_rho_min, y_rho_min),
min_rho_color, line_thick)
return frame
def DrawHoughLine(frame, hough_line, color):
'''
@brief Draw hough line on frame.
Hough lines are give in rho, theta coordinates.
@param frame
@param hough_line (rho, theta)
@param color RGB color (R,G,B)
@return frame (with drawn line)
'''
frame = CheckColor(frame)
width, height = GetShape(frame)
size = math.sqrt(math.pow(width, 2.0) + math.pow(height, 2.0))
rho, theta = hough_line
if not (np.isnan(theta)) and not (np.isnan(rho)):
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + size * (-b))
y1 = int(y0 + size * (a))
x2 = int(x0 - size * (-b))
y2 = int(y0 - size * (a))
cv2.line(frame, (x1, y1), (x2, y2), color, 2)
return frame
def UpdatePQImage(self,
frame,
key_tag,
process_events=True,
rotate=True,
autoDownsample=True):
'''
@brief Plot image in real-time
@param frame
@param key_tag (also frame title)
@param process_events (True/False for processing new events (plotting new data))
@param rotate (True/False for rotating the image so that it is shown properly. (default=True))
@param autoDownsample (True/False (default=True))
'''
if not (self.__win_open):
self.InitPQImage(title=self.__title)
self.AssertPQImageInititalized()
if not (key_tag in self.__img_items):
self.AddPQImagePlot(key_tag)
if rotate:
width, height = GetShape(frame)
M = cv2.getRotationMatrix2D((height / 2, width / 2), -90, 1)
self.__img_items[key_tag].setImage(cv2.warpAffine(
frame, M, (height, width)),
autoDownsample=autoDownsample)
else:
self.__img_items[key_tag].setImage(frame,
autoDownsample=autoDownsample)
if process_events:
self.TriggProcessEvents()
if self.CheckFlagReset():
self.UpdatePQImage(frame, key_tag, process_events, rotate,
autoDownsample)
def Undistort(self, frame):
'''
@brief Undistort frame using the remapping method.
Should give same result as Undistort()
@param frame
@return Undistorted frame
'''
self.AssertCameraCalibrated()
if not (self.CheckIntrinsicScale(GetShape(frame))):
self.RectifyCamera(GetShape(frame))
self.InitUndistortRectifyMap()
und_frame = cv2.remap(frame, self.__mapx, self.__mapy,
cv2.INTER_LINEAR)
return self.CropUndistortedFrame(und_frame)
def CalibrateCamera(self, frame_size=None):
'''
@brief Compute the camera matrix, distortion coefficients, rotation and translation vectors.
@param frame_size (height. width)
'''
if not (isinstance(frame_size, tuple)) and not (isinstance(
frame_size, list)):
frame_size = GetShape(GetImage(self.__calib_img_fnames[0]))
if self.__focal_length != None and self.__sensor_size != None:
#flags = cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_ASPECT_RATIO
flags = cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_PRINCIPAL_POINT
self.__calib_params['intrinsic_mtx'] = self.ComputeCameraMatrix(
frame_size, self.__focal_length, self.__sensor_size)
retval, self.__calib_params['intrinsic_mtx'], self.__calib_params[
'distortion_coeffs'], rvecs, tvecs = cv2.calibrateCamera(
self.__calib_params['objpoints'],
self.__calib_params['imgpoints'],
(frame_size[1], frame_size[0]),
self.__calib_params['intrinsic_mtx'],
None,
flags=flags)
else:
retval, self.__calib_params['intrinsic_mtx'], self.__calib_params[
'distortion_coeffs'], rvecs, tvecs = cv2.calibrateCamera(
self.__calib_params['objpoints'],
self.__calib_params['imgpoints'],
(frame_size[1], frame_size[0]), None, None)
if not (retval):
raise Exception('Camera calibration failed!')
def FindChessBoardCorners(self):
'''
@brief Find chessboard matches and append to imgpoints.
'''
if self.__show_chessboard_img:
realTimePlot = self.GetNewRealTimePlot()
ret_calib = False
for i in range(len(self.__calib_img_fnames)):
fname = self.__calib_img_fnames[i]
img = GetImage(fname, gray=False)
gray = CheckGrayScale(img)
gray_shape = GetShape(gray)
if i > 0:
if not (self.CheckIntrinsicScale(gray_shape)):
raise ValueError(
'Calibration image dimensions do not match!')
else:
self.SetScale(gray_shape)
# Find the chess board corners
flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK
ret, corners = cv2.findChessboardCorners(
gray, (self.__calib_chess_rows, self.__calib_chess_columns),
flags=flags)
# If found, add object points, image points (after refining them)
print_msg = '{0}/{1} images processed'.format(
i + 1, len(self.__calib_img_fnames))
if ret:
self.PrintCalibrationProcess(
print_msg + ' - {0} was successfull'.format(fname))
ret_calib = True
self.__calib_params['objpoints'].append(self.__objp)
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
self.__criteria)
self.__calib_params['imgpoints'].append(corners)
if self.__show_chessboard_img:
cv2.drawChessboardCorners(
img,
(self.__calib_chess_rows, self.__calib_chess_columns),
corners, ret)
realTimePlot(reset=True)
realTimePlot([(fname[len(self.__calib_folder):], img)],
'calibration_frames')
else:
self.PrintCalibrationProcess(
print_msg + ' - {0} was not successfull'.format(fname))
if self.__show_chessboard_img:
realTimePlot(reset=True)
realTimePlot([
('Failed: ' + fname[len(self.__calib_folder):], gray)
], 'calibration_frames')
if not (ret_calib):
err_msg = 'None of the chessboard calibration frames could be used for calibration. Folder = {0}'.format(
self.__calib_folder)
raise Exception(err_msg)
def GetFrameProperties(self):
'''
@brief Get frame properties such as fps, width, length
@return fps, width, height
'''
dim = GetShape(self.__frame)
return 1.0, dim[0], dim[1]
def InitRecording(self, initFrame):
'''
@brief Initialize recording by adapting the video to the first frame input.
@param initFrame
'''
self.__height, self.__width = GetShape(initFrame)
self.StartRecording()
def Undistort(self, frame):
'''
@brief Stereo undistorting
@return undistorted frame
'''
self.AssertStereoCalibrated()
if not (self.CheckIntrinsicStereoScale(GetShape(frame))):
self.SetIntrinsicStereoScale(GetShape(frame))
if self.__me_master:
und_frame = cv2.remap(frame, self.__left_rectify_maps[0],
self.__left_rectify_maps[1],
cv2.INTER_LANCZOS4)
else:
und_frame = cv2.remap(frame, self.__right_rectify_maps[0],
self.__right_rectify_maps[1],
cv2.INTER_LANCZOS4)
return self.CropUndistortedFrame(und_frame)
def GetFrame(self, get_normal_frame_only=False):
'''
@brief Pull image and structured light image
@param get_normal_frame_only (Only implemented to fit with the CameraLink)
@return frame, sl_frame
'''
self.__frame = GetImage(self.__folder +
self.__image_filenames[self.__frame_i],
gray=False)
self.__sl_frame = GetImage(self.__folder +
self.__sl_image_filenames[self.__frame_i],
gray=False)
if not (GetShape(self.__frame)[0] == GetShape(
self.__sl_frame)[0]) or not (GetShape(self.__frame)[1]
== GetShape(self.__sl_frame)[1]):
raise Exception(
'Normal image and sl image dimensions are not consistent.')
self.__frame_i += 1
return self.__frame, self.__sl_frame
def ProcessBoundaryHoughLine(self,
frame,
boundary_hough_line,
bound_thres=10,
draw=False,
color=(255, 255, 0)):
'''
@brief Process boundary hough line.
Check if the hough line indicates the blade edge,
and compute the heading angle according to the edge.
@param frame (original undistorted frame)
@param boundary_hough_line (Touple as (rho, theta))
@param bound_thres (Threshold in pixels for the line to be detected as a blade edge)
@param draw (default=False)
@param color (draw color if draw=True (default=(255,255,0)))
if draw:
@return edge_line, rho, theta, frame (color frame with circles of hough line center points)
else:
@return edge_line, rho, theta
(edge_line = True/False in regard to if the line is following a blade edge
rho = distance to edge (normalized)
theta = heading angle along the detected edge (normalized.)
'''
width, height = GetShape(frame)
rho, theta = boundary_hough_line
rho, theta = self.NormalizeEdgeHeading(frame, rho, theta)
x_center = height / 2.0 + np.cos(theta) * rho
y_center = width / 2.0 + np.sin(theta) * rho
edge_line = False
if (np.abs(x_center - (height - 1)) > bound_thres and
x_center > bound_thres) and (np.abs(y_center -
(width - 1)) > bound_thres
and y_center > bound_thres):
edge_line = True
if draw:
cv2.circle(frame, (int(round(x_center)), int(round(y_center))), 10,
color, 2)
if edge_line:
frame = self.DrawHeading(frame, (rho, theta), color=color)
if draw:
return edge_line, rho, theta, frame
return edge_line, rho, theta
def GetTipDetected(self, frame, selected_tip_heading, diag_percent=1/3.0): ''' @brief Detect if tip is detected within a given percent of the diagonal distance of the frame. @param frame @param selected_tip_heading @param diag_percent (default=25%) @return tip_detected, selected_tip_heading (Return: tip_detected = True/False, selected_tip_heading = (rho, theta)) ''' tip_detected = False if not(selected_tip_heading[0] == None): width, height = GetShape(frame) tip_threshold_distance = math.sqrt(width*width + height*height)*diag_percent if selected_tip_heading[0] <= tip_threshold_distance: tip_detected = True return tip_detected, selected_tip_heading
def CalibrateStandardScaling(self, printInfo=False):
'''
@brief Run standard scale calibration.
@param printInfo (Print info during calibration (default=False))
'''
self.SetScalingImages()
standard_scale_distances = []
mean_point_sizes = []
for sl_fname, fname in self.__calib_img_fnames:
frame = GetImage(fname, gray=False)
sl_frame = GetImage(sl_fname, gray=False)
try:
delta_frame, keypoints, descriptors, frame, sl_frame = self.GetPointList(
frame,
sl_frame,
concatenate_points=True,
compute_descriptors=False)
except DroneVisionError, err:
warnings.simplefilter('always')
warnings.warn(
str(err) +
' - file: {0}, SL file: {1}'.format(fname, sl_fname),
Warning)
warnings.simplefilter('default')
continue
width, height = GetShape(sl_frame)
blockSize = int(
math.sqrt(math.pow(width, 2.0) + math.pow(height, 2.0)) // 2)
scaling_point_frame, mean_point_size = self.PointScaleMatch(
delta_frame, keypoints, blockSize)
standard_scale_distance = self.ComputeStandardScaleDistance(
scaling_point_frame)
if np.isnan(standard_scale_distance):
err = 'No scaling points detected - file: {0}, SL file: {1}'.format(
fname, sl_fname)
warnings.simplefilter('always')
warnings.warn(str(err), Warning)
warnings.simplefilter('default')
continue
standard_scale_distances.append(standard_scale_distance)
mean_point_sizes.append(mean_point_size)
def GetHoughLineSegment(self, frame, rho, theta):
'''
@brief Get start and end position of hough line.
The hough line is normally positioned within the frame,
but for theta > pi/2 will give positiones outside of the frame.
The algorithm uses sin/cos/tan to compute the correct start and end position
of the hough transform inside the frame.
@param frame
@param rho
@param theta
@return x_start, y_start, x_end, y_end
'''
width, height = GetShape(frame)
size = math.sqrt(math.pow(width, 2.0) + math.pow(height, 2.0))
x_start = np.cos(theta) * rho - size * (-np.sin(theta))
y_start = np.sin(theta) * rho - size * np.cos(theta)
x_end = np.cos(theta) * rho + size * (-np.sin(theta))
y_end = np.sin(theta) * rho + size * np.cos(theta)
return x_start, y_start, x_end, y_end
def DrawHeading(self,
frame,
heading,
color=(255, 255, 0),
line_thick=2,
draw_arrow=True):
'''
@brief Draw heading on frame.
Heading is measured from image center.
@param frame
@param heading (rho, theta)
@param color (default=(255,255,0) (YELLOW))
@param line_thick (default=5)
@param draw line as arrow
@return frame (color)
'''
rho, theta = heading[:2]
if not (rho == None or theta == None):
frame = CheckColor(frame)
I_l, I_w = GetShape(frame)
x0 = I_w / 2.0
y0 = I_l / 2.0
x1 = int(round(x0))
y1 = int(round(y0))
x2 = int(round(x0 + np.cos(theta) * rho))
y2 = int(round(y0 + np.sin(theta) * rho))
if draw_arrow:
cv2.arrowedLine(frame, (x1, y1), (x2, y2),
color,
line_thick,
tipLength=0.1)
else:
cv2.line(frame, (x1, y1), (x2, y2), color, line_thick)
return frame
def TestBlobScaleDetector(self, folder, fn_frame, fn_slframe,
actual_distance):
'''
@brief Test function for blob scale detector unit.
@param folder Input folder
@param fn_frame Frame filename without points.
@param fn_slframe Frame filename with points.
@param actual_distance (mm)
'''
import timeit, math
import numpy as np
from src.DroneVision.DroneVision_src.hardware.imageTools import GetImage, MatplotShow
from src.DroneVision.DroneVision_src.imgProcessing.frameTools.frameTools import GetShape
print '\n'
print '#----------- TESTING SCALING BASED BLOB DETECTION \t---------------#'
print '#----------- Image without points: {0} \t---------------#'.format(
fn_frame)
print '#----------- Image with points: {0} \t---------------#'.format(
fn_slframe)
settings_inst = self.Settings.Settings()
settings_inst.ChangeSetting('BASIC', 'reset_calibration', False)
pointDet = self.PointDetection.PointDetection(
True, settings_inst.GetSettings())
pointDet.CalibrateBlobScaleDetector(printInfo=True,
force_calibration=True)
print 'Min distance between blobs: {0}'.format(
pointDet.GetMinDistanceBetweenBlobs())
fn_frame = folder + fn_frame
fn_slframe = folder + fn_slframe
delay = timeit.default_timer()
frame = GetImage(fn_frame)
sl_frame = GetImage(fn_slframe)
print 'Delay reading images: {0} sec'.format(timeit.default_timer() -
delay)
total_delay = timeit.default_timer()
delta_frame, points_kp, blob_desc, frame, sl_frame = pointDet.GetPointList(
frame, sl_frame, concatenate_points=True, draw=True)
delay = timeit.default_timer()
width, height = GetShape(sl_frame)
blockSize = int(
math.sqrt(math.pow(width, 2.0) + math.pow(height, 2.0)) // 4)
scaling_point_frame, mean_point_size = pointDet.PointScaleMatch(
delta_frame, points_kp, blockSize)
print 'Delay for computing disparity points: {0} sec'.format(
timeit.default_timer() - delay)
timeout = timeit.default_timer() - total_delay
print 'Total delay for downscaling + undistort + blob + feature based stereopsis: {0} sec'.format(
timeout)
touple_frames = []
touple_frames.append(('SL frame', sl_frame))
touple_frames.append(('Delta frame', delta_frame))
if not (self.CheckAllTests()):
MatplotShow(touple_frames,
fn_frame + '_Blob_scale_test',
save_fig=self.save_figs,
save_fig_only=self.save_figs_only)
def FindHeading(self, frame, selected_blade_edge_headings, rho_step_distance=None, default_rho_step_distance_diag_perc=1/4.0, rho_min_diag_perc=1/4.0, flip_heading=False): ''' @brief Find the next heading based on selected edge heading and current heading. Computed using equations (6.14 to 6.17) in MSc final thesis - Hans Erik Heggem Reset the heading by using ResetHeading() @param frame @param selected_blade_edge_headings @param rho_step_distance (step distance in pixels for the drone to fly during each iteration. If None = then each step distance is set to a quarter the diagonal frame size (default=None)) @param default_rho_step_distance_diag_perc (Default step distance if rho_step_distance==None. Given as a percent of the diagonal frame size (default=0.25)) @param rho_min_diag_perc (Default percent of the diagonal length from center to image edge, used to calculate rho_min to navigate along a singel edge. Given as a percent between 0-1 (default=0.25)) @param flip_heading (Flip heading in opposite direction (True/False)) @return selected_heading (rho = distance to next heading position (in image frame), theta = heading angle in radian degrees) ''' selected_heading = (0.0,0.0) if len(selected_blade_edge_headings) > 0: I_l, I_w = GetShape(frame) diag_size = np.sqrt(I_l*I_l + I_w*I_w) if rho_step_distance == None: rho_step_distance = diag_size*default_rho_step_distance_diag_perc if len(selected_blade_edge_headings) > 1: if self.VerifyEdgeQuadrant(selected_blade_edge_headings[0]) and self.VerifyEdgeQuadrant(selected_blade_edge_headings[1]): omega = (selected_blade_edge_headings[0][1] + selected_blade_edge_headings[1][1])/2.0 + np.pi if not(self.GetMovingTowardsTipOrRoot()): # True for moving towards tip. omega -= np.pi if flip_heading: omega += np.pi else: # Either one of the edges are in the incorrect quadrant - estimate heading based on closest edge if selected_blade_edge_headings[0][0] < selected_blade_edge_headings[1][0]: selected_blade_edge_headings = selected_blade_edge_headings[0:1] else: selected_blade_edge_headings = selected_blade_edge_headings[1:2] return self.FindHeading(frame, selected_blade_edge_headings, rho_step_distance, default_rho_step_distance_diag_perc, rho_min_diag_perc, flip_heading) else: sign = 1.0 flip_rot = 0.0 if not(self.VerifyEdgeQuadrant(selected_blade_edge_headings[0])): sign = -1.0 # Everything must be flipped if the edge is in the wrong quadrant flip_rot = np.pi # Edge is in the wrong quadrant, so find rho_f in the correct direction. edge_rho, edge_theta, max_line, hor_or_vert = selected_blade_edge_headings[0] I_cos = I_w*np.cos(edge_theta + flip_rot) I_sin = I_l*np.sin(edge_theta + flip_rot) diag_rot_l = 0.5*np.sqrt(I_cos*I_cos + I_sin*I_sin) rho_f = diag_rot_l - sign*edge_rho rho_min = diag_rot_l*rho_min_diag_perc if flip_rot != 0.0: # Flip situations if the edge is in wrong quadrant rho_f_cp = rho_f rho_f = rho_min rho_min = rho_f_cp if rho_min <= rho_f: W_f = 2 - rho_min/rho_f else: W_f = rho_f/rho_min if max_line: # Mirrored direction depending on the side of the blade to follow. sign *= -1.0 if not(self.GetMovingTowardsTipOrRoot()): # True for moving towards tip. sign *= -1.0 if flip_heading: sign *= -1.0 omega = edge_theta + sign*(np.pi/2)*W_f selected_heading = (rho_step_distance, omega) return selected_heading #else
def PointScaleMatch(self,
frame,
keypoints,
blockSize,
filtrate_scaling_points=None):
'''
@brief Compute scaling map between points.
@param frame
@param keypoints
@param blockSize (Should be about a quarter of the frame size if the points are distributed evenly on the frame)
@param filtrate_scaling_points (If None, then is is set by the class instance settings (default=None))
@return scaling_point_frame, mean_point_size
'''
# Class instance settings
if filtrate_scaling_points == None:
filtrate_scaling_points = self.__filtrate_points
height, width = GetShape(frame)
points_frame = np.zeros((height, width), dtype=np.uint16)
scaling_point_frame = np.zeros((height, width))
float_points_list = [None] * (width * height)
mean_point_size = 0.0
for point in keypoints:
x = int(round(point.pt[1]))
y = int(round(point.pt[0]))
float_points_list[x * width + y] = point
mean_point_size += point.size
points_frame[x, y] = point.size
mean_point_size /= len(keypoints)
blockSize_radi = blockSize // 2
points_pos = np.argwhere(points_frame > 0)
for i in range(len(points_pos)):
x = points_pos[i, 0]
y = points_pos[i, 1]
point = float_points_list[x * width + y]
if point == None:
raise Exception('None point: {0}'.format(point))
x_offset_neg = 0
x_offset_pos = 0
y_offset_neg = 0
y_offset_pos = 0
if x - blockSize_radi < 0:
x_offset_neg = x - blockSize_radi
elif x + blockSize_radi >= height:
x_offset_pos = x + blockSize_radi - (height - 1)
if y - blockSize_radi < 0:
y_offset_neg = y - blockSize_radi
elif y + blockSize_radi >= width:
y_offset_pos = y + blockSize_radi - (width - 1)
x_block_start = x - blockSize_radi - x_offset_neg
y_block_start = y - blockSize_radi - y_offset_neg
block = points_frame[x_block_start:(x + 1) + blockSize_radi -
x_offset_pos, y_block_start:(y + 1) +
blockSize_radi - y_offset_pos]
x_block_center = len(block) // 2
y_block_center = len(block[x_block_center]) // 2
block[x_block_center, y_block_center] = 0
block_disparities = np.argwhere(block > 0)
shortest_distance = -1
for j in range(len(block_disparities)):
x_block = x_block_start + block_disparities[j, 0]
y_block = y_block_start + block_disparities[j, 1]
block_point = float_points_list[x_block * width + y_block]
if block_point == None:
raise Exception(
'None block point: {0}'.format(block_point))
distance = math.sqrt(
math.pow(point.pt[0] - block_point.pt[0], 2) +
math.pow(point.pt[1] - block_point.pt[1], 2))
if distance < shortest_distance or shortest_distance < 0:
shortest_distance = distance
if shortest_distance >= 0:
scaling_point_frame[x, y] = shortest_distance
if filtrate_scaling_points:
scaling_point_frame = self.FiltrateScalingPoints(
scaling_point_frame)
return scaling_point_frame, mean_point_size
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
def HoughLineEdgePoints(frame,
edge_points,
horizontal_points,
hough_peak_param=1.2,
dilation_kernel_size=3,
dilation_iterations=1):
'''
@brief Speeded up Houg lines transform for lines by iterating over known key point positions.
Inspired by: https://alyssaq.github.io/2014/understanding-hough-transform/
@param frame
@param edge_points List of detected edge points as [[x], [y]]
@param horizontal_points Set True for detecting horizontal lines, and False for detecting vertical lines.
The lines will be focused around the vertical or horizontal axis.
@param hough_peak_param Delimiter for increasing number of peaks to validate for finding the most significant peaks. Must be >= 1.0.
@param dilation_kernel_size (Increase strong areas of peak points by dilation.
Set the kernel size for dilation as a dilation_kernel_size*dilation_kernel_size (f.eks 3*3 kernel size).
Set to 1 to give the dilation no effect. (default=3))
@param dilation_iterations (Number of dilation iterations to increase the width of strong areas (default=1))
@return (rho, theta) (Distance and angle of the most significant edge.)
'''
# Rho and Theta ranges
if horizontal_points:
start_degree = 0.0
end_degree = 45.0
step_degree = 1.0
thetas_small = np.deg2rad(
np.arange(start_degree, end_degree, step_degree))
start_degree = 135.0
end_degree = 180.0
step_degree = 1.0
thetas_big = np.deg2rad(
np.arange(start_degree, end_degree, step_degree))
thetas = np.concatenate((thetas_small, thetas_big))
else: # vertical points
start_degree = 45.0
end_degree = 135.0
step_degree = 1.0
thetas = np.deg2rad(np.arange(start_degree, end_degree, step_degree))
width, height = GetShape(frame)
diag_len = np.ceil(np.sqrt(width * width + height * height)) # max_dist
rhos = np.linspace(-np.int(diag_len), np.int(diag_len),
np.int(diag_len) * 2)
# Cache some reusable values
cos_t = np.cos(thetas)
sin_t = np.sin(thetas)
num_thetas = len(thetas)
# Hough accumulator array of theta vs rho
accumulator = np.zeros((int(2 * diag_len), num_thetas), dtype=np.float32)
acc_shape = GetShape(accumulator)
# Vote in the hough accumulator
for i in range(len(edge_points[0])):
x = edge_points[0][i]
y = edge_points[1][i]
for t_idx in range(num_thetas):
# Calculate rho. diag_len is added for a positive index
rho = int(round(x * cos_t[t_idx] + y * sin_t[t_idx]) + diag_len)
accumulator[rho, t_idx] += 1
# Dilate the accumulator so that close neighboring voting points are stronger, and add the Gaussian smoothed accumulator to highlight the strongest peak in the strongest areas.
accumulator = cv2.dilate(accumulator,
np.ones(
(dilation_kernel_size, dilation_kernel_size),
dtype=np.uint8),
iterations=dilation_iterations)
accumulator += cv2.GaussianBlur(
accumulator, (dilation_kernel_size, dilation_kernel_size), 0
) # Let sigma be calculated according to the kernel size. See cv2 doc.
# Peak finding based on max votes.
id_m = np.argwhere(accumulator >= np.max(accumulator) / hough_peak_param)
len_id_m = len(id_m)
rhos_res = [0] * len_id_m
thetas_res = [0] * len_id_m
for i in range(len_id_m):
idx = id_m[i][0] * acc_shape[1] + id_m[i][1]
rhos_res[i] = rhos[idx / acc_shape[1]]
thetas_res[i] = thetas[idx % acc_shape[1]]
rho = np.median(rhos_res)
theta = np.median(thetas_res)
return (rho, theta)
def HoughLinesPointMatrix(frame,
keypoints,
min_lines=2,
radi_threshold=None,
radi_threshold_tuning_param=2.0):
'''
@brief Speeded up Houg lines transform for lines by iterating over known key point positions.
Inspired by: https://alyssaq.github.io/2014/understanding-hough-transform/
@param frame
@param keypoints List of detected points (using the blob detection algorithm.)
@param min_lines Minimum lines to finally end up with. Set to -1 to hinder any concateniation of detected lines.
@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).
@return hough_lines Returned as list of touples (rho, theta)
'''
# Rho and Theta ranges
thetas = np.deg2rad(np.array([0.0, 90.0]))
width, height = GetShape(frame)
diag_len = np.ceil(np.sqrt(width * width + height * height)) # max_dist
rhos = np.linspace(-np.int(diag_len), np.int(diag_len),
np.int(diag_len) * 2)
# Cache some resuable values
cos_t = np.cos(thetas)
sin_t = np.sin(thetas)
num_thetas = len(thetas)
# Hough accumulator array of theta vs rho
accumulator = np.zeros((int(2 * diag_len), num_thetas), dtype=np.float32)
acc_shape = GetShape(accumulator)
biggest_point = 0
# Vote in the hough accumulator
for i in range(len(keypoints)):
x = keypoints[i].pt[0]
y = keypoints[i].pt[1]
if keypoints[i].size > biggest_point:
biggest_point = keypoints[i].size
for t_idx in range(num_thetas):
# Calculate rho. diag_len is added for a positive index
rho = int(round(x * cos_t[t_idx] + y * sin_t[t_idx]) + diag_len)
accumulator[rho, t_idx] += 1
# Peak finding based on max votes
id_m = np.argwhere(accumulator >= 1)
len_id_m = len(id_m)
hough_lines = [0] * len_id_m
#PrintHoughAccumulator(accumulator, id_m, rhos)
for i in range(len_id_m):
idx = id_m[i][0] * acc_shape[1] + id_m[i][1]
rho = rhos[idx / acc_shape[1]]
theta = thetas[idx % acc_shape[1]]
hough_lines[i] = (rho, theta)
if radi_threshold != None:
origin_threshold = radi_threshold * radi_threshold_tuning_param
else:
origin_threshold = biggest_point
threshold = origin_threshold
n_lines = len(hough_lines
) + 1 #Stop concatenating when there is no change in n_lines
while len(hough_lines) > min_lines and n_lines - len(
hough_lines) > 0 and min_lines > 0:
n_lines = len(hough_lines)
hough_lines = concatenateLines(hough_lines, threshold)
threshold += origin_threshold / 2
return hough_lines # returned as touples of (rho, theta)
def TestPointMatchStereoVision(self,
folder,
left_fn_frame,
left_fn_slframe,
right_fn_frame,
right_fn_slframe,
baseline,
actual_distance,
filtrate_3Dpoints=True,
use_triangulation=True,
use_opencv_triangulation=True,
use_block_matching=True,
use_brute_force_matching=False):
'''
@brief Test function for stereo detection unit using point matching.
@param folder Input folder
@param left_fn_frame Frame filename without points.
@param left_fn_slframe Frame filename with points.
@param right_fn_frame Frame filename without points.
@param right_fn_slframe Frame filename with points.
@param baseline (mm)
@param actual_distance (mm)
@param filtrate_3Dpoints (Filtrate 3D points which are outside of the standard deviation)
@param use_triangulation (True for using tringulation method, False for using easy disparity to depth method)
@param use_opencv_triangulation (True for using the opencv implementation of triangulation)
@param use_block_matching (use the block matching feature matching method)
@param use_brute_force_matching (use the brute force matching method (if use_block_matching=False))
'''
import timeit
import numpy as np
from src.DroneVision.DroneVision_src.imgProcessing.frameTools.frameTools import GetShape
from src.DroneVision.DroneVision_src.hardware.imageTools import GetImage, MatplotShow
if use_block_matching:
title = 'BLOCK SEARCH'
else:
if use_brute_force_matching:
title = 'BRUTE F.'
else:
title = 'FLANN'
if use_triangulation:
extra_info = 'TRIANGULATION TO DEPTH'
else:
extra_info = 'DISPARITY TO DEPTH'
print '\n'
print '#----------- TESTING FEATURE BASED STEREO DETECTION - {0} MATCHING - {1}\t---------------#'.format(
title, extra_info)
print '#----------- Left Image without points: {0} \t\t\t\t\t---------------#'.format(
left_fn_frame)
print '#----------- Right Image without points: {0} \t\t\t\t---------------#'.format(
right_fn_frame)
settings_inst = self.Settings.Settings()
settings_inst.ChangeSetting('BASIC', 'reset_calibration', False)
#settings_inst.ChangeSetting('CALIB', 'baseline', baseline)
settings_inst.ChangeSetting('F_STEREO', 'use_triangulation',
use_triangulation)
settings_inst.ChangeSetting('F_STEREO', 'use_cv2_triangulation',
use_opencv_triangulation)
settings_inst.ChangeSetting('F_STEREO', 'use_block_matching',
use_block_matching)
settings_inst.ChangeSetting('F_STEREO', 'use_brute_force',
use_brute_force_matching)
left_pointDet = self.PointDetection.PointDetection(
True, settings_inst.GetSettings())
left_pointDet.CalibratePointDetection(printInfo=True)
right_pointDet = self.PointDetection.PointDetection(
False, settings_inst.GetSettings())
right_pointDet.CalibratePointDetection(printInfo=False)
#if not(left_pointDet.GetBaseline() == baseline) or not(right_pointDet.GetBaseline() == baseline):
# raise Exception('Fail baseline!')
left_fn_frame = folder + left_fn_frame
left_fn_slframe = folder + left_fn_slframe
right_fn_frame = folder + right_fn_frame
right_fn_slframe = folder + right_fn_slframe
delay = timeit.default_timer()
left_frame = GetImage(left_fn_frame)
left_sl_frame = GetImage(left_fn_slframe)
right_frame = GetImage(right_fn_frame)
right_sl_frame = GetImage(right_fn_slframe)
print 'Delay reading images: {0} sec'.format(timeit.default_timer() -
delay)
#MatplotShow([('left', left_sl_frame), ('right', right_sl_frame)], 'Feature Stereopsis', save_fig=self.save_figs, save_fig_only=self.save_figs_only)
left_delta_frame, left_points_kp, left_blob_desc, left_frame, left_sl_frame = left_pointDet.GetPointList(
left_frame,
left_sl_frame,
compute_descriptors=not (use_block_matching),
draw=True)
right_delta_frame, right_points_kp, right_blob_desc, right_frame, right_sl_frame = right_pointDet.GetPointList(
right_frame,
right_sl_frame,
compute_descriptors=not (use_block_matching),
draw=True)
total_delay = timeit.default_timer()
points3D, match_points_frame = left_pointDet.Get3DPoints(
GetShape(left_delta_frame),
GetShape(right_delta_frame),
left_points_kp,
left_blob_desc,
right_points_kp,
right_blob_desc,
filtrate_3Dpoints=filtrate_3Dpoints,
draw=True,
left_delta_frame=left_delta_frame,
right_delta_frame=right_delta_frame)
points3D_m = left_pointDet.Points3DToMatrix(points3D)
average_point3D = np.mean(points3D_m, axis=1)
std_point3D = np.std(points3D_m, axis=1)
timeout = timeit.default_timer() - total_delay
print 'Total delay for feature based stereopsis: {0} sec'.format(
timeout)
delay = timeit.default_timer()
points3D = left_pointDet.Get3DPoints(
GetShape(left_delta_frame),
GetShape(right_delta_frame),
left_points_kp,
left_blob_desc,
right_points_kp,
right_blob_desc,
filtrate_3Dpoints=filtrate_3Dpoints,
draw=False,
left_delta_frame=left_delta_frame,
right_delta_frame=right_delta_frame)
timeout_stereopsis = timeit.default_timer() - delay
for point3D in points3D:
print 'Point3D: x = {0} \t y = {1} \t z = {2}'.format(
point3D[0, 0], point3D[1, 0], point3D[2, 0])
print 'Average Point3D: x = {0} \t y = {1} \t z = {2}'.format(
average_point3D[0, 0], average_point3D[1, 0], average_point3D[2,
0])
print 'STD Point3D: x = {0} \t y = {1} \t z = {2}'.format(
std_point3D[0, 0], std_point3D[1, 0], std_point3D[2, 0])
print 'Delay for computing distance points: {0} sec, average distance: {1} mm, actual distance: {2} mm, distance error: {3} mm, baseline = {4} mm'.format(
timeout_stereopsis, average_point3D[2, 0], actual_distance,
average_point3D[2, 0] - actual_distance, baseline)
#MatplotShow([(title, match_points_frame)], save_fig=self.save_figs, save_fig_only=self.save_figs_only)
#title += ':Z(mean)={0:.2f}mm'.format(average_point3D[2,0])
if not (self.CheckAllTests()):
if self.show_delta_frames:
MatplotShow([('left', left_delta_frame),
('right', right_delta_frame)],
left_fn_frame +
'_Feature_stereo_test_delta_frames',
save_fig=self.save_figs,
save_fig_only=self.save_figs_only)
return (title, match_points_frame)
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