예제 #1
0
  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
예제 #4
0
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
예제 #5
0
  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
예제 #6
0
파일: rrt.py 프로젝트: satyarth934/rrt-uav
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)
예제 #7
0
  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
예제 #8
0
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))
예제 #10
0
파일: rrt.py 프로젝트: satyarth934/rrt-uav
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
예제 #11
0
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!")
예제 #12
0
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)
예제 #14
0
    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]
예제 #15
0
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
예제 #16
0
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)
예제 #18
0
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