def showPredictions(self,g):
''' show predictions '''
result=g.mu
#d=self.getView().getDrawing()
# predictions
self.lanmarkFigures=[]
self.predictionFigures=pyHCompositeFigure()
d=self.predictionFigures
i=0
x=result.value[2*i][0]
y=result.value[2*i+1][0]
fs=pyHLandMarkPredictionFigure(x-0.5,y-0.5,5,5,i)
fs.setColor(255,0,0)
d.addFigure(fs)
# motion prediction
for i in range(1,N):
x=result.value[2*i][0]
y=result.value[2*i+1][0]
fe=pyHNodePosePredictionFigure(x-0.5,y-0.5,5,5,i)
fe.setColor(255,0,0)
d.addFigure(fe)
cf=pyHConnectionFigure()
cf.setColor(255,0,0,100)
cf.connectFigures(fs, fe)
d.addFigure(cf)
fs=fe
# landmark prediction
for i in range(num_landmarks):
x=result.value[2*(N+i)][0]
y=result.value[2*(N+i)+1][0]
f=pyHLandMarkPredictionFigure(x-0.5,y-0.5,2,2,i)
f.setColor(0,0,255)
d.addFigure(f)
self.getView().getDrawing().addFigure(d)
def onMouseUp(self, e):
ex = e.getX()
ey = e.getY()
d = self.view.getDrawing()
d.removeFigure(self.f)
try:
nf = self.view.findFigure(pyHPoint(ex, ey))
if isinstance(nf, pyHNodeLandMarkFigure):
cnf = nf.getDisplayBox().getCenterPoint()
cow = self.owner.getDisplayBox().getCenterPoint()
pf = cnf * 0.5 + cow * 0.5
lmmf = pyHNodeLandMarkMeasureFigure(pf.getX() - 1,
pf.getY() - 1, 2, 2, "?")
d.addFigure(lmmf)
cf = pyHConnectionFigure()
cf.setColor(0, 0, 255, 100)
cf.connectFigures(self.owner, lmmf)
d.addFigure(cf)
cf = pyHConnectionFigure()
cf.setColor(100, 0, 0, 100)
cf.connectFigures(lmmf, nf)
d.addFigure(cf)
lmmf.setText(nf.getText())
d.addFigure(cf)
if isinstance(nf, pyHNodePoseFigure):
cnf = nf.getDisplayBox().getCenterPoint()
cow = self.owner.getDisplayBox().getCenterPoint()
pf = cnf * 0.5 + cow * 0.5
npf = pyHNodePoseFigure(pf.getX() - 1,
pf.getY() - 1, 2, 2, "?")
d.addFigure(npf)
cf = pyHConnectionFigure()
cf.setColor(0, 0, 255, 100)
cf.connectFigures(self.owner, npf)
d.addFigure(cf)
cf = pyHConnectionFigure()
cf.setColor(100, 0, 0, 100)
cf.connectFigures(npf, nf)
d.addFigure(cf)
npf.setText(nf.getText())
d.addFigure(cf)
except pyHFigureNotFound:
print "No figure found"
def __init__(self):
super(pyHGraphSLAMEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
self.getView().setDrawingGrid(False)
pyImg=pyHImage("itc_corridors_2020030301.jpg")
#pyImg=pyHImage("../images/20200923_095151.jpg")
bf=pyHImageFigure(0,0,160*2,140,pyImg)
self.getView().setBackground(bf)
# Init data
self.data,self.r = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
data=self.data
r=self.r
result,Omega,Xi,g = slam(data, N, num_landmarks, motion_noise, measurement_noise)
print_result(N, num_landmarks, result)
d=self.getView().getDrawing()
# ''' build information matrix figure '''
imf=pyHInformationMatrixFigure(g)
d.addFigure(imf)
txt=pyHTextFigure(45,100,20,3.5," pyHotGraphSLAM ",border=True)
d.addFigure(txt)
''' s h o w p r e d i c t i o n s '''
self.showPredictions(g)
''' show last groundtruth pose '''
rf=pyHStarFigure(r.x,r.y,0.5,1.5,5)
rf.setColor(0,255,0)
rf.setWidth(4)
d.addFigure(rf)
''' s h o w landmarks groundtruth '''
''' Just the map '''
landmarks=[]
for i,(x,y) in enumerate(self.r.landmarks):
#x=result.value[2*(N+i)][0]
#y=result.value[2*(N+i)+1][0]
f=pyHNodeLandMarkFigure(x-0.5,y-0.5,4,4,i)
f.setColor(100,100,100)
f.setFillColor(255, 255, 230,100)
d.addFigure(f)
landmarks.append(f)
''' s h o w g r a p h '''
x=world_size/2
y=world_size/2
fs=pyHNodePoseFigure(x-0.5,y-0.5,5,5,0)
d.addFigure(fs)
for k in range(len(data)):
# n is the index of the robot pose in the matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
idx=measurement[i][0]
mdx=measurement[i][1]
mdy=measurement[i][2]
mx=x+mdx
my=y+mdy
lmf=pyHNodeLandMarkMeasureFigure(mx-1,my-1,2,2,idx)
d.addFigure(lmf)
cf=pyHConnectionFigure()
cf.setColor(0,0,255,100)
cf.connectFigures(fs, lmf)
d.addFigure(cf)
f=landmarks[idx] #pyHNodeLandMarkFigure(mx-0.25,my-0.25,2.0,2.0,idx)
cf=pyHConnectionFigure()
cf.setColor(100,0,0,100)
cf.connectFigures(lmf, f)
d.addFigure(cf)
#d.addFigure(plm)
pid=motion[0]
dx =motion[1]
dy =motion[2]
x+=dx
y+=dy
nmf=pyHNodePoseMeasureFigure(x-1,y-1,2,2,pid)
d.addFigure(nmf)
cf=pyHConnectionFigure()
cf.setColor(0,100,0,100)
cf.connectFigures(fs, nmf)
d.addFigure(cf)
#print x,y,dx,dy
fe=pyHNodePoseFigure(x-2.5,y-2.5,5,5,k+1)
d.addFigure(fe)
cf=pyHConnectionFigure()
cf.setColor(100,0,0,100)
cf.connectFigures(nmf, fe)
d.addFigure(cf)
fs=fe
self.getView().setTransformFitToDrawing()
def creatingLineImageFilterConnection(self):
self.setCurrentTool(pyHConnectionImageFilterTool(self.getView(),pyHConnectionFigure()))
def creatingLineConnection(self):
self.setCurrentTool(pyHConnectionTool(self.getView(),pyHConnectionFigure()))
def creatingLineImageFilterConnection(self):
self.setCurrentTool(
pyHConnectionImageFilterTool(self.getView(),
pyHConnectionFigure()))
def creatingLineConnection(self):
self.setCurrentTool(
pyHConnectionTool(self.getView(), pyHConnectionFigure()))
def __init__(self):
super(pyHGraphSLAMEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
# Init data
self.data,self.r = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
data=self.data
r=self.r
result,Omega,Xi = slam(data, N, num_landmarks, motion_noise, measurement_noise)
print_result(N, num_landmarks, result)
d=self.getView().getDrawing()
#Units metres
cellSize=3
gf=pyHGridFigure(100,28*cellSize,1,14,cellSize*2,cellSize*2)
for i in range(14):
if i>9:
tf=pyHTextFigure(0,0,cellSize*2,cellSize*2,i-10)
tf.setColor(0,0,255)
else:
tf=pyHTextFigure(0,0,cellSize*2,cellSize*2,i)
gf.addFigure(tf)
d.addFigure(gf)
OmegaFigure=pyHGridFigure(100,0,28,28,cellSize,cellSize)
XiFigure=pyHGridFigure(100+28*cellSize,0,28,1,cellSize*2,cellSize)
d.addFigure(XiFigure)
for i in range(28):
if abs(Xi.value[i][0])>0.000001:
tf=pyHTextFigure(0,0,cellSize*2,cellSize,Xi.value[i][0])
else:
tf=pyHTextFigure(0,0,cellSize*2,cellSize," ")
XiFigure.addFigure(tf)
for i in range(28):
for j in range(28):
if abs(Omega.value[i][j])>0.000001:
tf=pyHTextFigure(0,0,cellSize,cellSize,Omega.value[i][j])
else:
tf=pyHTextFigure(0,0,cellSize,cellSize," ")
if i>19 or j>19:
tf.setColor(0,0,255)
OmegaFigure.addFigure(tf)
d.addFigure(OmegaFigure)
txt=pyHTextFigure(45,100,20,3.5," pyHotGraphSLAM ",border=True)
d.addFigure(txt)
rf=pyHStarFigure(r.x,r.y,0.5,1.5,5)
rf.setColor(0,255,0)
d.addFigure(rf)
x=world_size/2
y=world_size/2
fs=pyHMotionNodeFigure(x-0.5,y-0.5,2,2,0)
d.addFigure(fs)
for k in range(len(data)):
# n is the index of the robot pose in the matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
idx=measurement[i][0]
mdx=measurement[i][1]
mdy=measurement[i][2]
mx=x+mdx
my=y+mdy
f=pyHDiamondFigure(mx-0.25,my-0.25,2.0,2.0)
f.setColor(100,100,100,100)
d.addFigure(f)
cf=pyHConnectionFigure()
cf.setColor(0,255,0,100)
cf.connectFigures(fs, f)
d.addFigure(cf)
#d.addFigure(plm)
pid=motion[0]
dx =motion[1]
dy =motion[2]
x+=dx
y+=dy
#print x,y,dx,dy
fe=pyHMotionNodeFigure(x-0.5,y-0.5,2,2,k+1)
d.addFigure(fe)
cf=pyHConnectionFigure()
cf.setColor(0,100,100)
cf.connectFigures(fs, fe)
d.addFigure(cf)
fs=fe
# predictions
self.lanmarkFigures=[]
i=0
x=result.value[2*i][0]
y=result.value[2*i+1][0]
fs=pyHNodeLandmarkPredictionFigure(x-0.5,y-0.5,1.5,1.5,i)
fs.setColor(255,0,0)
d.addFigure(fs)
for i in range(1,N):
x=result.value[2*i][0]
y=result.value[2*i+1][0]
fe=pyHNodeLandmarkPredictionFigure(x-0.5,y-0.5,1.5,1.5,i)
fe.setColor(255,0,0)
d.addFigure(fe)
cf=pyHConnectionFigure()
cf.setColor(255,0,0,100)
cf.connectFigures(fs, fe)
d.addFigure(cf)
fs=fe
for i in range(num_landmarks):
x=result.value[2*(N+i)][0]
y=result.value[2*(N+i)+1][0]
f=pyHMotionPredictionFigure(x-0.5,y-0.5,1.5,1.5,i)
f.setColor(0,0,255)
d.addFigure(f)
self.getView().setTransformFitToDrawing()
