Mercurial > code > home > repos > homeauto

annotate service/beacon/locator.py @ 1462:2b29f14eb6bd

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
try new graph+view widget Ignore-this: d5f9c5dc52f04324368716ba2f604fdb darcs-hash:44e85a5c075ef73c34a58deaa3a3c1e8390dae52
author drewp <drewp@bigasterisk.com>
date Sun, 24 Nov 2019 00:01:00 -0800
parents 1ddd1f057749
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
1265
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
1 from __future__ import division
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
2 import ast, math
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
3
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
4 class Measurement(object):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
5 def __init__(self, d):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
6 self.d = d
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
7
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
8 def distanceTo(self, other):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
9 sharedKeys = set(self.d).intersection(other.d)
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
10 return math.sqrt(sum((other.d[f] - self.d[f])**2 for f in sharedKeys))
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
11
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
12 class Locator(object):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
13 def __init__(self):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
14 self.points = {} # num : (meas, coord)
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
15 for lineLev, lineCoord in zip(open('saved_points'), open('saved_points_coords')):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
16 n, d = lineLev.split(None, 1)
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
17 c = ast.literal_eval(lineCoord)
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
18 if c['num'] != int(n):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
19 raise ValueError("%r vs %r" % (lineLev, lineCoord))
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
20 self.points[c['num']] = (Measurement(ast.literal_eval(d)),
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
21 (round(c['x'], 3), round(c['y'], 3), round(c['z'], 3)))
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
22 for junk in [1, 2, 28, 29]:
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
23 del self.points[junk]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
24
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
25 def nearestPoints(self, meas):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
26 out = [] # (dist, coord)
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
27 for num, (knownMeas, coord) in self.points.iteritems():
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
28 out.append((round(knownMeas.distanceTo(meas), 2), coord))
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
29 out.sort()
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
30 return out
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
31
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
32 def estimatePosition(self, nearest):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
33 """list of (dist, coord) -> weighted coord"""
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
34 floors = [row[1][2] for row in nearest]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
35 freqs = [(floors.count(z), z) for z in floors]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
36 freqs.sort()
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
37 bestFloor = freqs[-1][1]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
38 sameFloorMatches = [(dist, coord) for dist, coord in nearest
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
39 if coord[2] == bestFloor]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
40 sameFloorMatches.sort()
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
41 sameFloorMatches = sameFloorMatches[:3]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
42
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
43 weightedCoord = [0, 0, 0]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
44 totalWeight = 0
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
45 for dist, coord in sameFloorMatches:
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
46 weight = (1 / (dist - sameFloorMatches[0][0] + .2))
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
47 totalWeight += weight
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
48 for i in range(3):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
49 weightedCoord[i] += weight * coord[i]
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
50 for i in range(3):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
51 weightedCoord[i] /= totalWeight
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
52 return weightedCoord
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
53
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
54 if __name__ == '__main__':
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
55 loc = Locator()
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
56 for row in loc.nearestPoints(Measurement({"bed":-78.2,"changing":-90.2,"living":-68})):
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
57 print row
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
58
1ddd1f057749 some old Measurement and Locator edits
drewp <drewp@bigasterisk.com>
parents:
diff changeset
59