def testInvertedSoftmaxModels():
b = GM()
b.addG(Gaussian([2, 2], [[1, 0], [0, 1]], 1))
b.addG(Gaussian([4, 2], [[1, 0], [0, 1]], 1))
b.addG(Gaussian([2, 4], [[1, 0], [0, 1]], 1))
b.addG(Gaussian([3, 3], [[1, 0], [0, 1]], 1))
b.normalizeWeights()
b.plot2D()
pz = Softmax()
pz.buildOrientedRecModel([2, 2], 0, 1, 1, 5)
#pz.plot2D();
startTime = time.clock()
b2 = GM()
for i in range(1, 5):
b2.addGM(pz.runVBND(b, i))
print(time.clock() - startTime)
b2.plot2D()
startTime = time.clock()
b3 = GM()
b3.addGM(b)
tmpB = pz.runVBND(b, 0)
tmpB.normalizeWeights()
tmpB.scalerMultiply(-1)
b3.addGM(tmpB)
tmpBWeights = b3.getWeights()
mi = min(b3.getWeights())
#print(mi);
for g in b3.Gs:
g.weight = g.weight - mi
b3.normalizeWeights()
print(time.clock() - startTime)
#b3.display();
b3.plot2D()
def stateObsUpdate(self,name,relation,pos="Is"): if(name == 'You'): #Take Cops Position, builid box around it cp=self.copPose; points = [[cp[0]-5,cp[1]-5],[cp[0]+5,cp[1]-5],[cp[0]+5,cp[1]+5],[cp[0]-5,cp[1]+5]]; soft = Softmax() soft.buildPointsModel(points,steepness=3); else: soft = self.sketches[name]; softClass = self.spatialRealtions[relation]; if(pos=="Is"): self.belief = soft.runVBND(self.belief,softClass); self.belief.normalizeWeights(); else: tmp = GM(); for i in range(0,5): if(i!=softClass): tmp.addGM(soft.runVBND(self.belief,i)); tmp.normalizeWeights(); self.belief=tmp; if(self.belief.size > self.MAX_BELIEF_SIZE): self.belief.condense(self.MAX_BELIEF_SIZE); self.belief.normalizeWeights()
#Get N Vertices of the shape
cHull = ConvexHull(pairedPoints)
vertices = fitSimplePolyToHull(cHull, pairedPoints, N=5)
#vertices = fitBestPolyToHull(cHull,pairedPoints);
#Show Vertices
plt.scatter([vertices[i][0] for i in range(0, len(vertices))],
[vertices[i][1] for i in range(0, len(vertices))])
plt.show()
#Make softmax model
pz = Softmax()
pz.buildPointsModel(vertices, steepness=5)
#Update belief
post = pz.runVBND(prior, 4)
#display
fig, axarr = plt.subplots(3)
[xprior, yprior, cprior] = prior.plot2D(low=[0, 0],
high=[10, 10],
vis=False)
[xobs, yobs, cobs] = pz.plot2D(low=[0, 0],
high=[10, 10],
delta=0.1,
vis=False)
[xpost, ypost, cpost] = post.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[0].contourf(xprior, yprior, cprior, cmap='viridis')
axarr[1].contourf(xobs, yobs, cobs, cmap='inferno')
axarr[2].contourf(xpost, ypost, cpost, cmap='viridis')
plt.show()
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 buildRadialSoftmaxModels():
dims = 2
#Target Model
steep = 2
weight = (np.array([-2, -1, 0]) * steep).tolist()
bias = (np.array([6, 4, 0]) * steep).tolist()
for i in range(0, len(weight)):
weight[i] = [weight[i], 0]
pz = Softmax(weight, bias)
obsOffset = [-7, -4]
observation = 2
#pz.plot2D(low=[0,0],high=[1,6.28]);
'''
H = np.matrix([[2,math.pi*2,1],[2,math.pi*3/4,1],[2,math.pi/2,1]]);
print(nullspace(H));
#Modified Target Model
#def buildGeneralModel(self,dims,numClasses,boundries,B,steepness=1):
B = np.matrix([0.447,0,-0.8944,0.447,0,-0.894]).T;
boundries = [[1,0],[2,0]];
pz = Softmax();
pz.buildGeneralModel(2,3,boundries,B,steepness=2);
'''
'''
cent = [0,0];
length = 3;
width = 2;
orient = 0;
pz = Softmax();
pz.buildOrientedRecModel(cent,orient,length,width,steepness=5);
'''
print('Plotting Observation Model')
[xobs, yobs, domObs] = plot2DPolar(pz,
low=[-10, -10],
high=[10, 10],
delta=0.1,
offset=obsOffset,
vis=False)
[xobsPol, yobsPol, domObsPol] = pz.plot2D(low=[0, -3.14],
high=[10, 3.14],
delta=0.1,
vis=False)
# fig = plt.figure()
# ax = fig.gca(projection='3d');
# colors = ['b','g','r','c','m','y','k','w','b','g'];
# for i in range(0,len(model)):
# ax.plot_surface(x,y,model[i],color = colors[i]);
# plt.show();
#pz.plot2D(low=[-10,-10],high=[10,10]);
scaling = o1
bcart = GM()
for i in range(-10, 11):
for j in range(-10, 11):
# if(i != 0 or j != 0):
bcart.addG(Gaussian([i, j], [[scaling, 0], [0, scaling]], 1))
bcart.normalizeWeights()
[xpri, ypri, cpri] = bcart.plot2D(low=[-10, -10], high=[10, 10], vis=False)
bpol = transformCartToPol(bcart, obsOffset)
for i in range(0, 3):
bpolPrime = pz.runVBND(bpol, i)
bcartPrime = transformPolToCart(bpolPrime, obsOffset)
bcartPrime.normalizeWeights()
[xpos, ypos, cpos] = bcartPrime.plot2D(low=[-10, -10],
high=[10, 10],
vis=False)
fig, axarr = plt.subplots(3)
axarr[0].contourf(xpri, ypri, cpri)
axarr[0].set_ylabel("Prior")
axarr[1].contourf(xobs, yobs, domObs)
axarr[1].set_ylabel("Observation: " + str(i))
axarr[2].contourf(xpos, ypos, cpos)
axarr[2].set_ylabel("Posterior")
plt.show()
fig, axarr = plt.subplots(1, 2)
axarr[0].contourf(xobs, yobs, domObs)
axarr[1].contourf(xobsPol, yobsPol, domObsPol)
axarr[0].set_title("Cartesian Observations")
axarr[1].set_title("Polar Observations")
axarr[0].set_xlabel("X")
axarr[0].set_ylabel("Y")
axarr[1].set_xlabel("Radius (r)")
axarr[1].set_ylabel("Angle (theta)")
plt.show()
bTestCart = GM()
bTestCart.addG(Gaussian([1, 2], [[1, 0], [0, 1]], .25))
bTestCart.addG(Gaussian([-3, 1], [[3, 0], [0, 1]], .25))
bTestCart.addG(Gaussian([1, -4], [[1, 0], [0, 2]], .25))
bTestCart.addG(Gaussian([-3, -3], [[2, 1.2], [1.2, 2]], .25))
bTestPol = transformCartToPol(bTestCart, [0, 0])
[xTestCart, yTestCart, cTestCart] = bTestCart.plot2D(low=[-10, -10],
high=[10, 10],
vis=False)
[xTestPol, yTestPol, cTestPol] = bTestPol.plot2D(low=[0, -3.14],
high=[10, 3.14],
vis=False)
fig, axarr = plt.subplots(1, 2)
axarr[0].contourf(xTestCart, yTestCart, cTestCart)
axarr[1].contourf(xTestPol, yTestPol, cTestPol)
axarr[0].set_title("Cartesian Gaussians")
axarr[1].set_title("Polar Gaussians")
axarr[0].set_xlabel("X")
axarr[0].set_ylabel("Y")
axarr[1].set_xlabel("Radius (r)")
axarr[1].set_ylabel("Angle (theta)")
plt.show()
'''
def buildRectangleModel():
#Specify the lower left and upper right points
recBounds = [[2, 2], [3, 4]]
#recBounds = [[1,1],[8,4]];
B = np.matrix([
-1, 0, recBounds[0][0], 1, 0, -recBounds[1][0], 0, 1, -recBounds[1][1],
0, -1, recBounds[0][1]
]).T
M = np.zeros(shape=(12, 15))
#Boundry: Left|Near
rowSB = 0
classNum1 = 1
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
#Boundry: Right|Near
rowSB = 1
classNum1 = 2
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
#Boundry: Up|Near
rowSB = 2
classNum1 = 3
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
#Boundry: Down|Near
rowSB = 3
classNum1 = 4
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
A = np.hstack((M, B))
#print(np.linalg.matrix_rank(A))
#print(np.linalg.matrix_rank(M))
Theta = linalg.lstsq(M, B)[0].tolist()
weight = []
bias = []
for i in range(0, len(Theta) // 3):
weight.append([Theta[3 * i][0], Theta[3 * i + 1][0]])
bias.append(Theta[3 * i + 2][0])
steep = 1
weight = (np.array(weight) * steep).tolist()
bias = (np.array(bias) * steep).tolist()
pz = Softmax(weight, bias)
#print('Plotting Observation Model');
#pz.plot2D(low=[0,0],high=[10,5],vis=True);
prior = GM()
for i in range(0, 10):
for j in range(0, 5):
prior.addG(Gaussian([i, j], [[1, 0], [0, 1]], 1))
# prior.addG(Gaussian([4,3],[[1,0],[0,1]],1));
# prior.addG(Gaussian([7,2],[[4,1],[1,4]],3))
prior.normalizeWeights()
dela = 0.1
x, y = np.mgrid[0:10:dela, 0:5:dela]
fig, axarr = plt.subplots(6)
axarr[0].contourf(x, y,
prior.discretize2D(low=[0, 0], high=[10, 5], delta=dela))
axarr[0].set_title('Prior')
titles = ['Inside', 'Left', 'Right', 'Up', 'Down']
for i in range(0, 5):
post = pz.runVBND(prior, i)
c = post.discretize2D(low=[0, 0], high=[10, 5], delta=dela)
axarr[i + 1].contourf(x, y, c, cmap='viridis')
axarr[i + 1].set_title('Post: ' + titles[i])
plt.show()
