def init_assignment(formation_setted):
global marker_1
global marker_2
global marker_3
global marker_4
marker_1=Marker(1,position_3D=(formation_setted[0][0],formation_setted[0][1],0))
marker_2=Marker(2,position_3D=(formation_setted[1][0],formation_setted[1][1],0))
marker_3=Marker(3,position_3D=(formation_setted[2][0],formation_setted[2][1],0))
marker_4=Marker(4,position_3D=(formation_setted[3][0],formation_setted[3][1],0))
markers=[marker_1,marker_2,marker_3,marker_4]
hung_index=assignment_change(formation_setted)
ms_pos=MarkersPosition()
ms_pos.markers_pos=markers
ms_pos.hungarian_index=hung_index
def test():
f1=MarkersPosition()
marker1=Marker(1,position_3D=(1,2,3))
marker2=Marker(2,position_3D=(4,5,6))
# marker6=Marker(5,position_3D=(1,3,6))
marker3=Marker(3,position_3D=(1,2,3))
marker4=Marker(4,position_3D=(4,5,6))
marker5=Marker(5,position_3D=(4,5,6))
marker6=Marker(5,position_3D=(4,5,6))
markers=[marker1,marker2,marker3,marker4,marker5,marker6]
f1.markers_pos=markers
f1.hungarian_index=[1,2,3,4]
print(f1._marker1_pos)
print(f1._marker2_pos)
print(f1.markers_pos_string)
f2=MarkersPosition()
marker_append=Marker(5)
f2.markers_pos=[marker_append]
print(f1.hungarian_index_string)
print("f1.hungarian_index_string",f1.hungarian_index_string)
def generate_marker_5_pos(markers):
gen_markers = []
for marker in markers:
if marker.id != 5:
gen_markers.append(marker)
continue
marker_position = marker.position
pos_1 = (marker_position[0], marker_position[1], marker_position[2])
new_marker_1 = Marker(id=5, contours=marker.contour, position_3D=pos_1)
gen_markers.append(new_marker_1)
break
return gen_markers
def test_assignment():
global marker_1
global marker_2
global marker_3
global marker_4
marker1 = Marker(1, position_3D=(-505.41, 301.34, 2604.7))
marker2 = Marker(2, position_3D=(638.98, 349.74, 2649.97))
marker3 = Marker(3, position_3D=(604.34, -611.95, 2619.11))
marker4 = Marker(4, position_3D=(189.05, -202.78, 2596.1))
# marker5=Marker(5,position_3D=(0,5,0))
markers = [marker1, marker2, marker3, marker4]
for marker in markers:
if marker.id == 1:
marker_1 = marker
elif marker.id == 2:
marker_2 = marker
elif marker.id == 3:
marker_3 = marker
elif marker.id == 4:
marker_4 = marker
markers_after = assignment_change(formation_H)
print("index:", markers_after)
def for_adjust():
f2 = MarkersPosition()
marker1 = Marker(1, position_3D=(400, 2, 400))
marker2 = Marker(2, position_3D=(800, 5, 700))
marker3 = Marker(3, position_3D=(1200, 2, 400))
marker4 = Marker(4, position_3D=(1600, 5, 700))
marker5 = Marker(5, position_3D=(0, 5, 0))
marker6 = Marker(5, position_3D=(3000, 3, 0))
markers = [marker1, marker2, marker3, marker4, marker5, marker6]
f2.markers_pos = markers
def test():
f2 = MarkersPosition()
marker1 = Marker(1, position_3D=(400, 2, 400))
marker2 = Marker(2, position_3D=(800, 5, 700))
marker3 = Marker(3, position_3D=(1200, 2, 400))
marker4 = Marker(4, position_3D=(1600, 5, 700))
marker5 = Marker(5, position_3D=(0, 5, 0))
marker6 = Marker(5, position_3D=(3000, 3, 0))
# marker7=Marker(5,position_3D=(2,5,7))
# marker8=Marker(5,position_3D=(1,3,6))
markers = [marker1, marker2, marker3, marker4, marker5, marker6]
f2.markers_pos = markers
# f2.hungarian_index=[1,2,3,0]
print("starting")
test_case = UDP_Server() #('192.168.1.103',5000)
test_case.start_server()
print("ending")
while True:
# print("hello")
a = 1 + 2
continue
from marker_detection.udp_server import UDP_Server
from marker_detection.marker import Marker
from sklearn.externals import joblib
import time
import math
import numpy as np
from marker_detection.hungarian_assignment import TaskAssignment
from marker_detection.util import draw_line_two_points, calc_middle_two_points
Load_Autoencoder = False
formation_H = [[0, 0], [800, 0], [800, -800], [0, -800]]
formation_I = [[0, 400], [0, 0], [0, -400], [0, -800]]
formation_T = [[-400, 0], [0, 0], [0, 800], [400, 0]]
marker_1 = Marker(id=1)
marker_2 = Marker(id=2)
marker_3 = Marker(id=3)
marker_4 = Marker(id=4)
marker_5 = []
def test():
frame = cv2.imread("./test_img/test_img_81.jpg")
print(frame.shape)
model = joblib.load("./marker_detection/neigh_model.m")
markers = detect_markers(frame, model)
# markers=simple_seprate_contour(markers)
# print("markers:",markers)
for marker in markers:
try:
def simple_seprate_contour(markers):
def calc_distance(point1, point2):
return math.sqrt((point1[0] - point2[0])**2 +
(point1[1] - point2[1])**2)
def clac_area_triangle(point1, point2, point3):
a = calc_distance(point1, point2)
b = calc_distance(point2, point3)
c = calc_distance(point3, point1)
p = (a + b + c) / 2
# print("a:",a,"b:",b,"c:",c,"p:",p)
return math.sqrt(abs(p * (p - a) * (p - b) * (p - c)))
def calc_area_square(contour, point):
area_1 = clac_area_triangle(point, contour[0], contour[1])
area_2 = clac_area_triangle(contour[1], point, contour[2])
area_3 = clac_area_triangle(contour[3], contour[2], point)
area_4 = clac_area_triangle(contour[0], contour[3], point)
return (area_1 + area_2 + area_3 + area_4)
def point_in_contour(point, contour):
# print("contour:",contour)
# input()
contour_center = np.mean(contour, axis=0)
contour_area = calc_area_square(contour, contour_center)
point_to_contour_area = calc_area_square(contour, point)
epsilon = 0.1
if point_to_contour_area > contour_area + epsilon:
return False
else:
return True
markers_info = [] #tuple包含了四个组成部分:(id,center,contour,area)
# print("markers:",markers)
# input()
for marker in markers:
# print("marker__:",marker.contour)
if (marker.center == None):
continue
cur_contour = []
cur_contour.append(marker.contour[0][0])
cur_contour.append(marker.contour[1][0])
cur_contour.append(marker.contour[2][0])
cur_contour.append(marker.contour[3][0])
marker_info = (marker.id, marker.center, cur_contour,
calc_area_square(cur_contour, marker.center))
markers_info.append(marker_info)
sorted(markers_info, key=lambda x: (x[3]), reverse=True)
marker_list = []
for index, marker_info in enumerate(markers_info):
if index + 1 < len(markers_info):
marker_need_remove = False
for i in range(index + 1, len(markers_info)):
if point_in_contour(marker_info[1], markers_info[i][2]):
marker_need_remove = True
break
if marker_need_remove == False:
marker_list.append(
Marker(id=marker_info[0],
contours=np.array(marker_info[2])))
else:
marker_list.append(
Marker(id=marker_info[0], contours=np.array(marker_info[2])))
return marker_list
def detect_markers(img, model, edge_length=120, Load_Autoencoder=False):
def validate_digit_marker(detector, marker):
rows, cols = marker.shape
res_4 = []
# cv2.imshow("frame_marker:",marker)
# print("marker",marker)
# cv2.waitKey(0)
# _label=input()
# cv2.imwrite("./img_dataset/new_img_dataset"+str(random.randint(1,3000))+"_"+str(_label)+".png",marker)
# cv2.destroyAllWindows()
for i in range(4):
M = cv2.getRotationMatrix2D(
(int(MARKER_SIZE / 2), int(MARKER_SIZE / 2)), 90 * i, 1)
rotated_img = cv2.warpAffine(marker, M, (0, 0))
# cv2.imshow("frame:",rotated_img)
# cv2.waitKey(0)
rotated_img_reshaped = rotated_img.reshape(
-1, MARKER_SIZE * MARKER_SIZE)
if Load_Autoencoder:
rotated_img_reshaped = eval_model(rotated_img_reshaped)
pred_res = detector.predict(rotated_img_reshaped)[0]
res_4.append(pred_res)
res_4.append(res_4[0])
# print("res_4:",res_4)
counts = np.bincount(res_4)
res = int(np.argmax(counts))
#返回众数
return res
width, height, _ = img.shape
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# cv2.namedWindow('image', cv2.WINDOW_NORMAL)
# cv2.imshow('image',gray)
# cv2.waitKey(0)
# cv2.destroyAllWindows()
# gray = cv2.medianBlur(gray,3)
# Otsu's thresholding
ret2, gray_edge = cv2.threshold(gray.copy(), 175, 255,
cv2.THRESH_BINARY) #+cv2.THRESH_OTSU
# gray_close = cv2.medianBlur(gray_edge,5)
# cv2.namedWindow('image_close', cv2.WINDOW_NORMAL)
# cv2.imshow("image_close", gray_edge)
# cv2.waitKey(0)
#
# gray = cv2.equalizeHist(gray)
edges = cv2.Canny(gray_edge, 20, 200)
# _img_cont, cont, hier = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
# _img = cv2.drawContours(gray, cont, -1, 0, 3)
# cv2.namedWindow('image', cv2.WINDOW_NORMAL)
# cv2.imshow('image',_img)
# cv2.waitKey(0)
# cv2.destroyAllWindows()
#
contours, hierarchy = cv2.findContours(edges.copy(), cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)[-2:]
# We only keep the long enough contours
min_contour_length = min(width, height) / 80
contours = [
contour for contour in contours if len(contour) > min_contour_length
]
warped_size = 49
canonical_marker_coords = array(
((0, 0), (warped_size - 1, 0), (warped_size - 1, warped_size - 1),
(0, warped_size - 1)),
dtype='float32')
# print("canonical_marker_coords:",canonical_marker_coords)
markers_list = []
# print("len(contours):",len(contours))
for contour in contours:
approx_curve = cv2.approxPolyDP(contour, len(contour) * 0.01, True)
if not (cv2.isContourConvex(approx_curve) and len(approx_curve) == 4):
continue
sorted_curve = array(cv2.convexHull(approx_curve, clockwise=False),
dtype='float32')
# print("sorted_curve:",sorted_curve)
persp_transf = cv2.getPerspectiveTransform(sorted_curve,
canonical_marker_coords)
warped_img = cv2.warpPerspective(gray_edge, persp_transf,
(warped_size, warped_size))
# warped_gray = cv2.cvtColor(warped_img, cv2.COLOR_BGR2GRAY)
warped_gray = 255 - warped_img
# _, warped_bin = cv2.threshold(warped_gray, 210, 255, cv2.THRESH_BINARY)
# cv2.imshow('image_gry',warped_bin)
# cv2.waitKey(0)
# print("shape:",warped_bin.shape)
warped_bin = cv2.resize(warped_gray, (MARKER_SIZE, MARKER_SIZE))
# cv2.imshow('image_gry',warped_bin)
# cv2.waitKey(0)
ret2, wraped_bin = cv2.threshold(warped_bin.copy(), 190, 255,
cv2.THRESH_BINARY) #
marker = warped_bin.reshape([
MARKER_SIZE,
int(warped_size / MARKER_SIZE), MARKER_SIZE,
int(warped_size / MARKER_SIZE)
])
# cv2.imshow('image_gry',warped_bin)
# cv2.waitKey(0)
# print("marker:",marker.shape)
marker = marker.mean(axis=3).mean(axis=1)
marker = marker / 255.0
marker[marker < 0.3] = 0
marker[marker >= 0.3] = 1
try:
# print("############ validating ##################")
marker_id = validate_digit_marker(model, marker)
_marker = Marker(id=marker_id, contours=approx_curve)
# print("_marker:",_marker)
if marker_id == 0:
continue
markers_list.append(_marker)
# print(mar)
except ValueError:
continue
markers_list = simple_seprate_contour(markers_list)
for marker in markers_list:
position_3D = calc_distance_to_marker(marker, edge_length)
marker.position = position_3D
return markers_list