-
Notifications
You must be signed in to change notification settings - Fork 0
/
reference_algorithms.py
181 lines (125 loc) · 6.55 KB
/
reference_algorithms.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
import numpy as np
from scipy.stats import multivariate_normal, expon
from loglin_algorithm import LoglinDistribution
#scale factors derived from experimental data, determines relationship
#between distance of command and variance in the direction of the command
variance_scale = 0.43
variance_offset = -0.6
#TODO: create generalized "result object"
#all results from algorithm functions will return a function that has a .pdf method
#estimates the "exact" position that a command is referring to
def estimate_pos(cmd):
ref = cmd.reference
direction = cmd.direction
vector = cmd.distance*direction
return estimate_reference_pt(ref, direction) + vector
def estimate_reference_pt(ref, direction):
center = ref.center
offset = ref.width/2.*direction if direction[0] else ref.height/2.*direction
return center + offset
# takes in training data set for a given command and returns a numpy.multivariate_norm distribution
# with the sample mean and covariance
def cheating_algorithm(pts):
mean = np.mean(pts, axis=0)
covariance = np.cov(pts.T)
mv_dist = multivariate_normal(mean, covariance)
return mv_dist
#takes in a command object and a world object, and returns a numpy.multivariate_norm distribution
#with random mean and fixed covariance
def random_algorithm(cmd, world):
randx = world.xdim*np.random.random_sample()
randy = world.ydim*np.random.random_sample()
mv_dist = multivariate_normal(np.array([randx, randy]), np.array([[1, 0], [0, 1]]))
return mv_dist
#takes in a command and a world object, and returns a multivariate normal distribution
#with mean equal to the "precise" point specified in the command
#with variance in the direction of the command equal to 0.43*distance - 0.6 (derived from experimental plot of distance vs variance)
def naive_algorithm(cmd, world):
mean = estimate_pos(cmd)
#variances in the command direction and in the other direction
variance_cmd_parallel = max(cmd.distance*variance_scale + variance_offset, 0.5)
variance_cmd_ortho = 0.5 # inches hard-coding this for now, but there's definitely a relationship between it and something else
if cmd.direction[0]: #if the command is in the x direction
covar = np.array([[variance_cmd_parallel, 0], [0, variance_cmd_ortho]])
else:
covar = np.array([[variance_cmd_ortho, 0], [0, variance_cmd_parallel]])
mv_dist = multivariate_normal(mean, covar)
return mv_dist
# Estimate new mean position
# Find argmax_{x, y} naive_algorithm(cmd, world)*1/scale*e^{||(x, y) - (x_ref, y_ref)|| - min_{obj}||(x, y) - (x_obj, y_obj)||/scale}
def objects_algorithm(cmd, world):
x, y = np.mgrid[0:world.xdim:.1, 0:world.ydim:.1]
# Calculate naive distribution
naive_dist = naive_algorithm(cmd, world)
naive_vals = naive_dist.pdf(np.dstack((x, y)))
# Find Distance to closest object
ref_dists = {ref : np.sqrt((x - ref.center[0])**2 + (y - ref.center[1])**2) for ref in world.references}
min_dists = np.min(np.dstack(ref_dists[ref] for ref in ref_dists), axis=2)
# Difference between distance to closest object and object reference in command
distance_diff = ref_dists[cmd.reference] - min_dists
exp_vals = expon.pdf(distance_diff, scale=0.7)
vals = naive_vals*exp_vals
loc = np.where(vals == vals.max())
mean = 0.1*np.array([loc[0][0], loc[1][0]])
mv_dist = multivariate_normal(mean, naive_dist.cov)
return mv_dist
def objects_walls_algorithm(cmd, world, k1=4.2, k2=4.4):
x, y = np.mgrid[0:world.xdim:.1, 0:world.ydim:.1]
# Calculate naive distribution
naive_dist = naive_algorithm(cmd, world)
naive_vals = naive_dist.pdf(np.dstack((x, y)))
# Find Distance to closest object
ref_dists = {ref : np.sqrt((x - ref.center[0])**2 + (y - ref.center[1])**2) for ref in world.references}
min_ref_dists = np.min(np.dstack(ref_dists[ref] for ref in ref_dists), axis=2)
# Difference between distance to closest object and object reference in command
ref_distance_diff = ref_dists[cmd.reference] - min_ref_dists
ref_distance_vals = expon.pdf(ref_distance_diff, scale=k1)
# Find distance to nearest wall
min_wall_dists = np.min(np.dstack((x, y, world.xdim - x, world.ydim - y)), axis=2)
# Difference between distance to closest wall and object reference in command
wall_distance_diff = ref_dists[cmd.reference] - min_wall_dists
wall_distance_diff[wall_distance_diff < 0] = 0
wall_distance_vals = expon.pdf(wall_distance_diff, scale=k2)
mean_prob = naive_vals*ref_distance_vals*wall_distance_vals
loc = np.where(mean_prob == mean_prob.max())
mean = 0.1*np.array([loc[0][0], loc[1][0]])
mv_dist = multivariate_normal(mean, naive_dist.cov)
return mv_dist
# Objects + Walls, closest reference point algorithm
# Uses distance to closest reference pt on object, rather than dist to center
# Significant slowdown
def ow_refpt_algorithm(cmd, world, k1=4.8, k2=3.9):
x, y = np.mgrid[0:world.xdim:.1, 0:world.ydim:.1]
# Calculate naive distribution
naive_dist = naive_algorithm(cmd, world)
naive_vals = naive_dist.pdf(np.dstack((x, y)))
# Find Distance to closest object
ref_dists = {}
for ref in world.references:
directions = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]])
ref_pts = np.array([estimate_reference_pt(ref, direction) for direction in directions])
possible_dists = np.dstack(np.sqrt((x - pt[0])**2 + (y - pt[1])**2) for pt in ref_pts)
ref_dists[ref] = np.min(possible_dists, axis=2)
#ref_dists = {ref : np.sqrt((x - ref.center[0])**2 + (y - ref.center[1])**2) for ref in world.references}
min_ref_dists = np.min(np.dstack(ref_dists[ref] for ref in ref_dists), axis=2)
# Difference between distance to closest object and object reference in command
ref_distance_diff = ref_dists[cmd.reference] - min_ref_dists
ref_distance_vals = expon.pdf(ref_distance_diff, scale=k1)
# Find distance to nearest wall
min_wall_dists = np.min(np.dstack((x, y, world.xdim - x, world.ydim - y)), axis=2)
# Difference between distance to closest wall and object reference in command
wall_distance_diff = ref_dists[cmd.reference] - min_wall_dists
wall_distance_diff[wall_distance_diff < 0] = 0
#
wall_distance_vals = expon.pdf(wall_distance_diff, scale=k2)
mean_prob = naive_vals*ref_distance_vals*wall_distance_vals
loc = np.where(mean_prob == mean_prob.max())
mean = 0.1*np.array([loc[0][0], loc[1][0]])
mv_dist = multivariate_normal(mean, naive_dist.cov)
return mv_dist
def loglin_alg(cmd, world): # w = [0.57, 0.19, 0.24]
return LoglinDistribution(cmd, world)
#convenience function, for a 2d vector of x y points returns a vector of corresponding feature vectors
def feat_vals_vec(points, cmd, world):
feat_vals = [get_feature_vals(point[0], point[1], cmd, world).flatten() for point in points]
return np.array(feat_vals)