add capability to localize the target when multiple 11mc devices are used
via trilateration
This commit is contained in:
parent
110cc00483
commit
d58044615d
|
|
@ -0,0 +1,184 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import os
|
||||
import time
|
||||
import math
|
||||
import argparse
|
||||
|
||||
from numpy import median
|
||||
|
||||
# ======= start =======
|
||||
# excerpt from
|
||||
# https://github.com/noomrevlis/trilateration/blob/master/trilateration2D.py
|
||||
|
||||
|
||||
class Point(object):
|
||||
def __init__(self, x, y):
|
||||
self.x = round(float(x), 6)
|
||||
self.y = round(float(y), 6)
|
||||
|
||||
|
||||
class Circle(object):
|
||||
def __init__(self, p, radius):
|
||||
if isinstance(p, Point):
|
||||
self.center = p
|
||||
elif isinstance(p, list):
|
||||
self.center = Point(p[0], p[1])
|
||||
self.radius = round(float(radius), 6)
|
||||
|
||||
|
||||
def get_distance(p1, p2):
|
||||
return math.sqrt(
|
||||
(p1.x - p2.x) * (p1.x - p2.x) +
|
||||
(p1.y - p2.y) * (p1.y - p2.y)
|
||||
)
|
||||
|
||||
|
||||
def get_two_circles_intersecting_points(c1, c2):
|
||||
p1 = c1.center
|
||||
p2 = c2.center
|
||||
r1 = c1.radius
|
||||
r2 = c2.radius
|
||||
|
||||
d = get_distance(p1, p2)
|
||||
# if to far away, or self contained - can't be done
|
||||
if d >= (r1 + r2) or d <= math.fabs(r1 - r2):
|
||||
return None
|
||||
|
||||
a = (r1 * r1 - r2 * r2 + d * d) / (2 * d)
|
||||
h = math.sqrt(pow(r1, 2) - pow(a, 2))
|
||||
x0 = p1.x + a*(p2.x - p1.x)/d
|
||||
y0 = p1.y + a*(p2.y - p1.y)/d
|
||||
rx = -(p2.y - p1.y) * (h/d)
|
||||
ry = -(p2.x - p1.x) * (h / d)
|
||||
return [
|
||||
Point(x0 + rx, y0 - ry),
|
||||
Point(x0 - rx, y0 + ry)
|
||||
]
|
||||
|
||||
|
||||
def get_intersecting_points(circles):
|
||||
points = []
|
||||
num = len(circles)
|
||||
for i in range(num):
|
||||
j = i + 1
|
||||
for k in range(j, num):
|
||||
res = get_two_circles_intersecting_points(circles[i], circles[k])
|
||||
if res:
|
||||
points.extend(res)
|
||||
return points
|
||||
|
||||
|
||||
def is_contained_in_circles(point, circles):
|
||||
for i in range(len(circles)):
|
||||
if (
|
||||
get_distance(point, circles[i].center) > circles[i].radius
|
||||
):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def get_polygon_center(points):
|
||||
center = Point(float('nan'), float('nan'))
|
||||
xs = []
|
||||
ys = []
|
||||
num = len(points)
|
||||
if num is 0:
|
||||
return center
|
||||
for i in range(num):
|
||||
xs.append(points[i].x)
|
||||
ys.append(points[i].y)
|
||||
center.x = median(xs)
|
||||
center.y = median(ys)
|
||||
return center
|
||||
# ======= end =======
|
||||
|
||||
|
||||
def trilateration2d(mydict, bounds=None, verbose=False):
|
||||
'''
|
||||
bound format: {
|
||||
'x_min': float,
|
||||
'x_max': float,
|
||||
'y_min': float,
|
||||
'y_max': float
|
||||
}
|
||||
'''
|
||||
points = []
|
||||
circles = []
|
||||
for loc in mydict:
|
||||
tmp = loc.split(',')
|
||||
p = Point(tmp[0], tmp[1])
|
||||
points.append(p)
|
||||
c = Circle(p, mydict[loc])
|
||||
circles.append(c)
|
||||
# print(len(points), len(circles))
|
||||
inner_points = []
|
||||
for p in get_intersecting_points(circles):
|
||||
if not is_contained_in_circles(p, circles):
|
||||
continue
|
||||
if bounds is not None:
|
||||
if bounds.get('x_min', None) is not None and bounds['x_min'] > p.x:
|
||||
continue
|
||||
if bounds.get('x_max', None) is not None and bounds['x_max'] < p.x:
|
||||
continue
|
||||
if bounds.get('y_min', None) is not None and bounds['y_min'] > p.y:
|
||||
continue
|
||||
if bounds.get('y_max', None) is not None and bounds['y_max'] < p.y:
|
||||
continue
|
||||
inner_points.append(p)
|
||||
if verbose:
|
||||
print('* Inner points:')
|
||||
if len(inner_points) is 0:
|
||||
print('*** No inner points detected!!')
|
||||
for point in inner_points:
|
||||
print('*** ({0:.3f}, {1:.3f})'.format(point.x, point.y))
|
||||
center = get_polygon_center(inner_points)
|
||||
return (center.x, center.y)
|
||||
|
||||
|
||||
def deriveLocation(args, results):
|
||||
# TODO: currently assume 2D
|
||||
loc_distance = {}
|
||||
for mac in results:
|
||||
if mac not in args['config_entry']:
|
||||
continue
|
||||
loc = args['config_entry'][mac].get('location', None)
|
||||
if loc is None:
|
||||
continue
|
||||
loc_distance[loc] = results[mac]
|
||||
loc = trilateration2d(
|
||||
loc_distance,
|
||||
bounds=args.get('loc_bounds', None),
|
||||
verbose=args.get('verbose', False)
|
||||
)
|
||||
if args.get('filepath', False):
|
||||
with open("{0}_locs".format(args['filepath']), 'a') as f:
|
||||
f.write(
|
||||
"{0:.6f},{1:.4f},{2:.4f}\n"
|
||||
.format(time.time(), loc[0], loc[1])
|
||||
)
|
||||
return loc
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
deriveLocation(
|
||||
{
|
||||
'config_entry': {
|
||||
'34:f6:4b:5e:69:1f': {'location': '0,0'},
|
||||
'34:f6:4b:5e:69:1e': {'location': '1,1'},
|
||||
'34:f6:4b:5e:69:1d': {'location': '0,2'},
|
||||
'34:f6:4b:5e:69:1a': {'location': '1,2'}
|
||||
},
|
||||
'verbose': True,
|
||||
'filepath': 'test.txt',
|
||||
'loc_bounds': {
|
||||
'x_min': 0
|
||||
}
|
||||
},
|
||||
{
|
||||
'34:f6:4b:5e:69:1f': 100,
|
||||
'34:f6:4b:5e:69:1e': 100,
|
||||
'34:f6:4b:5e:69:1d': 220
|
||||
}
|
||||
)
|
||||
|
|
@ -9,6 +9,7 @@ import argparse
|
|||
import subprocess
|
||||
|
||||
from numpy import median, sqrt
|
||||
from libLocalization import deriveLocation
|
||||
|
||||
|
||||
def which(program):
|
||||
|
|
@ -114,6 +115,7 @@ class Measurement(object):
|
|||
if not matches:
|
||||
return []
|
||||
result = []
|
||||
mytime = time.time()
|
||||
for match in matches:
|
||||
mac = match.group(1)
|
||||
status = int(match.group(3))
|
||||
|
|
@ -140,7 +142,7 @@ class Measurement(object):
|
|||
.format(
|
||||
mac, distance, rtt, rtt_var,
|
||||
raw_distance, raw_distance_var,
|
||||
rssi, time.time()
|
||||
rssi, mytime
|
||||
)
|
||||
)
|
||||
return result
|
||||
|
|
@ -204,6 +206,13 @@ def wrapper(args):
|
|||
)
|
||||
for mac in results:
|
||||
print('* {0} is {1:.4f}cm away.'.format(mac, results[mac]))
|
||||
# calculate location info
|
||||
if args['locs']:
|
||||
loc = deriveLocation(args, results)
|
||||
print(
|
||||
'* Derived location: ({0:.3f}, {1:.3f})'
|
||||
.format(loc[0], loc[1])
|
||||
)
|
||||
except KeyboardInterrupt:
|
||||
break
|
||||
except Exception as e:
|
||||
|
|
@ -229,9 +238,9 @@ def main():
|
|||
)
|
||||
p.add_argument(
|
||||
'--rounds',
|
||||
default=3,
|
||||
default=1,
|
||||
type=int,
|
||||
help="how many rounds to run one command; default is 3"
|
||||
help="how many rounds to run one command; default is 1"
|
||||
)
|
||||
p.add_argument(
|
||||
'--interface', '-i',
|
||||
|
|
@ -258,6 +267,15 @@ def main():
|
|||
"(will be ignored if `cali` is being used)"
|
||||
)
|
||||
)
|
||||
p.add_argument(
|
||||
'--locs',
|
||||
default=False,
|
||||
action="store_true",
|
||||
help=(
|
||||
"if set, derive location" +
|
||||
"and store it to file"
|
||||
)
|
||||
)
|
||||
try:
|
||||
args = vars(p.parse_args())
|
||||
except Exception as e:
|
||||
|
|
|
|||
Loading…
Reference in New Issue