def testMakeNear():
pzIn = Softmax()
pzOut = Softmax()
cent = [4, 4]
orient = 0
nearness = 2
lengthIn = 3
lengthOut = lengthIn + nearness
widthIn = 2
widthOut = widthIn + nearness
pzIn.buildOrientedRecModel(cent, orient, lengthIn, widthIn, steepness=10)
pzOut.buildOrientedRecModel(cent,
orient,
lengthOut,
widthOut,
steepness=10)
#pzIn.plot2D(low=[0,0],high=[10,10]);
#pzOut.plot2D(low=[0,0],high=[10,10]);
b = GM()
for i in range(0, 10):
for j in range(0, 10):
b.addG(Gaussian([i, j], [[1, 0], [0, 1]], 1))
b.normalizeWeights()
b1 = GM()
for i in range(1, 5):
b1.addGM(pzIn.runVBND(b, i))
b1.normalizeWeights()
b2 = GM()
b2.addGM(pzOut.runVBND(b1, 0))
b2.normalizeWeights()
fig, axarr = plt.subplots(3)
[x, y, c] = b.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[0].contourf(x, y, c)
[x, y, c] = b1.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[1].contourf(x, y, c)
[x, y, c] = b2.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[2].contourf(x, y, c)
plt.show()
def testMakeNear(): pzIn = Softmax(); pzOut = Softmax(); cent = [3.5,3.5]; orient = 0; nearness = 2; lengthIn = 3; lengthOut = lengthIn+nearness; widthIn = 2; widthOut = widthIn+nearness; pzIn.buildOrientedRecModel(cent,orient,lengthIn,widthIn,steepness=10); pzOut.buildOrientedRecModel(cent,orient,lengthOut,widthOut,steepness=10); #pzIn.plot2D(low=[0,0],high=[10,10]); #pzOut.plot2D(low=[0,0],high=[10,10]); b = GM(); for i in range(0,11): for j in range(0,11): b.addG(Gaussian([i,j],[[1,0],[0,1]],1)); b.normalizeWeights(); b1 = GM(); for i in range(1,5): b1.addGM(pzIn.runVBND(b,i)); b1.normalizeWeights(); b2 = GM(); b2.addGM(pzOut.runVBND(b1,0)); b2.normalizeWeights(); fig,axarr = plt.subplots(3); [x,y,c] = b.plot2D(low=[0,0],high=[10,10],vis=False); axarr[0].contourf(x,y,c); [x,y,c] = b1.plot2D(low=[0,0],high=[10,10],vis=False); axarr[1].contourf(x,y,c); [x,y,c] = b2.plot2D(low=[0,0],high=[10,10],vis=False); axarr[2].contourf(x,y,c); plt.show();
def testLWIS():
pz = Softmax()
pose = [0, 0, 0]
pz.buildTriView(pose, length=2, steepness=10)
prior = GM()
#prior.addG(Gaussian([1,0],[[1,0],[0,1]],1));
for i in range(0, 100):
prior.addG(
Gaussian([np.random.random() * 4 - 2,
np.random.random() * 4 - 2], [[0.1, 0], [0, 0.1]], 1))
prior.normalizeWeights()
post = GM()
for g in prior:
post.addG(pz.lwisUpdate(g, 0, 500, inverse=True))
#post.display();
[x1, y1, c1] = prior.plot2D(low=[-5, -5], high=[5, 5], vis=False)
[x3, y3, c3] = pz.plot2D(low=[-5, -5], high=[5, 5], vis=False)
[x2, y2, c2] = post.plot2D(low=[-5, -5], high=[5, 5], vis=False)
diffs = c2 - c1
print(np.amax(c2))
print(np.amax(diffs))
print(np.amin(diffs))
fig, axarr = plt.subplots(4)
axarr[0].contourf(x1, y1, c1)
axarr[0].set_title('Prior')
axarr[1].contourf(x3, y3, c3)
axarr[1].set_title('Likelihood')
axarr[2].contourf(x2, y2, c2)
axarr[2].set_title('Posterior')
axarr[3].contourf(x2, y2, diffs)
axarr[3].set_title('Diffs')
plt.show()
def makeInitialBelief():
bel = GM()
numMix = 100
varscale = .2
for i in range(0, numMix):
w = np.random.random()
mean = [np.random.random() * 8,
np.random.random() * 8]
tmp = np.random.random() * varscale
var = [[np.random.random() * 1 + varscale, 0],
[0, np.random.random() * 1 + varscale]]
bel.addG(Gaussian(mean, var, w))
bel.addG(Gaussian([4, 7], [[1, 0], [0, 1]], 4))
bel.normalizeWeights()
[x, y, belView] = bel.plot2D(low=[0, 0], high=[10, 10], vis=False)
plt.contourf(x, y, belView)
plt.savefig('../img/testBel.png')
plt.cla()
plt.clf()
plt.close()
return bel
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 runFactoredSim(self):
numSteps = 50
initialPose = [8,2];
b = GM();
'''
mean = [0]*len(self.delA[0]);
var = [[0 for k in range(0,len(self.delA[0]))] for j in range(0,len(self.delA[0]))];
for k in range(0,len(self.delA[0])):
mean[k] = random.random()*(self.bounds[k][1]-self.bounds[k][0]) + self.bounds[k][0];
var[k][k] = random.random()*10;
b.addG(Gaussian(mean,var,0.5));
'''
b.addG(Gaussian([6,3],[[4,0],[0,4]],6));
b.addG(Gaussian([.8,3.2],[[1,0],[0,1]],1));
b.addG(Gaussian([3,4.5],[[1,0],[0,1]],1));
b.addG(Gaussian([3,1.5],[[1,0],[0,1]],1));
b.normalizeWeights();
#Setup data gathering
x = initialPose;
allX = [];
allX.append(x);
allXInd = [0]*len(self.delA[0]);
for i in range(0,len(self.delA[0])):
allXInd[i] = [x[i]];
reward = 0;
allReward = [0];
allB = [];
allB.append(b);
allAct = [];
fig,ax = plt.subplots();
#Simulate
for count in range(0,numSteps):
if(self.exitFlag):
break;
if(self.distance(1,3,x[0],x[1]) < 1):
print('Robber Found');
break;
plt.cla();
[xxx,yyy,ccc] = b.plot2D(low=[0,0],high=[10,5],vis=False);
plt.gca().add_patch(Rectangle([2,2],1,2,fc='sandybrown'));
plt.gca().add_patch(Circle([1,3],0.15,fc='r'));
ax.contourf(xxx,yyy,ccc);
'''
#Get action
if(greedy):
act = self.getGreedyAction(b);
elif(belGen):
act = random.randint(0,len(self.delA)-1);
else:
act = self.getAction(b);
'''
act = self.getAction(b);
#Take action
x = np.random.multivariate_normal(np.array(x)-np.array(self.delA[act[0]]),self.delAVar,size =1)[0].tolist();
#bound the movement
for i in range(0,len(x)):
x[i] = max(self.bounds[i][0],x[i]);
x[i] = min(self.bounds[i][1],x[i]);
'''
#Get observation and update belief
if(not self.useSoft):
ztrial = [0]*len(self.pz);
for i in range(0,len(self.pz)):
ztrial[i] = self.pz[i].pointEval(x);
z = ztrial.index(max(ztrial));
b = self.beliefUpdate(b,act,z);
else:
ztrial = [0]*self.pz.size;
for i in range(0,self.pz.size):
ztrial[i] = self.pz.pointEval2D(i,x);
z = ztrial.index(max(ztrial));
b = self.beliefUpdateSoftmax(b,act,z);
'''
#fig,axarr = plt.subplots(2);
#[x,y,c] = b.plot2D(low=[0,0],high=[10,5],)
'''
if(self.distance(1,3,x[0],x[1]) > 1):
for i in range(1,4):
b = self.pz2.runVBND(b,i);
b = self.beliefUpdateSoftmax(b,act[0],4);
else:
'''
b = self.beliefUpdateSoftmax(b,act[0],2);
#Handle Question response
questionHandles = ['left of','right of', 'in front of', 'behind'];
question = 'Am I ' + questionHandles[act[1]] + ' the table?';
ax.scatter(x[0],x[1],c='k');
plt.pause(0.5);
humanInput = raw_input(question);
if('y' in humanInput or 'Y' in humanInput):
b = self.pz.runVBND(b,act[1]+1);
else:
for i in range(0,4):
if(i!=act[1]):
b = self.pz.runVBND(b,i+1);
b.normalizeWeights();
#save data
allB.append(b);
allX.append(x);
allAct.append(act);
for i in range(0,len(x)):
allXInd[i].append(x[i]);
reward += self.r[act[0]].pointEval(x);
allReward.append(reward);
allAct.append(-1);
#print("Simulation Complete. Accumulated Reward: " + str(reward));
return [allB,allX,allXInd,allAct,allReward];
def test2DSoftmax():
#Specify Parameters
#2 1D robots obs model
#weight = [[0.6963,-0.6963],[-0.6963,0.6963],[0,0]];
#bias = [-0.3541,-0.3541,0];
#Colinear Problem
weight = [[-1.3926, 1.3926], [-0.6963, 0.6963], [0, 0]]
bias = [0, .1741, 0]
low = [0, 0]
high = [5, 5]
#Differencing Problem
#weight = [[0,1],[-1,1],[1,1],[0,2],[0,0]]
#bias = [1,0,0,0,0];
# low = [-5,-5];
# high = [5,5];
MMS = True
softClass = 2
detect = 0
res = 100
steep = 2
for i in range(0, len(weight)):
for j in range(0, len(weight[i])):
weight[i][j] = weight[i][j] * steep
bias[i] = bias[i] * steep
#Define Likelihood Model
a = Softmax(weight, bias)
[x1, y1, dom] = a.plot2D(low=low, high=high, res=res, vis=False)
a.plot2D(low=low, high=high, res=res, vis=True)
#Define a prior
prior = GM()
prior.addG(Gaussian([2, 4], [[1, 0], [0, 1]], 1))
prior.addG(Gaussian([4, 2], [[1, 0], [0, 1]], 1))
prior.addG(Gaussian([1, 3], [[1, 0], [0, 1]], 1))
[x2, y2, c2] = prior.plot2D(low=low, high=high, res=res, vis=False)
if (MMS):
#run Variational Bayes
if (detect == 0):
post1 = a.runVBND(prior, 0)
post2 = a.runVBND(prior, 2)
post1.addGM(post2)
else:
post1 = a.runVBND(prior, 1)
else:
post1 = a.runVBND(prior, softClass)
post1.normalizeWeights()
[x3, y3, c3] = post1.plot2D(low=low, high=high, res=res, vis=False)
post1.display()
softClassLabels = ['Near', 'Left', 'Right', 'Up', 'Down']
detectLabels = ['No Detection', 'Detection']
#plot everything together
fig, axarr = plt.subplots(3, sharex=True, sharey=True)
axarr[0].contourf(x2, y2, c2, cmap='viridis')
axarr[0].set_title('Prior GM')
axarr[1].contourf(x1, y1, dom, cmap='viridis')
axarr[1].set_title('Likelihood Softmax')
axarr[2].contourf(x3, y3, c3, cmap='viridis')
if (MMS):
axarr[2].set_title('Posterior GM with observation:' +
detectLabels[detect])
else:
axarr[2].set_title('Posterior GM with observation:' +
softClassLabels[softClass])
fig.suptitle('2D Fusion of a Gaussian Prior with a Softmax Likelihood')
plt.show()
self.pub.publish(msg)
if __name__ == "__main__":
prior = GM([[-9, 3],
[-8, 3],
[-7, 3.5]
],
[[[1.5, 1.0],
[1.0, 1.5]],
[[0.5, -0.3],
[-0.3, 0.5]],
[[2.5, -0.3],
[-0.3, 2.5]]],
[0.2, 0.6, 0.2]
)
bounds = [-9.6, -3.6, 4, 3.6]
delta = 0.1
prior.plot2D(low=bounds[0:2],high=bounds[2:4])
q = Questioner(human_sensor=None, target_order=['Pris','Roy'],
target_weights=[11., 10.],bounds=bounds,delta=delta)
q.weigh_questions({'Roy':prior})
rospy.sleep(3)
# for qu in q.weighted_questions:
# print qu
q.transmit_questions()
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()
'''
