229 lines
8.4 KiB
Python
229 lines
8.4 KiB
Python
|
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)
|