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() '''