def __init__(self,x,y,w,h):
super(pyHImageSourceFigure,self).__init__(x,y,w,h)
self.changedImageObservers=[]
self.hImg=pyHImage()
self.hImgPrev=pyHImage()
self.width=w
self.height=h
def __init__(self,x0,y0,w,h,img=None):
super(pyHImageFigure,self).__init__(x0,y0,w,h)
if img!=None:
self.img=img
else:
self.setImage(pyHImage())
self.imageSourceFigure=None
def __init__(self, x0, y0, w, h, img=None):
super(pyHImageFigure, self).__init__(x0, y0, w, h)
if img != None:
self.img = img
else:
self.setImage(pyHImage())
self.imageSourceFigure = None
def __init__(self,x0,y0,w,h,img=None):
super(pyHImageFigure,self).__init__(x0,y0,w,h)
if img is None:
self.setImage(pyHImage())
else:
self.img=img
self.border=True
self.imageSourceFigure=None
def draw(self,g):
#super(pyHImageDottedFigure,self).draw(g)
self.filter.imgcv1=self.img.getData()
self.filter.imgcv2=self.img2.getData()
imgcv=self.filter.process()
hImg=pyHImage()
hImg.setData(imgcv)
g.drawImage(self.x0,self.y0,self.w,self.h,hImg)
def draw(self,g):
#super(pyHImageDottedFigure,self).draw(g)
self.filter.imgcv1=self.img.getData()
self.filter.imgcv2=self.img2.getData()
imgcv=self.filter.process()
hImg=pyHImage()
hImg.setData(imgcv)
g.drawImage(self.x0,self.y0,self.w,self.h,hImg)
def launchFilter(self,hImg1,hImg2):
mat1=hImg1.getData()
mat2=hImg2.getData()
self.imageFilter.imgcv1=mat1
self.imageFilter.imgcv2=mat2
matO=self.imageFilter.process()
hImgO=pyHImage()
hImgO.setData(matO)
return hImgO
def launchFilter(self,hImg1,hImg2):
mat1=hImg1.getData()
mat2=hImg2.getData()
self.imageFilter.imgcv1=mat1
self.imageFilter.imgcv2=mat2
matO=self.imageFilter.process()
hImgO=pyHImage()
hImgO.setData(matO)
return hImgO
def launchFilter(self,hImgI):
#get numpy or cv2 image from pyHImage and make a new copy
matI=hImgI.getData()
#lauch the filter and get theh numpy or cv2 image
matO=self.imageFilter.process(matI)
#build a new pyHImage
hImgO=pyHImage()
#set internal image to this pyHImage
hImgO.setData(matO)
return hImgO
def launchFilter(self,hImgI):
#get numpy or cv2 image from pyHImage and make a new copy
matI=hImgI.getData()
#lauch the filter and get theh numpy or cv2 image
matO=self.imageFilter.process(matI)
#build a new pyHImage
hImgO=pyHImage()
#set internal image to this pyHImage
hImgO.setData(matO)
return hImgO
def set_math_text(self, math_text, font_size=None):
if self.non_math_text:
self.text = r'$\mathrm{\mathsf{' + math_text + '}}$'
else:
if is_float(math_text):
self.text = r'$ {:g} $'.format(float(math_text))
else:
self.text = r'$ {} $'.format(math_text)
if font_size:
self.fontSize = float(font_size)
self.img = pyHImage()
self.updateFigure()
def setText(self, mathTex, fontSize=30):
self.text = r'$' + str(mathTex) + '$'
self.fontSize = float(fontSize)
self.img = pyHImage()
"""
if color:
if len(color) is 3:
r, g, b = color
a = 0
else:
r, g, b, a = color
self.fillColor = (float(r)/255, float(g)/255, float(b)/255, float(a)/255)
"""
self.updateFigure()
def __init__(self,x,y,w,h):
super(pyHImageSourceFigure,self).__init__(x,y,w,h)
self.changedImageObservers=[]
self.hImg=pyHImage()
self.hImgPrev=pyHImage()
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 __init__(self):
super(pyHStandardEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
d = self.getView().getDrawing()
txt = pyHTextFigure(0, 0, 20, 20, "Hola Caracola")
d.addFigure(txt)
#cam1=pyHCameraFigure(0,200,50,50,1)
#d.addFigure(cam1)
imgc1 = pyHImageDottedFigure(150, 250, 320, 240)
imgc1.setPoints([pyHPoint(20, 20), pyHPoint(30, 30)])
d.addFigure(imgc1)
#cam1.addChangedImageObserver(imgc1)
cam = pyHCameraFigure(0, 100, 50, 50, 0)
d.addFigure(cam)
img0 = pyHImageFigure(50, 10, 320, 240)
d.addFigure(img0)
cam.addChangedImageObserver(img0)
img1 = pyHImageFigure(400, 10, 320, 240)
d.addFigure(img1)
fimg = pyHImageFilterFigure(300, 300, 40, 40)
d.addFigure(fimg)
cam.addChangedImageObserver(fimg)
fimg.addChangedImageObserver(img1)
#img2=pyHImageFigure(0,300,320,240)
#d.addFigure(img2)
img3 = pyHImageFigure(400, 10, 320, 240)
d.addFigure(img3)
fimg1 = pyHImageFilterFigure(300, 400, 40, 40)
d.addFigure(fimg1)
fimg1.setFilter(Gaussian())
cam.addChangedImageObserver(fimg1)
fimg1.addChangedImageObserver(img3)
fimgH = pyHImageFilterFigure(400, 500, 30, 30)
d.addFigure(fimgH)
fimgH.setFilter(HistogramColor())
fimg1.addChangedImageObserver(fimgH)
imgH = pyHImageFigure(500, 0, 256, 150)
d.addFigure(imgH)
fimgH.addChangedImageObserver(imgH)
fimg2 = pyHImageFilterFigure(300, 500, 40, 40)
d.addFigure(fimg2)
fimg2.setFilter(SobelX())
fimg1.addChangedImageObserver(fimg2)
img4 = pyHImageFigure(0, 300, 320, 240)
d.addFigure(img4)
fimg2.addChangedImageObserver(img4)
fimg2y = pyHImageFilterFigure(240, 300, 40, 40)
d.addFigure(fimg2y)
fimg2y.setFilter(SobelY())
fimg1.addChangedImageObserver(fimg2y)
img4y = pyHImageFigure(240, 300, 320, 240)
d.addFigure(img4y)
fimg2y.addChangedImageObserver(img4y)
img4 = pyHImageFigure(10, 600, 320, 240)
d.addFigure(img4)
fimg4 = pyHImageFilterFigure(0, 600, 40, 40)
d.addFigure(fimg4)
fimg4.setFilter(OpticalFlow())
cam.addChangedImageObserver(fimg4)
fimg4.addChangedImageObserver(img4)
img5 = pyHImageFigure(400, 600, 320, 240)
d.addFigure(img5)
fimg5 = pyHImageFilterFigure(400, 600, 40, 40)
d.addFigure(fimg5)
fimg5.setFilter(FastFeatureDetector())
cam.addChangedImageObserver(fimg5)
img5.setImageSourceFigure(fimg5)
# #imgCm1=pyHImage('/Users/paco/Pictures/sfm/urjcMostolesMobilLG440-15011301/CAM00295.jpg')
# imgCm1=pyHImage('/media/francisco/Packard Bell/Users/paco/Pictures/sfm/pistaTenis/CAM00698.jpg')
# #imgCm1=pyHImage('../images/im2.png')
# ifcm1=pyHImageDottedFigure(0,0,240,320,imgCm1)
# ifcm1.setPoints([pyHPoint(400,690),pyHPoint(1776,690),pyHPoint(1394,560),pyHPoint(737,560)])
# d.addFigure(ifcm1)
#
# fd=FeatureDetector("SIFT")
# imgCvCm1Fd=fd.process(imgCm1.data)
# #imgCm1=pyHImage('../images/im2.png')
# imgCm1Fd=pyHImage()
# imgCm1Fd.setData(imgCvCm1Fd)
# ifcm1Fd=pyHImageFigure(0,320,240,320,imgCm1Fd)
# d.addFigure(ifcm1Fd)
#
# #imgCm2=pyHImage('/Users/paco/Pictures/sfm/urjcMostolesMobilLG440-15011301/CAM00294.jpg')
# imgCm2=pyHImage('/media/francisco/Packard Bell/Users/paco/Pictures/sfm/pistaTenis/CAM00699.jpg')
# #imgCm1=pyHImage('../images/im2.png')
# ifcm2=pyHImageDottedFigure(240,0,240,320,imgCm2)
# ifcm2.setPoints([pyHPoint(400,690),pyHPoint(1776,690),pyHPoint(1394,560),pyHPoint(737,560)])
# d.addFigure(ifcm2)
#
# imgCvCm2Fd=fd.process(imgCm2.data)
# imgCm2Fd=pyHImage()
# imgCm2Fd.setData(cv2.addWeighted(imgCvCm1Fd,0.5,imgCvCm2Fd,0.5,0))
# ifcm2Fd=pyHImageFigure(0,0,240,320,imgCm2Fd)
# d.addFigure(ifcm2Fd)
#
#imgCm10=pyHImage('/media/francisco/Packard Bell/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_000.bmp')
#imgCm11=pyHImage('/media/francisco/Packard Bell/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_001.bmp')
imgCm10 = pyHImage(
'/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_000.bmp'
)
imgCm11 = pyHImage(
'/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_001.bmp'
)
#fm=FlannMacher()
fm = FundamentalMatrix()
#fm=HomographyMatrix()
fm.imgcv1 = imgCm10.data
fm.imgcv2 = imgCm11.data
#fm.imgcv1=cam1.getImage().data
#fm.imgcv2=cam.getImage().data
imgCm5 = pyHImage()
imgCm5.setData(fm.process())
ifcm5 = pyHImageFigure(0, 0, 640, 240, imgCm5)
d.addFigure(ifcm5)
immf = pyHImagesMixedFigure(20, 20, 640, 240, imgCm10)
immf.setImage2(imgCm11)
immf.setFilter(fm)
immf.setImageSourceFigure1(cam)
immf.setImageSourceFigure2(cam1)
d.addFigure(immf)
imsf = pyHImageSecFilterFigure(300, 20, 50, 50)
imsf.setFilter(fm)
imfsec = pyHImageFigure(330, 20, 640, 240)
imsf.addChangedImageObserver(imfsec)
cam.addChangedImageObserver(imsf)
d.addFigure(imsf)
d.addFigure(imfsec)
def onMouseDown(self,e):
i=pyHImage('../images/im2.png')
f=pyHImageFigure(e.getX(),e.getY(),i.getWidth(),i.getHeight(),i)
self.view.getDrawing().addFigure(f)
self.view.update()
def onMouseDown(self, e):
i = pyHImage('../images/im2.png')
f = pyHImageFigure(e.getX(), e.getY(), i.getWidth(), i.getHeight(), i)
self.view.getDrawing().addFigure(f)
self.view.update()
def __init__(self):
super(pyHStandardEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
d=self.getView().getDrawing()
txt=pyHTextFigure(0,0,20,20,"Hola Caracola")
d.addFigure(txt)
cam1=pyHCameraFigure(0,200,50,50,1)
d.addFigure(cam1)
imgc1=pyHImageDottedFigure(150,250,320,240)
imgc1.setPoints([pyHPoint(20,20),pyHPoint(30,30)])
d.addFigure(imgc1)
cam1.addChangedImageObserver(imgc1)
cam=pyHCameraFigure(0,100,50,50,0)
d.addFigure(cam)
img0=pyHImageFigure(50,10,320,240)
d.addFigure(img0)
cam.addChangedImageObserver(img0)
img1=pyHImageFigure(400,10,320,240)
d.addFigure(img1)
fimg=pyHImageFilterFigure(300,300,40,40)
d.addFigure(fimg)
cam.addChangedImageObserver(fimg)
fimg.addChangedImageObserver(img1)
#img2=pyHImageFigure(0,300,320,240)
#d.addFigure(img2)
img3=pyHImageFigure(400,10,320,240)
d.addFigure(img3)
fimg1=pyHImageFilterFigure(300,400,40,40)
d.addFigure(fimg1)
fimg1.setFilter(Gaussian())
cam.addChangedImageObserver(fimg1)
fimg1.addChangedImageObserver(img3)
fimgH=pyHImageFilterFigure(400,500,30,30)
d.addFigure(fimgH)
fimgH.setFilter(HistogramColor())
fimg1.addChangedImageObserver(fimgH)
imgH=pyHImageFigure(500,0,256,150)
d.addFigure(imgH)
fimgH.addChangedImageObserver(imgH)
fimg2=pyHImageFilterFigure(300,500,40,40)
d.addFigure(fimg2)
fimg2.setFilter(SobelX())
fimg1.addChangedImageObserver(fimg2)
img4=pyHImageFigure(0,300,320,240)
d.addFigure(img4)
fimg2.addChangedImageObserver(img4)
fimg2y=pyHImageFilterFigure(240,300,40,40)
d.addFigure(fimg2y)
fimg2y.setFilter(SobelY())
fimg1.addChangedImageObserver(fimg2y)
img4y=pyHImageFigure(240,300,320,240)
d.addFigure(img4y)
fimg2y.addChangedImageObserver(img4y)
img4=pyHImageFigure(10,600,320,240)
d.addFigure(img4)
fimg4=pyHImageFilterFigure(0,600,40,40)
d.addFigure(fimg4)
fimg4.setFilter(OpticalFlow())
cam.addChangedImageObserver(fimg4)
fimg4.addChangedImageObserver(img4)
img5=pyHImageFigure(400,600,320,240)
d.addFigure(img5)
fimg5=pyHImageFilterFigure(400,600,40,40)
d.addFigure(fimg5)
fimg5.setFilter(FastFeatureDetector())
cam.addChangedImageObserver(fimg5)
img5.setImageSourceFigure(fimg5)
# #imgCm1=pyHImage('/Users/paco/Pictures/sfm/urjcMostolesMobilLG440-15011301/CAM00295.jpg')
# imgCm1=pyHImage('/media/francisco/Packard Bell/Users/paco/Pictures/sfm/pistaTenis/CAM00698.jpg')
# #imgCm1=pyHImage('../images/im2.png')
# ifcm1=pyHImageDottedFigure(0,0,240,320,imgCm1)
# ifcm1.setPoints([pyHPoint(400,690),pyHPoint(1776,690),pyHPoint(1394,560),pyHPoint(737,560)])
# d.addFigure(ifcm1)
#
# fd=FeatureDetector("SIFT")
# imgCvCm1Fd=fd.process(imgCm1.data)
# #imgCm1=pyHImage('../images/im2.png')
# imgCm1Fd=pyHImage()
# imgCm1Fd.setData(imgCvCm1Fd)
# ifcm1Fd=pyHImageFigure(0,320,240,320,imgCm1Fd)
# d.addFigure(ifcm1Fd)
#
# #imgCm2=pyHImage('/Users/paco/Pictures/sfm/urjcMostolesMobilLG440-15011301/CAM00294.jpg')
# imgCm2=pyHImage('/media/francisco/Packard Bell/Users/paco/Pictures/sfm/pistaTenis/CAM00699.jpg')
# #imgCm1=pyHImage('../images/im2.png')
# ifcm2=pyHImageDottedFigure(240,0,240,320,imgCm2)
# ifcm2.setPoints([pyHPoint(400,690),pyHPoint(1776,690),pyHPoint(1394,560),pyHPoint(737,560)])
# d.addFigure(ifcm2)
#
# imgCvCm2Fd=fd.process(imgCm2.data)
# imgCm2Fd=pyHImage()
# imgCm2Fd.setData(cv2.addWeighted(imgCvCm1Fd,0.5,imgCvCm2Fd,0.5,0))
# ifcm2Fd=pyHImageFigure(0,0,240,320,imgCm2Fd)
# d.addFigure(ifcm2Fd)
#
#imgCm10=pyHImage('/media/francisco/Packard Bell/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_000.bmp')
#imgCm11=pyHImage('/media/francisco/Packard Bell/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_001.bmp')
imgCm10=pyHImage('/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_000.bmp')
imgCm11=pyHImage('/Users/paco/Dropbox/Tranquinet/I+D+I/Electronica/robotica/software/tracking/tranquiTrack/images/logitech/shot_0_001.bmp')
#fm=FlannMacher()
fm=FundamentalMatrix()
#fm=HomographyMatrix()
fm.imgcv1=imgCm10.data
fm.imgcv2=imgCm11.data
#fm.imgcv1=cam1.getImage().data
#fm.imgcv2=cam.getImage().data
imgCm5=pyHImage()
imgCm5.setData(fm.process())
ifcm5=pyHImageFigure(0,0,640,240,imgCm5)
d.addFigure(ifcm5)
immf=pyHImagesMixedFigure(20,20,640,240,imgCm10)
immf.setImage2(imgCm11)
immf.setFilter(fm)
immf.setImageSourceFigure1(cam)
immf.setImageSourceFigure2(cam1)
d.addFigure(immf)
imsf=pyHImageSecFilterFigure(300,20,50,50)
imsf.setFilter(fm)
imfsec=pyHImageFigure(330,20,640,240)
imsf.addChangedImageObserver(imfsec)
cam.addChangedImageObserver(imsf)
d.addFigure(imsf)
d.addFigure(imfsec)