def oldbeliefUpdate(self, belief, responses=None, copPoses=None):
print('UPDATING BELIEF')
#1. partition means into separate GMs, 1 for each room
allBels = []
allBounds = []
copBounds = []
weightSums = []
for room in self.map_.rooms:
tmp = GM()
tmpw = 0
allBounds.append([
self.map_.rooms[room]['min_x'], self.map_.rooms[room]['min_y'],
self.map_.rooms[room]['max_x'], self.map_.rooms[room]['max_y']
])
for g in belief:
m = [g.mean[2], g.mean[3]]
# if mean is inside the room
if (m[0] < self.map_.rooms[room]['max_x']
and m[0] > self.map_.rooms[room]['min_x']
and m[1] < self.map_.rooms[room]['max_y']
and m[1] > self.map_.rooms[room]['min_y']):
tmp.addG(deepcopy(g))
tmpw += g.weight
tmp.normalizeWeights()
allBels.append(tmp)
weightSums.append(tmpw)
pose = copPoses[-1]
roomCount = 0
copBounds = 0
for room in self.map_.rooms:
if (pose[0] < self.map_.rooms[room]['max_x']
and pose[0] > self.map_.rooms[room]['min_x']
and pose[1] < self.map_.rooms[room]['max_y']
and pose[1] > self.map_.rooms[room]['min_y']):
copBounds = self.rooms_map_inv[room]
roomCount += 1
viewCone = Softmax()
viewCone.buildTriView(pose, length=1, steepness=10)
for i in range(0, len(viewCone.weights)):
viewCone.weights[i] = [
0, 0, viewCone.weights[i][0], viewCone.weights[i][1]
]
#Only update room that cop is in with view cone update
#Make sure to renormalize that room
# newerBelief = GM();
# for i in range(1,5):
# tmpBel = viewCone.runVBND(allBels[copBounds],i);
# newerBelief.addGM(tmpBel);
# allBels[copBounds] = newerBelief;
#Update all rooms
for i in range(0, len(allBels)):
newerBelief = GM()
for j in range(1, 5):
tmpBel = viewCone.runVBND(allBels[i], j)
if (j == 1):
tmpBel.scalerMultiply(.8)
newerBelief.addGM(tmpBel)
allBels[i] = newerBelief
for i in range(0, len(allBels)):
allBels[i].normalizeWeights()
#allBels[copBounds].normalizeWeights();
print('allBels LENGTH: {}'.format(len(allBels)))
#2. use queued observations to update appropriate rooms GM
if (responses is not None):
for res in responses:
roomNum = res[0]
mod = res[1]
clas = res[2]
sign = res[3]
if (roomNum == 0):
#apply to all
for i in range(0, len(allBels)):
if (sign == True):
allBels[i] = mod.runVBND(allBels[i], 0)
else:
tmp = GM()
for j in range(1, mod.size):
tmp.addGM(mod.runVBND(allBels[i], j))
allBels[i] = tmp
# else:
# print('ROOM NUM: {}'.format(roomNum))
# #apply to roomNum-1;
# if(sign == True):
# allBels[roomNum-1] = mod.runVBND(allBels[roomNum-1],clas);
# else:
# tmp = GM();
# for i in range(1,mod.size):
# if(i!=clas):
# tmp.addGM(mod.runVBND(allBels[roomNum-1],i));
# allBels[roomNum-1] = tmp;
else:
print('ROOM NUM: {}'.format(roomNum))
#apply to all rooms
for i in range(0, len(allBels)):
if (sign == True):
allBels[i] = mod.runVBND(allBels[i], clas)
else:
tmp = GM()
for j in range(1, mod.size):
if (j != clas):
tmp.addGM(mod.runVBND(allBels[i], j))
allBels[i] = tmp
#2.5. Make sure all GMs stay within their rooms bounds:
#Also condense each mixture
for gm in allBels:
for g in gm:
g.mean[2] = max(g.mean[2],
allBounds[allBels.index(gm)][0] - 0.01)
g.mean[2] = min(g.mean[2],
allBounds[allBels.index(gm)][2] + 0.01)
g.mean[3] = max(g.mean[3],
allBounds[allBels.index(gm)][1] - 0.01)
g.mean[3] = min(g.mean[3],
allBounds[allBels.index(gm)][3] + 0.01)
for i in range(0, len(allBels)):
allBels[i].condense(15)
# allBels[i] = allBels[i].kmeansCondensationN(6)
#3. recombine beliefs
newBelief = GM()
for g in allBels:
g.scalerMultiply(weightSums[allBels.index(g)])
newBelief.addGM(g)
newBelief.normalizeWeights()
#4. fix cops position in belief
for g in newBelief:
g.mean = [copPoses[0][0], copPoses[0][1], g.mean[2], g.mean[3]]
g.var[0][0] = 0.1
g.var[0][1] = 0
g.var[1][0] = 0
g.var[1][1] = 0.1
#5. add uncertainty for robber position
for g in newBelief:
g.var[2][2] += 0
g.var[3][3] += 0
# newBelief.normalizeWeights();
if copPoses is not None:
pose = copPoses[len(copPoses) - 1]
print("MAP COP POSE TO PLOT: {}".format(pose))
self.makeBeliefMap(newBelief, pose)
return newBelief
def beliefUpdate(self, belief, responses=None, copPoses=None):
# #Create Cop View Cone
# pose = copPoses[-1];
# viewCone = Softmax();
# viewCone.buildTriView(pose,length=1,steepness=10);
# for i in range(0,len(viewCone.weights)):
# viewCone.weights[i] = [0,0,viewCone.weights[i][0],viewCone.weights[i][1]];
#Update Cop View Cone
# newerBelief = GM();
# for j in range(1,5):
# tmpBel = viewCone.runVBND(belief,j);
# if(j==1):
# tmpBel.scalerMultiply(.4);
# newerBelief.addGM(tmpBel);
#Dont Update Cop View Cone
#newerBelief = belief
#4. update cops position to current position
for g in belief:
g.mean = [copPoses[-1][0], copPoses[-1][1], g.mean[2], g.mean[3]]
g.var[0][0] = 0.1
g.var[0][1] = 0
g.var[1][0] = 0
g.var[1][1] = 0.1
#5. update belief with robber dynamics
for g in belief:
g.var[2][2] += 0.03
g.var[3][3] += 0.03
#Distance Cutoff
#How many standard deviations away from the cop should gaussians be updated with view cone?
distCut = 2
#Update Cop View Cone Using LWIS
newerBelief = GM()
for pose in copPoses:
#Create Cop View Cone
#pose = copPoses[-1];
viewCone = Softmax()
viewCone.buildTriView(pose, length=1, steepness=10)
for i in range(0, len(viewCone.weights)):
viewCone.weights[i] = [
0, 0, viewCone.weights[i][0], viewCone.weights[i][1]
]
for g in belief:
#If the gaussian is suffciently close to the pose
#based on mahalanobis distance.
#Logic: M-dist basically says how many standard devs away the point is from the mean
#If it's more than distCut, it should be left alone
gprime = Gaussian()
gprime.mean = [g.mean[2], g.mean[3]]
gprime.var = [[g.var[2][2], g.var[2][3]],
[g.var[3][2], g.var[3][3]]]
gprime.weight = g.weight
#print(gprime.mean,gprime.mahalanobisDistance([pose[0]-np.cos(pose[2])*.5,pose[1]-np.sin(pose[2])*.5]));
if (gprime.mahalanobisDistance([
pose[0] - np.cos(pose[2]) * .5,
pose[1] - np.sin(pose[2]) * .5
]) <= distCut):
newG = viewCone.lwisUpdate(g, 0, 500, inverse=True)
newerBelief.addG(newG)
else:
newerBelief.addG(g)
#after each bayes update for the view cone, re-normalize
#Just to be sure, it never hurts to check
newerBelief.normalizeWeights()
# newerBelief= belief
#Update From Responses
if (responses is not None):
for res in responses:
roomNum = res[0]
mod = res[1]
clas = res[2]
sign = res[3]
if (roomNum == 0):
#apply to all
if (sign == True):
newerBelief = mod.runVBND(newerBelief, 0)
else:
tmp = GM()
for j in range(1, mod.size):
tmp.addGM(mod.runVBND(newerBelief, j))
newerBelief = tmp
else:
print('ROOM NUM: {}'.format(roomNum))
#apply to all rooms
if (sign == True):
newerBelief = mod.runVBND(newerBelief, clas)
else:
tmp = GM()
for j in range(1, mod.size):
if (j != clas):
tmp.addGM(mod.runVBND(newerBelief, j))
newerBelief = tmp
#Each response recieves a full bayes update, so we need to normalize each time
newerBelief.normalizeWeights()
#Condense the belief
newerBelief.condense(15)
print("*********************")
print(newerBelief.size)
print("*********************")
#Make sure there is a belief in each room
#A bit of a hack, but if this isn't here the lower level query fails
# for room in self.map_.rooms:
# centx = (self.map_.rooms[room]['max_x'] + self.map_.rooms[room]['min_x'])/2;
# centy = (self.map_.rooms[room]['max_y'] + self.map_.rooms[room]['min_y'])/2;
# var = np.identity(4).tolist();
# newerBelief.addG(Gaussian([0,0,centx,centy],var,0.00001));
#3. recombine beliefs (if we're still doing that sort of thing)
newBelief = newerBelief
newBelief.normalizeWeights()
#Moved to before observation updates
# #4. update cops position to current position
# for g in newBelief:
# g.mean = [copPoses[-1][0],copPoses[-1][1],g.mean[2],g.mean[3]];
# g.var[0][0] = 0.1;
# g.var[0][1] = 0;
# g.var[1][0] = 0;
# g.var[1][1] = 0.1;
# #5. update belief with robber dynamics
# for g in newBelief:
# g.var[2][2] += 0.05;
# g.var[3][3] += 0.05;
print("*********************")
print(newBelief.size)
print("*********************")
return newBelief
