# -*- coding: utf-8 -*-
import math
import cv2 as cv
class Point(object):
def __init__(self, position, parent):
self.position = position
self.parent = parent
self.F = 0
self.G = 0
self.H = 0
# 全局阈值
def threshold_demo(image):
gray = cv.cvtColor(image, cv.COLOR_RGB2GRAY) # 把输入图像灰度化
# 直接阈值化是对输入的单通道矩阵逐像素进行阈值分割。
ret, binary = cv.threshold(gray, 0, 255, cv.THRESH_BINARY | cv.THRESH_TRIANGLE)
# print("threshold value %s" % ret)
# cv.imshow("binary0", binary)
return binary
src = cv.imread('C:/tensor/map.jpg')
# cv.imshow('input_image', src)
bi = threshold_demo(src)
def estimate_distance(from_point, target_point):
return math.sqrt(math.pow(target_point.position[0] - from_point.position[0], 2) + math.pow(
target_point.position[1] - from_point.position[1], 2))
def is_same_node(point, target_point):
if point.position[0] == target_point.position[0] and point.position[1] == target_point.position[1]:
return True
return False
def is_point_in_list(point, p_list):
for p in p_list:
if is_same_node(p, point):
return True
return False
def get_point_from_list(point, p_list):
for p in p_list:
if is_same_node(p, point):
return p
return None
def display_path(last_point):
point_path = [last_point]
last_point = last_point.parent
while last_point is not None:
point_path.append(last_point)
last_point = last_point.parent
point_path.reverse()
path_str = ''
for p in point_path:
path_str += '[' + str(p.position[0]) + ',' + str(p.position[1]) + ']-->'
print(path_str)
image = src
for point in point_path:
cv.circle(image, (point.position[1], point.position[0]), 1, (0, 0, 255), 1)
image = cv.resize(image, (bi.shape[1]*4, bi.shape[0]*4))
cv.imshow("final", image)
def filter_not_reachables(map, points):
new_points = []
for point in points:
if map[point.position[0]][point.position[1]] == 255:
new_points.append(point)
return new_points
def get_periphery_points(map, point):
points = []
x = point.position[0]
y = point.position[1]
points.append(Point([x - 1, y - 1], None))
points.append(Point([x, y - 1], None))
points.append(Point([x + 1, y - 1], None))
points.append(Point([x - 1, y], None))
points.append(Point([x + 1, y], None))
points.append(Point([x - 1, y + 1], None))
points.append(Point([x, y + 1], None))
points.append(Point([x + 1, y + 1], None))
valid_points = []
for p in points:
if 0 <= p.position[0] < map.shape[0] and 0 <= p.position[1] < map.shape[1]:
valid_points.append(p)
return valid_points
def pick_one_min_F_point(p_list):
if len(p_list) == 0:
return None
if len(p_list) == 1:
return p_list[0]
min_F = p_list[0].F
min_idx = 0
for idx, p in enumerate(p_list[1:]):
if p.F < min_F:
min_F = p.F
min_idx = idx + 1
return p_list[min_idx]
def filter_ignored(points):
new_points = []
if len(points) <= 0:
return new_points
for p in points:
if p.ignore:
continue
new_points.append(p)
return new_points
def a_star(map):
width, height = map.shape
print(' ', width, 'height: ', height)
print(width * height)
target_point = Point([width - 1, height - 1], None)
from_point = Point([0, 0], None)
from_point.G = 0
from_point.H = estimate_distance(from_point, target_point)
from_point.F = from_point.G + from_point.H
open_list = []
close_list = []
open_list.append(from_point)
while len(open_list) > 0:
cur_point = pick_one_min_F_point(open_list)
if cur_point is None:
raise ValueError('无法找到可达路径')
points = get_periphery_points(map, cur_point)
points = filter_not_reachables(map, points)
for point in points:
if is_point_in_list(point, open_list):
point.new_added = False
point.ignore = False
p = get_point_from_list(point, open_list)
point.parent = p.parent
point.F = p.F
point.G = p.G
point.H = p.H
elif is_point_in_list(point, close_list):
point.new_added = False
point.ignore = True
p = get_point_from_list(point, close_list)
point.parent = p.parent
point.F = p.F
point.G = p.G
point.H = p.H
else:
point.new_added = True
point.ignore = False
open_list.append(point)
points = filter_ignored(points)
for point in points:
if point.new_added:
point.parent = cur_point
# 计算FGH
point.G = cur_point.G + 1
point.H = estimate_distance(point, target_point)
point.F = point.G + point.H
else:
# 计算FGH
old_f = point.G + point.H
new_f = cur_point.G + 1 + point.H
# 比较新的和老的F值哪个大
if new_f < old_f:
# 覆盖新的FGH/PARENT
point.parent = cur_point
point.G = cur_point.G + 1
point.F = point.G + point.H
for point in points:
if is_same_node(point, target_point):
display_path(point)
return
open_list.remove(cur_point)
close_list.append(cur_point)
a_star(bi)
cv.waitKey(0)
cv.destroyAllWindows()
