def _two_cluster_min_dist(cur_points, other_points):
'''
>>> cur_points = np.array([ (1, 1), (2, 4), (6, 2), (3, -1) ])
>>> other_points = np.array([ (3, 2), (6, 1), (1, -3), (5.5, -2.5) ])
>>> Tree._two_cluster_min_dist(cur_points, other_points)
(1.0, [array([6, 2]), array([ 6., 1.])])
'''
min_dist = utl.euclideanDistance(cur_points[0], other_points[0])
min_point = [ cur_points[0], other_points[0] ]
for cp in cur_points:
for op in other_points:
# tmp_dist = ( (cp[0] - op[0]) ** 2 + (cp[1] - op[1]) ** 2 ) ** 0.5
tmp_dist = utl.euclideanDistance(cp, op)
if tmp_dist < min_dist:
# print("tmp_dist = {}, min_dist = {}".format(tmp_dist, min_dist))
min_dist = tmp_dist
min_point = [cp, op]
#
# plt.clf()
# plt.plot(cur_points[:, 0], cur_points[:, 1], 'ko')
# plt.plot(other_points[:, 0], other_points[:, 1], 'k+')
# plt.plot(min_point[0][0], min_point[0][1], 'bo')
# plt.plot(min_point[1][0], min_point[1][1], 'yo')
# plt.show()
return min_dist, min_point
def isDuplicate(self, random_node):
if random_node.orientation is None:
return (utils.euclideanDistance(self.current_coords,
random_node.current_coords) < 0.5)
return ((utils.euclideanDistance(self.current_coords,
random_node.current_coords) < 0.5)
and (abs(self.orientation - random_node.orientation) < 30))
def actionMove(current_node, next_action, goal_position, clearance, plotter=plt, viz_please=False): Xi, Yi = current_node.getXYCoords() Thetai = current_node.orientation UL, UR = next_action mc = current_node.movement_cost t = 0 r = 0.038 L = 0.354 dt = 0.1 Xn = Xi Yn = Yi Thetan = math.radians(Thetai) # Xi, Yi,Thetai: Input point's coordinates # Xs, Ys: Start point coordinates for plot function # Xn, Yn, Thetan: End point coordintes while t < 1: t = t + dt Xs = Xn Ys = Yn Xn += 0.5 * r * (UL + UR) * math.cos(Thetan) * dt Yn += 0.5 * r * (UL + UR) * math.sin(Thetan) * dt Thetan += (r / L) * (UR - UL) * dt if viz_please: plotter.plot([Xs, Xn], [Ys, Yn], color="blue") mc += utils.euclideanDistance((Xs, Ys), (Xn, Yn)) # if the intermediate step hits a boundary if (Yn < MIN_COORDS[1]) or (Yn >= MAX_COORDS[1]) or (Xn < MIN_COORDS[0]) or (Xn >= MAX_COORDS[0]): return None # if the intermediate step hits an obstacle if (obstacles.withinObstacleSpace((Xn, Yn), radius=a_star.ROBOT_RADIUS, clearance=clearance)): return None cc = (Yn, Xn) pc = current_node.current_coords ori = math.degrees(Thetan) pori = current_node.orientation gc = utils.euclideanDistance(cc, goal_position) ret_val = node.Node(current_coords=cc, parent_coords=pc, orientation=ori, parent_orientation=pori, action=next_action, movement_cost=mc, goal_cost=gc) return ret_val
def actionMove(data, row_step, col_step, goal_position=None):
if ((data.current_coords[0] + row_step) < 0) or (
(data.current_coords[0] + row_step) >= input_map_dummy.shape[0]
) or ((data.current_coords[1] + col_step) < 0) or (
(data.current_coords[1] + col_step) >= input_map_dummy.shape[1]):
return None
current_coords = (data.current_coords[0] + row_step,
data.current_coords[1] + col_step)
parent_coords = data.current_coords
if (np.abs(row_step) + np.abs(col_step)) == 1:
movement_cost = data.movement_cost + 1
elif (np.abs(row_step) + np.abs(col_step)) == 2:
movement_cost = data.movement_cost + np.sqrt(2)
else:
print("WRONG movement values!!")
return None
if goal_position is None:
goal_cost = None
else:
goal_cost = utils.euclideanDistance(current_coords, goal_position)
ret_val = node.Node(current_coords, parent_coords, movement_cost,
goal_cost)
return ret_val
def calc_wards_dist(cls_a, cls_b):
centroid_a = np.mean(cls_a, axis=0)
centroid_b = np.mean(cls_b, axis=0)
size_a = len(cls_a)
size_b = len(cls_b)
dist = utl.euclideanDistance(centroid_a, centroid_b)
scale = ( (2 * size_a * size_b) / (size_a + size_b) ) ** 0.5
return scale * dist
def findClosestNode(rrt_nodes, rand_node): closest_dist = 99999 closest_node = None for rrt_node in rrt_nodes: node_dist = utils.euclideanDistance(point_1=rrt_node.getXYCoords(), point_2=rand_node.getXYCoords()) if node_dist < closest_dist: closest_dist = node_dist closest_node = rrt_node return (closest_node, closest_dist)
def is_close_enough(cur_points, other_points):
cur_rep = np.mean(cur_points, axis=0)
other_rep = np.mean(other_points, axis=0)
'''
==================================================
[10, 5, 3, 2.5, 2]
'''
threshold = utl.euclideanDistance(cur_rep, other_rep) / 3
min_dist, min_point = Tree._two_cluster_min_dist(cur_points, other_points)
print("threshold = {}, min_dist = {}".format(threshold, min_dist))
# plt.clf()
#
# plt.plot(cur_points[:, 0], cur_points[:, 1], 'ko')
# plt.plot(other_points[:, 0], other_points[:, 1], 'k+')
#
# plt.plot(cur_rep[0], cur_rep[1], 'ro')
# plt.plot(other_rep[0], other_rep[1], 'g+')
#
# plt.plot(min_point[0][0], min_point[0][1], 'bo')
# plt.plot(min_point[1][0], min_point[1][1], 'yo')
#
# plt.axes().set_aspect('equal', 'datalim')
# # plt.axis([0, 500, 0, 500])
# plt.show()
'''
==================================================
[10, 5, 0]
'''
if min_dist <= (threshold+5):
# plt.show()
return True
# else:
# plt.clf()
#
# plt.plot(cur_points[:, 0], cur_points[:, 1], 'ko')
# plt.plot(other_points[:, 0], other_points[:, 1], 'k+')
#
# plt.plot(cur_rep[0], cur_rep[1], 'ro')
# plt.plot(other_rep[0], other_rep[1], 'g+')
#
# plt.plot(min_point[0][0], min_point[0][1], 'bo')
# plt.plot(min_point[1][0], min_point[1][1], 'yo')
#
# plt.axes().set_aspect('equal', 'datalim')
# # plt.axis([0, 500, 0, 500])
# plt.show()
return False
def findClosestWayPoint(ref_point, point_list): rx, ry = ref_point min_dist = 99999 closest_wp_idx = -1 for i in range(len(point_list)): dist = utils.euclideanDistance(point_1=ref_point, point_2=point_list[i]) if dist < min_dist: min_dist = dist closest_wp_idx = i return (min_dist, closest_wp_idx)
def isDuplicate(self, random_node, dist_thresh, orient_thresh):
"""
Determines whether the specified random node is a duplicate of the current node.
:param random_node: The node to match the current object with
:type random_node: Node
:param dist_thresh: The distance threshold for duplicacy
:type dist_thresh: float
:param orient_thresh: The orientation threshold for duplicacy
:type orient_thresh: float
:returns: True if duplicate, False otherwise.
:rtype: boolean
"""
if random_node.orientation is None:
return (utils.euclideanDistance(
self.current_coords, random_node.current_coords) < dist_thres)
return (
(utils.euclideanDistance(self.current_coords,
random_node.current_coords) < dist_thresh)
and
(abs(self.orientation - random_node.orientation) < orient_thresh))
def hitsObstacle(start_node, goal_node, step_size=0.1): sn_x, sn_y = start_node.getXYCoords() gn_x, gn_y = goal_node.getXYCoords() theta = np.arctan2((gn_y - sn_y), (gn_x - sn_x)) nn_x, nn_y = sn_x, sn_y while (utils.euclideanDistance(point_1=(nn_x, nn_y), point_2=(gn_x, gn_y)) > 0.001): # next node nn_x = nn_x + (step_size * np.cos(theta)) nn_y = nn_y + (step_size * np.sin(theta)) if obs.withinObstacleSpace(point=(nn_x, nn_y), radius=main.DRONE_RADIUS, clearance=main.DRONE_RADIUS / 2): return True return False
def newOdom(msg, args):
"""
Callback Function for Subscriber to get Robot's Current Location
:param msg: Robots Odometry
:type msg: nav_msgs/Odometry
:param args: (path_list, cmd_vel publisher)
:type args: tuple
"""
global x
global y
global theta
global intermediate_goal
global path_itr
path_list = args[0]
cmd_vel_pub = args[1]
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
roll, pitch, theta = euler_from_quaternion(
(rot_q.x, rot_q.y, rot_q.z, rot_q.w))
if utils.euclideanDistance((intermediate_goal.x, intermediate_goal.y),
(x, y)) < 0.2:
intermediate_goal.x, intermediate_goal.y = path_list[
path_itr].getXYCoords()
path_itr += 1
if path_itr >= len(path_list):
speed = Twist()
speed.linear.x = 0.0
speed.angular.z = 0.0
cmd_vel_pub.publish(speed)
rospy.signal_shutdown(
"Reached the last node of the path_list!!! Yayyy!")
def pseudoDistance(clustering):
# for each cluster, returns the estimated pairwise distances of every point to every point in the cluster
distances = []
total = 0.0
count = 0
startTime = time.time()
for cluster in clustering:
# print "Cluster Size:", len(cluster.members)
'''
print "Starting True Similarities"
startTime = time.time()
trueSimilarities = map(lambda r: map(lambda c: 1-c, r) ,utils.pairwise(cluster.members, lambda x,y: x.similarity(y)))
endTime = time.time()
print "Done. Elapsed Time:", endTime-startTime
'''
#Order Matters here.
'''
namedSims = map(lambda x: cluster.center.similarities_by_name(x).items(), [cluster.center] + cluster.members)
distToCenter = map(lambda x: [1-i[1] for i in x], namedSims)
euclid = utils.pairwise(distToCenter, lambda x,y: utils.euclideanDistance(x,y))
distances.append(euclid)
'''
vectors = map(lambda x: cluster.center.similarity_vector(x), [cluster.center] + cluster.members)
distToCenter = map(lambda x: map(lambda i: 1-i, x), vectors)
euclid = utils.pairwise(distToCenter, lambda x,y: utils.euclideanDistance(x,y))
distances.append(euclid)
endTime = time.time()
print "Done. Elapsed Time:", endTime-startTime
return distances
def aStar(start_pos,
goal_pos,
robot_radius,
clearance,
step_size,
theta=30,
duplicate_step_thresh=0.5,
duplicate_orientation_thresh=30):
start_r, start_c = start_pos
goal_r, goal_c = goal_pos
start_node = node.Node(current_coords=(start_r, start_c),
parent_coords=None,
orientation=0,
parent_orientation=None,
movement_cost=0,
goal_cost=utils.euclideanDistance(
start_pos, goal_pos))
goal_node = node.Node(current_coords=(goal_r, goal_c),
parent_coords=None,
orientation=None,
parent_orientation=None,
movement_cost=None,
goal_cost=0)
# Saving a tuple with total cost and the state node
minheap = [((start_node.movement_cost + start_node.goal_cost), start_node)]
heapq.heapify(minheap)
# defining the visited node like this avoids checking if two nodes are duplicate. because there is only 1 position to store the visited information for all the nodes that lie within this area.
visited = {}
visited[(utils.valRound(start_r), utils.valRound(start_c),
0)] = start_node # marking the start node as visited
viz_visited_coords = [start_node]
while len(minheap) > 0:
_, curr_node = heapq.heappop(minheap)
# if curr_node.isDuplicate(goal_node):
if curr_node.goal_cost < (1.5 * step_size):
print("Reached Goal!")
print("Current node:---")
curr_node.printNode()
print("Goal node:---")
goal_node.printNode()
# backtrack to get the path
path = actions.backtrack(curr_node, visited)
# path = None # FOR NOW FOR DEBUGGING PURPOSES
return (path, viz_visited_coords)
# for row_step, col_step in movement_steps:
for angle in range(-60, 61, theta):
# Action Move
# next_node = actions.actionMove(curr_node, row_step, col_step)
next_node = actions.actionMove(
current_node=curr_node,
theta_step=angle,
linear_step=step_size,
goal_position=goal_node.current_coords)
if next_node is not None:
# if hit an obstacle, ignore this movement
if obstacles.withinObstacleSpace(
(next_node.current_coords[1], next_node.current_coords[0]),
robot_radius, clearance):
continue
# Check if the current node has already been visited.
# If it has, then see if the current path is better than the previous one
# based on the total cost = movement cost + goal cost
node_state = (utils.valRound(next_node.current_coords[0]),
utils.valRound(next_node.current_coords[1]),
utils.orientationBin(next_node.orientation,
theta))
if node_state in visited:
# if current cost is a better cost
# if (next_node.movement_cost + next_node.goal_cost) < (visited[node_state].movement_cost + visited[node_state].goal_cost):
if (next_node < visited[node_state]):
visited[
node_state].current_coords = next_node.current_coords
visited[
node_state].parent_coords = next_node.parent_coords
visited[node_state].orientation = next_node.orientation
visited[
node_state].parent_orientation = next_node.parent_orientation
visited[
node_state].movement_cost = next_node.movement_cost
visited[node_state].goal_cost = next_node.goal_cost
h_idx = utils.findInHeap(next_node, minheap)
if (h_idx > -1):
minheap[h_idx] = ((next_node.movement_cost +
next_node.goal_cost), next_node)
else:
# visited.append(next_node)
visited[node_state] = next_node
heapq.heappush(minheap, ((next_node.movement_cost +
next_node.goal_cost), next_node))
viz_visited_coords.append(next_node)
heapq.heapify(minheap)
def blendImageAndBackground_KeepBlurLevel(image,
mask,
background,
xIni=0,
yIni=0,
scaleFactor=1,
rotAngle=0,
flipHor=False,
saveIntermediateImages=False,
folderSaveIntermImages=''):
# A paths were passed instead of loaded images
if isinstance(image, str) and os.path.isfile(image) == True:
imagePath = image
image = cv2.imread(image)
if isinstance(mask, str) and os.path.isfile(mask) == True:
maskPath = mask
mask = cv2.imread(mask)
if isinstance(background, str) and os.path.isfile(background) == True:
backgroundPath = background
background = cv2.imread(background)
if saveIntermediateImages:
_, fil = utils.splitPathFile(imagePath)
fol = folderSaveIntermImages + '/' + fil.replace(
'.png', '').replace('.jpg', '')
# if folder exists, delete it
if os.path.isdir(fol):
shutil.rmtree(fol, ignore_errors=True)
os.makedirs(fol)
# Flip horizontally
if flipHor == True:
image = cv2.flip(image, 0)
mask = cv2.flip(mask, 0)
# Rotate counter-clockwise by the angle (in degrees)
w, h, channels = image.shape
rotMatrix = cv2.getRotationMatrix2D((w / 2, h / 2), rotAngle, 1)
image = cv2.warpAffine(image, rotMatrix, (w, h))
mask = cv2.warpAffine(mask, rotMatrix, (w, h))
# Rescale image and mask
image = cv2.resize(image,
None,
fx=scaleFactor,
fy=scaleFactor,
interpolation=cv2.INTER_CUBIC)
mask = cv2.resize(mask,
None,
fx=scaleFactor,
fy=scaleFactor,
interpolation=cv2.INTER_CUBIC)
# Before rotating the mask, the values are either 0 or 255. After rotation, some of these values
# are changed. Therefore, we need to threshold
_, mask = cv2.threshold(mask, 127, 255, cv2.THRESH_BINARY)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/mask.png', mask, [cv2.IMWRITE_PNG_COMPRESSION, 0])
mask_inv_bin = (255 - mask) / 255
mask_inv_bin = mask_inv_bin.astype(np.uint8)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/mask_inv.png', mask_inv_bin*255, [cv2.IMWRITE_PNG_COMPRESSION, 0])
# Get binary mask
mask_bin = mask / 255
mask_bin = mask_bin.astype(np.uint8)
# Blend shoe with mask
imgMaskBlended = ObjectDatabase.blendImageAndMask(image, mask)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/imgMaskBlended.png', imgMaskBlended, [cv2.IMWRITE_PNG_COMPRESSION, 0])
# Obtain 2 layers:
# layer_1 => layer between mask_bin and enlarged by 5 iterations
# layer_2 => layer between enlarged by 7 iterations and 3 iterations
_, mask_larger_2, _, _ = utils.enlargeMask(mask, 3)
_, mask_larger_1_bin, _, layer_1 = utils.enlargeMask(mask, 5)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/mask_larger_1.png', mask_larger_1_bin*255, [cv2.IMWRITE_PNG_COMPRESSION, 0])
layer_1 = 1 - layer_1
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/layer_1.png', layer_1*255, [cv2.IMWRITE_PNG_COMPRESSION, 0])
_, mask_larger_3, _, _ = utils.enlargeMask(mask, 7)
layer_2_bin = np.subtract(mask_larger_2, mask_larger_3)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/layer_2.png', layer_2_bin*255, [cv2.IMWRITE_PNG_COMPRESSION, 0])
layer_2_bin_inv = 1 - layer_2_bin
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/layer_2_bin_inv.png',layer_2_bin_inv*255 , [cv2.IMWRITE_PNG_COMPRESSION, 0])
# It's necessary to get width and height once the image was rescaled
rsz_rows, rsz_cols, rsz_channels = image.shape
# Get blur level of the region of the background:
min_x = xIni
min_y = yIni
max_x = xIni + rsz_cols
max_y = yIni + rsz_rows
# print('min_x: %s' % str(min_x))
# print('min_y: %s' % str(min_y))
# print('max_x: %s' % str(max_x))
# print('max_y: %s' % str(max_y))
# print('scaleFactor: %s' % str(scaleFactor))
# print('image.shape: %s' % str(image.shape))
# print('mask.shape: %s' % str(mask.shape))
# print('background: %s' % str(background.shape))
# define region background without shoe
roiBackground = background[min_y:max_y, min_x:max_x, :]
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/roiBackground.png', roiBackground, [cv2.IMWRITE_PNG_COMPRESSION, 0])
# print('roiBackground: %s' % str(roiBackground.shape))
blurLevelReference = utils.blur_measurement(roiBackground)
# print('blurLevelReference: %s' % str(blurLevelReference))
# backgroundWithShoeNoTreatment is used as reference only
backgroundWithShoeNoTreatment = np.multiply(roiBackground,
mask_inv_bin)
backgroundWithShoeNoTreatment = backgroundWithShoeNoTreatment + imgMaskBlended
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/backgroundWithShoeNoTreatment.png', backgroundWithShoeNoTreatment, [cv2.IMWRITE_PNG_COMPRESSION, 0])
blurImageNoTreatment = utils.blur_measurement(
backgroundWithShoeNoTreatment)
# print("blur without treatment: %s" % str(blurImageNoTreatment))
dist = utils.euclideanDistance(blurImageNoTreatment,
blurLevelReference)
# print('dist: %f' % dist)
rsz_rows, rsz_cols, rsz_channels = imgMaskBlended.shape
layer_1_with_background = np.multiply(layer_1, roiBackground)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/layer_1_with_background.png', layer_1_with_background, [cv2.IMWRITE_PNG_COMPRESSION, 0])
layer_2_with_background = np.multiply(layer_2_bin, roiBackground)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/layer_2_with_background.png', layer_2_with_background, [cv2.IMWRITE_PNG_COMPRESSION, 0])
shoes_background = np.add(imgMaskBlended, layer_1_with_background)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/shoes_background.png', shoes_background, [cv2.IMWRITE_PNG_COMPRESSION, 0])
backgroundWithLarge1ShoeMask = np.multiply(roiBackground,
mask_larger_1_bin)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/backgroundWithLarge1ShoeMask.png', backgroundWithLarge1ShoeMask, [cv2.IMWRITE_PNG_COMPRESSION, 0])
if saveIntermediateImages:
fh = open(os.path.join(fol, 'log.txt'), "a")
fh.write(
"\nBlur vector of image without treatment (backgroundWithShoeNoTreatment.png): %s"
% str(blurImageNoTreatment))
fh.write('\nBlur vector of reference (roiBackground.png): %s' %
str(blurLevelReference))
fh.write(
'\nDistance blur (backgroundWithShoeNoTreatment and roiBackground): %f'
% utils.euclideanDistance(blurImageNoTreatment,
blurLevelReference))
cv2.imwrite(os.path.join(fol, 'mask.png'), mask,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'mask_inv.png'), mask_inv_bin * 255,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'imgMaskBlended.png'),
imgMaskBlended, [cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'mask_larger_1.png'),
mask_larger_1_bin * 255,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'layer_1.png'), layer_1 * 255,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'layer_2.png'), layer_2_bin * 255,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'layer_2_bin_inv.png'),
layer_2_bin_inv * 255,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'roiBackground.png'), roiBackground,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'backgroundWithShoeNoTreatment.png'),
backgroundWithShoeNoTreatment,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'layer_1_with_background.png'),
layer_1_with_background,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'layer_2_with_background.png'),
layer_2_with_background,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'shoes_background.png'),
shoes_background, [cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, 'backgroundWithLarge1ShoeMask.png'),
backgroundWithLarge1ShoeMask,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
iteration = 0
returnImage = roiBackground
while True:
# print('iteration: %d' % iteration)
blurred_R = cv2.GaussianBlur(shoes_background[:, :, 2], (3, 3), 0)
blurred_G = cv2.GaussianBlur(shoes_background[:, :, 1], (3, 3), 0)
blurred_B = cv2.GaussianBlur(shoes_background[:, :, 0], (3, 3), 0)
blurredShoeBackground = cv2.merge(
(blurred_B, blurred_G, blurred_R))
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/blurredShoeBackground.png', blurredShoeBackground, [cv2.IMWRITE_PNG_COMPRESSION, 0])
ghostyBackgroundWithShoe = np.add(backgroundWithLarge1ShoeMask,
blurredShoeBackground)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/ghostyBackgroundWithShoe.png', ghostyBackgroundWithShoe, [cv2.IMWRITE_PNG_COMPRESSION, 0])
prebackgroundWithShoe = np.multiply(ghostyBackgroundWithShoe,
layer_2_bin_inv)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/prebackgroundWithShoe.png', prebackgroundWithShoe, [cv2.IMWRITE_PNG_COMPRESSION, 0])
finalImage = layer_2_with_background + prebackgroundWithShoe
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/finalImage.png', finalImage, [cv2.IMWRITE_PNG_COMPRESSION, 0])
blurLevel = utils.blur_measurement(finalImage)
# print('blurLevel: %s' % str(blurLevel))
distLoop = utils.euclideanDistance(blurLevel, blurLevelReference)
# print('dist: %f' % distLoop)
# cv2.imwrite('/media/rafael/Databases/databases/VDAO/teste.png', shoes_background, [cv2.IMWRITE_PNG_COMPRESSION, 0])
# Distance was increased or did not change, break loop
if distLoop >= dist:
if saveIntermediateImages:
fh.write("\nStopping... Distance increased to %f" %
distLoop)
break
else: #continue loop
dist = distLoop
shoes_background = np.multiply(finalImage,
1 - mask_larger_1_bin)
iteration = iteration + 1
if saveIntermediateImages:
fh.write(
"\nBlur vector of image loop %d (%d_finalImage.png): %s" %
(iteration, iteration, str(blurLevel)))
fh.write(
'\nDistance blur (%d_finalImage and roiBackground): %f' %
(iteration, distLoop))
cv2.imwrite(
os.path.join(fol, '%d_shoes_background.png' % iteration),
shoes_background, [cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(
os.path.join(fol,
'%d_blurredShoeBackground.png' % iteration),
blurredShoeBackground, [cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(
os.path.join(fol, '%d_ghostyBackgroundWithShoe.png' %
iteration), ghostyBackgroundWithShoe,
[cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(
os.path.join(fol,
'%d_prebackgroundWithShoe.png' % iteration),
prebackgroundWithShoe, [cv2.IMWRITE_PNG_COMPRESSION, 0])
cv2.imwrite(os.path.join(fol, '%d_finalImage.png' % iteration),
finalImage, [cv2.IMWRITE_PNG_COMPRESSION, 0])
returnImage = finalImage
if saveIntermediateImages:
fh.close()
return returnImage, [min_x, min_y, max_x, max_y]
def EuclideanModel(train_users, dev_users, test_users):
subjects = train_users["user_id"].unique()
#Se calcula la media de cada usuario agrupando el dataframe de train
groupby = train_users.groupby("user_id").mean().copy()
#Se incluye la columna del usuario
train_users = groupby.reset_index()
users_evaluation_dev = []
#Se hace el cálculo para cada usuario
for subject in subjects:
#Media del vector del usuario actual
mean_vector = train_users.loc[train_users.user_id == subject,
"ft_1":"ft_60"]
#Para cada registro del subdataset de test
for index, row in dev_users.iterrows():
temp_obj = {}
#userid del del registro actual del subdataset de test
current_user_id = row[0]
#Vector de tiempo del registro actual del subdataset de test
current_data = row[1:]
temp_obj["user_model"] = subject
temp_obj["user_id"] = current_user_id
temp_obj["score"] = euclideanDistance(mean_vector, current_data)
temp_obj["std_score"] = euclideanStandardization(
euclideanDistance(mean_vector, current_data))
#temp_obj["std_score"] = standardizationMinMax(euclideanDistance(mean_vector, current_data))
if subject == current_user_id:
temp_obj["y_test"] = "genuine"
else:
temp_obj["y_test"] = "impostor"
users_evaluation_dev.append(temp_obj)
users_evaluation_dev = pd.DataFrame(users_evaluation_dev)
#Obtenemos la listas de scores de los registros que deberian de catalogarse como genuinos por los modelos
genuine_scores_dev = list(
users_evaluation_dev.loc[users_evaluation_dev.y_test == "genuine",
"std_score"])
#Obtenemos la listas de scores de los registros que deberian de catalogarse como impostores por los modelos
impostor_scores_dev = list(
users_evaluation_dev.loc[users_evaluation_dev.y_test == "impostor",
"std_score"])
#Calculo del ERR y del umbral de decisión global
err_dev, thresh_dev = evaluate_EER_Thresh_Distance(genuine_scores_dev,
impostor_scores_dev)
#-----------------------------------------------------------------------------------
users_evaluation_test = []
#Se hace el cálculo para cada usuario
for subject in subjects:
#Media del vector del usuario actual
mean_vector = train_users.loc[train_users.user_id == subject,
"ft_1":"ft_60"]
#Para cada registro del subdataset de test
for index, row in test_users.iterrows():
temp_obj = {}
#userid del del registro actual del subdataset de test
current_user_id = row[0]
#Vector de tiempo del registro actual del subdataset de test
current_data = row[1:]
temp_obj["user_model"] = subject
temp_obj["user_id"] = current_user_id
temp_obj["score"] = euclideanDistance(mean_vector, current_data)
temp_obj["std_score"] = euclideanStandardization(
euclideanDistance(mean_vector, current_data))
#temp_obj["std_score"] = standardizationMinMax(euclideanDistance(mean_vector, current_data))
if subject == current_user_id:
temp_obj["y_test"] = "genuine"
else:
temp_obj["y_test"] = "impostor"
users_evaluation_test.append(temp_obj)
users_evaluation_test = pd.DataFrame(users_evaluation_test)
# OJO, aca se esta usando el score como umbral, si usaramos el score estandarizado, deberiamos de cambiar el sentido de la comparación
users_evaluation_test['y_pred'] = np.where(
users_evaluation_test['std_score'] >= thresh_dev, 'genuine',
'impostor')
#Obtenemos los y_test y y_pred de nuestros resultados
y_test_test = users_evaluation_test.loc[:, "y_test"]
y_pred_test = users_evaluation_test.loc[:, "y_pred"]
#-----------------------------------------------------------------------------------
#Obtenemos la listas de scores de los registros que deberian de catalogarse como genuinos por los modelos
genuine_scores_test = list(
users_evaluation_test.loc[users_evaluation_test.y_test == "genuine",
"std_score"])
#Obtenemos la listas de scores de los registros que deberian de catalogarse como impostores por los modelos
impostor_scores_test = list(
users_evaluation_test.loc[users_evaluation_test.y_test == "impostor",
"std_score"])
thresh_x, thresh_y, _ = find_fpr_and_tpr_given_a_threshold_Distance(
genuine_scores_test, impostor_scores_test, thresh_dev)
thresh_std = round(thresh_dev.tolist(), 3)
return y_test_test, y_pred_test, genuine_scores_test, impostor_scores_test, thresh_std, thresh_x, thresh_y
def rrtPlannedPath(start_node, goal_node, robot_radius, plotter, write=False):
step_size = robot_radius # *2
rrt_nodes = {start_node.getXYCoords(): start_node}
step_node = start_node
itr = 0
while (not utils.sameRegion(step_node, goal_node,
GOAL_REACH_THRESH)) and (itr < MAX_ITER):
# print("Iteration number:", itr)
itr += 1
# get random node in the direction of the goal node 40% of the times.
rand_node = rrt.getRandomNode(x_lim=X_LIM,
y_lim=Y_LIM,
curr_node=start_node,
goal_node=goal_node,
goal_probability=0.6)
if plotter is not None:
utils.plotPoint(rand_node.getXYCoords(),
plotter,
radius=0.03,
color='red')
closest_node, _ = rrt.findClosestNode(rrt_nodes.values(), rand_node)
step_node = rrt.getStepNode(closest_node, rand_node, step_size)
if plotter is not None:
utils.plotPoint(step_node.getXYCoords(),
plotter,
radius=0.04,
color='blue')
cn_x, cn_y = closest_node.getXYCoords()
sn_x, sn_y = step_node.getXYCoords()
plotter.plot([cn_x, sn_x], [cn_y, sn_y], color='blue')
# plt.show()
# plt.pause(0.5)
if rrt.hitsObstacle(start_node=closest_node,
goal_node=step_node,
step_size=(robot_radius / 2)):
if plotter is not None:
utils.plotPoint(step_node.getXYCoords(),
plotter,
radius=0.04,
color='red')
cn_x, cn_y = closest_node.getXYCoords()
sn_x, sn_y = step_node.getXYCoords()
plotter.plot([cn_x, sn_x], [cn_y, sn_y], color='red')
# plt.show()
# plt.pause(0.5)
continue
if plotter is not None:
utils.plotPoint(step_node.getXYCoords(),
plotter,
radius=0.04,
color='green')
cn_x, cn_y = closest_node.getXYCoords()
sn_x, sn_y = step_node.getXYCoords()
plotter.plot([cn_x, sn_x], [cn_y, sn_y], color='green')
# plt.show()
# plt.pause(0.5)
rrt_nodes.update({step_node.getXYCoords(): step_node})
if plotter is not None:
plt.show()
plt.pause(0.05)
if write:
plt.savefig('./frames/' + str(itr) + '.png')
# Reached Goal
if utils.sameRegion(step_node, goal_node, GOAL_REACH_THRESH):
print("Reached Goal!")
print("Number of iterations:", itr)
goal_node.parent_coords = closest_node.getXYCoords()
goal_node.distance = utils.euclideanDistance(
point_1=closest_node.getXYCoords(),
point_2=goal_node.getXYCoords())
rrt_nodes.update({goal_node.getXYCoords(): goal_node})
# rrt_nodes.append(goal_node)
path = rrt.backtrack(rrt_nodes, goal_node)
print("path:", len(path), "rrt_nodes:", len(rrt_nodes))
return (path, rrt_nodes, itr)
return (None, None, None)
def a_star(start_rc,
goal_rc,
orientation,
rpm1=10,
rpm2=20,
clearance=0.2,
viz_please=False):
"""
A-star algorithm given the start and the goal nodes.
:param start_rc: The start position
:type start_rc: tuple
:param goal_rc: The goal position
:type goal_rc: tuple
:param orientation: The orientation
:type orientation: number
:param rpm1: The rpm 1
:type rpm1: number
:param rpm2: The rpm 2
:type rpm2: number
:param clearance: The clearance
:type clearance: number
:param viz_please: Whether to visualize or not
:type viz_please: boolean
:returns: Returns path nodes and the list of visited nodes.
:rtype: tuple
"""
"""
Inputs
"""
start_node = node.Node(current_coords=start_rc,
parent_coords=None,
orientation=0,
parent_orientation=None,
action=None,
movement_cost=0,
goal_cost=utils.euclideanDistance(
start_rc, goal_rc))
print("-----------------------")
print("Start Node:")
start_node.printNode()
print("-----------------------")
goal_node = node.Node(current_coords=goal_rc,
parent_coords=None,
orientation=None,
parent_orientation=None,
action=None,
movement_cost=None,
goal_cost=0)
"""
Initializations
"""
action_set = [(0, rpm1), (rpm1, 0), (0, rpm2), (rpm2, 0), (rpm1, rpm2),
(rpm2, rpm1), (rpm1, rpm1), (rpm2, rpm2)]
min_heap = [((start_node.movement_cost + start_node.goal_cost), start_node)
]
heapq.heapify(min_heap)
visited = {}
visited.update({
(utils.getKey(start_rc[0], start_rc[1], start_node.orientation)):
start_node
})
visited_viz_nodes = [start_node]
"""
Initialize the plot figures if the visualization flag is true.
Also mark the start and the goal nodes.
"""
if viz_please:
fig, ax = plt.subplots()
ax.set(xlim=(an.MIN_COORDS[0], an.MAX_COORDS[0]),
ylim=(an.MIN_COORDS[1], an.MAX_COORDS[1]))
# ax.set(xlim=(-5, 5), ylim=(-5, 5))
ax.set_aspect('equal')
obstacles.generateMap(plotter=ax)
viz.markNode(start_node, plotter=ax, color='#00FF00', marker='o')
viz.markNode(goal_node, plotter=ax, color='#FF0000', marker='^')
plt.ion()
"""
Run the loop for A-star algorithm till the min_heap queue contains no nodes.
"""
while (len(min_heap) > 0):
_, curr_node = heapq.heappop(min_heap)
# Consider all the action moves for all the selected nodes.
for action in action_set:
new_node = an.actionMove(current_node=curr_node,
next_action=action,
goal_position=goal_rc,
clearance=clearance)
# Check if all the nodes are valid or not.
if (new_node is not None):
"""
Check if the current node is a goal node.
"""
if new_node.goal_cost < GOAL_REACH_THRESH:
print("Reached Goal!")
print("Final Node:")
new_node.printNode()
visited_viz_nodes.append(new_node)
path = an.backtrack(new_node, visited)
print("------------------------")
if viz_please:
viz.plot_curve(new_node, plotter=ax, color="red")
viz.plotPath(path,
rev=True,
pause_time=0.5,
plotter=ax,
color="lime",
linewidth=4)
plt.ioff()
plt.show()
return (path, visited_viz_nodes)
"""
Mark node as visited,
Append to min_heap queue,
Update if already visited.
"""
node_key = (utils.getKey(new_node.current_coords[0],
new_node.current_coords[1],
new_node.orientation))
if node_key in visited:
if new_node < visited[node_key]:
visited[node_key] = new_node
min_heap.append(
((new_node.movement_cost + new_node.goal_cost),
new_node))
else:
if viz_please:
viz.plot_curve(new_node, plotter=ax, color="red")
plt.show()
plt.pause(0.001)
visited.update({node_key: new_node})
min_heap.append(
((new_node.movement_cost + new_node.goal_cost),
new_node))
visited_viz_nodes.append(new_node)
# Heapify the min heap to update the minimum node in the list.
heapq.heapify(min_heap)
def rrt(start_coords, goal_coords, radius, clearance):
x_start, y_start = start_coords
x_goal, y_goal = goal_coords
start_node = node.Node(current_coords=start_coords,
parent_coords=None,
distance=None)
goal_node = node.Node(current_coords=goal_coords,
parent_coords=None,
distance=None)
print(start_node.printNode())
print(goal_node.printNode())
tree = []
path = []
# tree.append(start_node)
tree.append([0, start_node.current_coords, (0, 0)])
N = 0
while (N < MAX_ITER):
print("N:", N)
x_random = (np.random.uniform(X_LIM[0], X_LIM[1]),
np.random.uniform(Y_LIM[0], Y_LIM[1]))
# print("x_random", x_random)
##########################################
#put a check if x_random is repeated
##########################################
# print("<<<<<<<<<<<<Before updating the distance<<<<")
# print("tree", tree)
# print("<<<<<<<<<<<<After updating the distance<<<<<")
for i in tree:
# while True:
distance = utils.euclideanDistance(point_1=i[1], point_2=x_random)
i[0] = distance
# print("tree", i)
heapq.heapify(tree)
# Finding the node in the tree which is the nearest to the x_random
min_node = heapq.heappop(tree)
# print("len(tree)", len(tree))
heapq.heappush(tree, [min_node[0], min_node[1], min_node[2]])
# sys.exit(0)
# print("min node", min_node)
# print("next_node", next_node)
# print("distance", distance)
# path.append(add_node)
# tree.append([min_node[0], x_random, min_node[1]])
# print("<<<<<<<<<<<After apending the latest x random<<<<<")
# print("tree", tree)
# if the random node is within the step size
if min_node[0] <= STEP_SIZE:
# create a node with this value and add it to the path
obstacle_status = obstacles.withinObstacleSpace(
point=min_node[1], radius=radius, clearance=clearance)
if (obstacle_status == False):
add_node = node.Node(current_coords=x_random,
parent_coords=min_node[1],
distance=min_node[0])
path.append(add_node)
# print("<<<<<<<<<<<<<_before<<<<<<<<<,")
# print("len(tree)", len(tree))
heapq.heappush(tree, [
add_node.distance, add_node.current_coords,
add_node.parent_coords
])
# print("<<<<<<<<<<<<after<<<<<<<<<<,")
# print("len(tree)", len(tree))
# tree.append([add_node.distance, add_node.current_coords, add_node.parent_coords])
else:
# Find an intermediate point in between a and b
x1, y1 = min_node[1]
x2, y2 = x_random
slope = (y2 - y1) / (x2 - x1)
theta = math.atan(slope)
x_near = (x1 + (STEP_SIZE * math.cos(theta)),
y1 + (STEP_SIZE * math.sin(theta)))
obstacle_status = obstacles.withinObstacleSpace(
point=min_node[1], radius=radius, clearance=clearance)
if (obstacle_status == False):
add_node = node.Node(current_coords=x_near,
parent_coords=min_node[1],
distance=STEP_SIZE)
# print("<<<<<<<<<<<<<_before<<<<<<<<<,")
# print("len(tree)", len(tree))
path.append(add_node)
heapq.heappush(tree, [
add_node.distance, add_node.current_coords,
add_node.parent_coords
])
# print("<<<<<<<<<<<<<_before<<<<<<<<<,")
# print("len(tree)", len(tree))
# tree.append([add_node.distance, add_node.current_coords, add_node.parent_coords])
goal_status = utils.goal_reached(
current_node=add_node,
goal_node=goal_node,
goal_reach_threshold=GOAL_REACH_THRESH)
if (goal_status):
print("Reached Goal!")
return path, goal_node
# break
N += 1
# print("-------------------")
# print(path)
return path, goal_node