import math import random import time from math import sqrt from typing import List, Tuple import numpy as np import sympy from shapely import MultiPoint, Polygon, Point def get_tag_pos_from_anchor(anchor_coord: List[List], tag_distance: List[float]) -> Tuple[float, float]: node_number = len(anchor_coord) d = [coord[0] ** 2 + coord[1] ** 2 for coord in anchor_coord] # 根据观测距离用最小二乘法估计目标位置 H = np.zeros((node_number - 1, 2)) b = np.zeros(node_number - 1) for i in range(1, node_number): # A 矩阵 和 B 矩阵 H[i - 1] = [2 * (anchor_coord[i][0] - anchor_coord[1][0]), 2 * (anchor_coord[i][1] - anchor_coord[1][1])] b[i - 1] = tag_distance[1] ** 2 - tag_distance[i] ** 2 + d[i] - d[1] H_zhuanzhi = np.transpose(H) # H' hhh = np.dot(np.linalg.inv(np.dot(np.transpose(H), H)), H_zhuanzhi) # inv(H'*H)*H' estimate = np.dot(hhh, b) tag_pos = (estimate[0], estimate[1]) return tag_pos def distance(coord1, coord2): return sqrt((coord1[0] - coord2[0]) ** 2 + (coord1[1] - coord2[1]) ** 2) # 计算定位坐标 def tri_position(xa, ya, da, xb, yb, db, xc, yc, dc): x, y = sympy.symbols('x y') f1 = 2 * x * (xa - xc) + np.square(xc) - np.square(xa) + 2 * y * (ya - yc) + np.square(yc) - np.square(ya) - ( np.square(dc) - np.square(da)) f2 = 2 * x * (xb - xc) + np.square(xc) - np.square(xb) + 2 * y * (yb - yc) + np.square(yc) - np.square(yb) - ( np.square(dc) - np.square(db)) result = sympy.solve([f1, f2], [x, y]) locx, locy = result[x], result[y] return [locx, locy] def trilateration(x1, y1, d1, x2, y2, d2, x3, y3, d3): a11 = 2 * (x1 - x3) a12 = 2 * (y1 - y3) b1 = pow(x1, 2) - pow(x3, 2) + pow(y1, 2) - pow(y3, 2) + pow(d3, 2) - pow(d1, 2) a21 = 2 * (x2 - x3) a22 = 2 * (y2 - y3) b2 = pow(x2, 2) - pow(x3, 2) + pow(y2, 2) - pow(y3, 2) + pow(d3, 2) - pow(d2, 2) d = [(b1 * a22 - a12 * b2) / (a11 * a22 - a12 * a21), (a11 * b2 - b1 * a21) / (a11 * a22 - a12 * a21)] return d def get_pos_from_2_anchor(p1, r1, p2, r2): x0 = p1[0] y0 = p1[1] x1 = p2[0] y1 = p2[1] d = math.sqrt((abs(x1 - x0)) ** 2 + (abs(y1 - y0)) ** 2) if d > (r1 + r2) or d < (abs(r1 - r2)): # print("Two circles have no intersection") center = [(x0 + x1) / 2, (y0 + y1) / 2] return center, center elif d == 0: # print("Two circles have same center!") return None, None else: A = (r1 ** 2 - r2 ** 2 + d ** 2) / (2 * d) h = math.sqrt(r1 ** 2 - A ** 2) x2 = x0 + A * (x1 - x0) / d y2 = y0 + A * (y1 - y0) / d x3 = x2 - h * (y1 - y0) / d y3 = y2 + h * (x1 - x0) / d x4 = x2 + h * (y1 - y0) / d y4 = y2 - h * (x1 - x0) / d c1 = [x3, y3] c2 = [x4, y4] return c1, c2 def is_in_poly(p, poly): """ :param p: [x, y] :param poly: [[], [], [], [], ...] :return: """ px, py = p is_in = False for i, corner in enumerate(poly): next_i = i + 1 if i + 1 < len(poly) else 0 x1, y1 = corner x2, y2 = poly[next_i] if (x1 == px and y1 == py) or (x2 == px and y2 == py): # if point is on vertex is_in = True break if min(y1, y2) < py <= max(y1, y2): # find horizontal edges of polygon x = x1 + (py - y1) * (x2 - x1) / (y2 - y1) if x == px: # if point is on edge is_in = True break elif x > px: # if point is on left-side of line is_in = not is_in return is_in def point_to_line_distance(point, line): px, py = point x1, y1, x2, y2 = line line_magnitude = distance([x1, y1], [x2, y2]) if line_magnitude < 0.00000001: return 9999 else: u1 = (((px - x1) * (x2 - x1)) + ((py - y1) * (y2 - y1))) u = u1 / (line_magnitude * line_magnitude) if (u < 0.00001) or (u > 1): # 点到直线的投影不在线段内, 计算点到两个端点距离的最小值即为"点到线段最小距离" ix = distance([px, py], [x1, y1]) iy = distance([px, py], [x2, y2]) if ix > iy: dis = iy else: dis = ix else: # 投影点在线段内部, 计算方式同点到直线距离, u 为投影点距离x1在x1x2上的比例, 以此计算出投影点的坐标 ix = x1 + u * (x2 - x1) iy = y1 + u * (y2 - y1) dis = distance([px, py], [ix, iy]) return dis class AcrossDetect: def __init__(self) -> None: super().__init__() self.anchor_coordinates = {} self.anchor_side = {} self.anchor_seq = [] self.anchor_poly = [] self.anchor_geometry: Polygon = None self.max_anchor_distance = 0 @staticmethod def distance(coord1, coord2): return sqrt((coord1[0] - coord2[0]) ** 2 + (coord1[1] - coord2[1]) ** 2) # 位置检测 def position_detect(self, record): e = 7 # precise_distance = 3 record = {anchor: distance / 100 for anchor, distance in record.items()} if any([distance < e for distance in record.values()]): return True else: return False # if any([distance > self.max_anchor_distance for distance in record.values()]): # return False # try: # if len(record) > 3: # anchor_coord_list = [self.anchor_coordinates[anchor] for anchor in record.keys()] # distance_mes = list(record.values()) # p = get_tag_pos_from_anchor(anchor_coord=anchor_coord_list, tag_distance=distance_mes) # if is_in_poly(p, self.anchor_poly): # return True # else: # return False # elif 2 < len(record) <= 3: # anchor_coord_list = [self.anchor_coordinates[anchor] for anchor in record.keys()] # distance_mes = list(record.values()) # p = trilateration( # x1=anchor_coord_list[0][0], y1=anchor_coord_list[0][1], d1=distance_mes[0], # x2=anchor_coord_list[1][0], y2=anchor_coord_list[1][1], d2=distance_mes[1], # x3=anchor_coord_list[2][0], y3=anchor_coord_list[2][1], d3=distance_mes[2], # ) # if is_in_poly(p, self.anchor_poly): # return True # else: # return False # elif 1 < len(record) <= 2: # anchor_coord_list = [self.anchor_coordinates[anchor] for anchor in record.keys()] # distance_mes = list(record.values()) # p1, p2 = get_pos_from_2_anchor( # p1=anchor_coord_list[0], r1=distance_mes[0], # p2=anchor_coord_list[1], r2=distance_mes[1], # ) # p_dis = self.distance(p1, p2) # p1_in = is_in_poly(p1, self.anchor_poly) # p2_in = is_in_poly(p2, self.anchor_poly) # if (p1_in or p2_in) and p_dis < precise_distance: # return True # else: # return False # else: # dis = list(record.values())[0] # if dis <= e: # return True # else: # return False # except: # return False # 设置基站坐标 def set_anchor_coordinates(self, anchor_coordinates): return self.anchor_coordinates = anchor_coordinates dis_list = [ distance(anchor_0, anchor_1) for anchor_0 in anchor_coordinates.values() for anchor_1 in anchor_coordinates.values() ] self.max_anchor_distance = max(dis_list) self.anchor_seq.clear() self.anchor_poly.clear() mp = MultiPoint(list(anchor_coordinates.values())) self.anchor_geometry = mp.convex_hull self.anchor_geometry: Polygon x, y = self.anchor_geometry.boundary.xy x = x.tolist()[0:-1] y = y.tolist()[0:-1] coords_seq = [[x[i], y[i]] for i in range(len(x))] for coords in coords_seq: for anchor, anchor_coord in anchor_coordinates.items(): if distance(coords, anchor_coord) < 0.1: self.anchor_seq.append(anchor) self.anchor_poly.append(anchor_coord)