def stateLWISUpdate(self):
cp=self.prevPoses[-1];
prev = self.prevPoses[-2];
theta = np.arctan2([cp[1]-prev[1]],[cp[0]-prev[0]]);
#print(theta);
radius = self.ROBOT_VIEW_RADIUS;
points = [[cp[0]-radius,cp[1]-radius],[cp[0]+radius,cp[1]-radius],[cp[0]+radius,cp[1]+radius],[cp[0]-radius,cp[1]+radius]];
soft = Softmax()
soft.buildPointsModel(points,steepness=1);
#soft.buildTriView(pose = [cp[0],cp[1],theta],length=10,steepness=5);
change = False;
post = GM();
for g in self.belief:
if(distance(cp,g.mean) > self.ROBOT_VIEW_RADIUS+5):
post.addG(g);
else:
change = True;
tmp = soft.lwisUpdate(g,0,20,inverse=True);
#self.bounds = {'low':[0,0],'high':[437,754]}
tmp.mean[0] = max(self.bounds['low'][0]+1,tmp.mean[0]);
tmp.mean[1] = max(self.bounds['low'][1]+1,tmp.mean[1]);
tmp.mean[0] = min(self.bounds['high'][0]-1,tmp.mean[0]);
tmp.mean[1] = min(self.bounds['high'][1]-1,tmp.mean[1]);
post.addG(tmp);
self.belief = post;
self.belief.normalizeWeights();
return change;
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
