def __init__(self):
pyHConnectionFigure.__init__(self)
self.forwardText = pyHTextFigure(0, 0, 0, 0, "")
self.backwardText = pyHTextFigure(0, 0, 0, 0, "")
self.set_default_style()
def __init__(self, conector, l=120):
pyHAbstractFigure.__init__(self)
self.setColor(pyHAttributeColor(150, 150, 150, 255))
self.setAttribute('WIDTH', pyHAttributeWidth(2))
self.connectorStart = None
self.points = [None] * 2
self.lenline = l
self.forwardText = pyHTextFigure(0, 0, 0, 0, "")
self.forwardText.setColor(COLOR.get('BLUE'))
self.backwardText = pyHTextFigure(0, 0, 0, 0, "")
self.backwardText.setColor(COLOR.get('RED'))
self._error_figure = MathTextFigure(0, 0, r'\varepsilon')
self._error_figure.setColor(pyHAttributeColor(0, 0, 0, 0))
self.setConnectorStart(conector)
def __init__(self,x0,y0,w,h,text="ImageFilter"):
#super(pyHImageFilterFigure,self).__init__(x0,y0,w,h)
super(pyHImageFilterFigure,self).__init__(x0,y0,w,h)
self.changedImageObservers=[]
self.imageSink=None
self.imageFilter=FaceShapeDetection()
#self.outputConnectionFigure=None #more than one output allowed
self.textFigure=pyHTextFigure(x0,y0,w,h,text,border=False)
self.inputConnectionFigure=None
def __init__(self,x,y,w=80,h=40,topic="/camera/rgb/image_color",text=" ROS Camera ",width=640,hight=480):
super(pyHROSCameraFigure,self).__init__(x,y,w,h)
#pyHImageSourceFigure.__init__(self, x, y, w, h)
"""Initialize camera."""
self.textFigure=pyHTextFigure(x,y,w,h,text,border=False)
self.inputConnectionFigure=None
self.flip=False
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(topic,Image,self.callback,None,1)
rospy.init_node('pyHROSCameraFigure', anonymous=True)
def __init__(self,xdata,ydata,labels=None,figures=None):
pyHCompositeTransform2DFigure.__init__(self)
self.maxX=1e-20
self.maxY=1e-20
self.minX=1e+20
self.minY=1e+20
for x,y in zip(xdata,ydata):
self.updateMaxMin(x,y)
f=pyHTextFigure(x-0.5,y-0.5,1,1," * ",border=False)
self.addFigure(f)
self.r=self.getRecFromMinMax()
def __init__(self,x,y,w=80,h=40,camID=0,text=" Camera ",width=640,hight=480):
super(pyHCameraFigure,self).__init__(x,y,w,h)
#pyHImageSourceFigure.__init__(self, x, y, w, h)
"""Initialize camera."""
self.camID=camID
self.capture = cv2.VideoCapture(camID)
#figure width and height same that image width and hegight. ????
#self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.width)
#self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height)
self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT,hight)
# self.timer = QtCore.QTimer()
# self.timer.timeout.connect(self.displayVideoStream)
# self.timer.start(200)
self.timer = Timer(0.5,self.displayVideoStream)
self.timer.start()
self.textFigure=pyHTextFigure(x,y,w,h,text,border=False)
self.inputConnectionFigure=None
self.flip=False
def __init__(self,g):
pyHCompositeFigure.__init__(self)
''' build information matrix figure '''
d=self
#Units metres
cellSize=3
# HEADER
Xi=g.Xi
Omega=g.Omega
NM=g.N+g.L
NM2=NM*g.subDim
gf=pyHGridFigure(100,NM2*cellSize,1,NM,cellSize*2,cellSize*2)
for i in range(NM):
if i>N-1:
tf=pyHTextFigure(0,0,cellSize*2,cellSize*2,i-N)
tf.setColor(0,0,255)
else:
tf=pyHTextFigure(0,0,cellSize*2,cellSize*2,i)
gf.addFigure(tf)
d.addFigure(gf)
# BODY
# Xi grid figure
XiFigure=pyHGridFigure(100+NM2*cellSize,0,NM2,1,cellSize*2,cellSize)
d.addFigure(XiFigure)
for i in range(NM2):
if abs(Xi.value[i][0])>0.000001:
tf=pyHTextFigure(0,0,cellSize*2,cellSize,Xi.value[i][0])
tf.setFillColor(255, 255, 255)
else:
tf=pyHTextFigure(0,0,cellSize*2,cellSize,"0.00")
XiFigure.addFigure(tf)
# Omega grid figure
OmegaFigure=pyHGridFigure(100,0,NM2,NM2,cellSize,cellSize)
for i in range(NM2):
for j in range(NM2):
if abs(Omega.value[i][j])>0.000001:
tf=pyHTextFigure(0,0,cellSize,cellSize,Omega.value[i][j])
tf.setFillColor(255,255,200)
if i>N*2-1 or j>N*2-1:
tf.setColor(0,0,255)
else:
tf=pyHTextFigure(0,0,cellSize,cellSize," ")
if i>N*2-1 or j>N*2-1:
tf.setColor(0,0,255,50)
else:
tf.setColor(0,0,0,50)
OmegaFigure.addFigure(tf)
d.addFigure(OmegaFigure)
def __init__(self, title):
super(pyHStandardEditor, self).__init__()
self.title = title
self.img = np.zeros((480, 640, 3), np.uint8)
self.img[:] = 255
self.initUI()
d = self.getView().getDrawing()
txt = pyHTextFigure(100, 200, 20, 20, "Hola Caracola")
d.addFigure(txt)
rf = pyHRectangleFigure(10, 10, 40, 40)
rf.setColor(255, 0, 0)
d.addFigure(rf)
cam = pyHCameraFigure(0, 50, 50, 50, 0)
d.addFigure(cam)
img0 = pyHImageFigure(250, 60, 320, 240)
cam.addChangedImageObserver(img0)
d.addFigure(img0)
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(pyHCVEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
d=self.getView().getDrawing()
txt=pyHTextFigure(0,400,20,20,"Hola Caracola")
d.addFigure(txt)
#cam1=pyHCameraFigure(0,200,50,50,0)
#d.addFigure(cam1)
cam=pyHCameraFigure(0,100,50,50,0)
d.addFigure(cam)
imgc1=pyHImageDottedFigure(150,250,320,240)
imgc1.setPoints([pyHPoint(20,20),pyHPoint(30,30)])
d.addFigure(imgc1)
cam.addChangedImageObserver(imgc1)
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)
def __init__(self):
super(pyHVisionEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
d=self.getView().getDrawing()
txt=pyHTextFigure(0,400,20,20,"Hola Caracola")
d.addFigure(txt)
#cam1=pyHCameraFigure(0,200,50,50,0)
#d.addFigure(cam1)
camD=pyHCameraFigure(0,520,80,50,0)
#camD=pyHROSCameraFigure(0,520,80,50,"/stereo/left/image_color")
d.addFigure(camD)
camD.addPreviewFigure(d)
cam=pyHImageFilterFigure(330,520,80,50,"UnDistorted")
d.addFigure(cam)
K=np.array([[962.42013601, 0. , 340.60034923],
[ 0. , 949.00606667, 217.94310531],
[ 0. , 0. , 1. ]])
dist=np.array([[-4.84892530e-01, 1.42225078e+00,
1.03498850e-03, -3.61441295e-03, -7.94542681e+00]])
cam.setFilter(Undistorter(K,dist))
cam.addPreviewFigure(d)
cf=cam.getInputConnectionFigure(camD)
d.addFigure(cf)
fimg=pyHImageFilterFigure(660,520,80,40," Gaussian ")
d.addFigure(fimg)
fimg.setFilter(Gaussian())
fimg.addPreviewFigure(d)
cf=fimg.getInputConnectionFigure(cam)
d.addFigure(cf)
fImgDetec=pyHImageFilterFigure(330,60,80,40," Object Detector SSD")
d.addFigure(fImgDetec)
fImgDetec.setFilter(pyHDetector())
fImgDetec.addPreviewFigure(d)
cf=fImgDetec.getInputConnectionFigure(cam)
d.addFigure(cf)
fimg1=pyHImageFilterFigure(660,60,80,40," Face Detector ")
d.addFigure(fimg1)
fimg1.addPreviewFigure(d)
#cam.addChangedImageObserver(fimg1)
cf=fimg1.getInputConnectionFigure(fImgDetec)
d.addFigure(cf)
#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(990,800,80,40,"SobelX ")
fimg2.setColor(0,100,0,100)
d.addFigure(fimg2)
fimg2.setFilter(SobelX())
fimg2.addPreviewFigure(d)
cf=fimg2.getInputConnectionFigure(fimg)
d.addFigure(cf)
fimg2y=pyHImageFilterFigure(990,330,80,40,"SobelY ")
fimg2y.setColor(0,100,0,255)
d.addFigure(fimg2y)
fimg2y.setFilter(SobelY())
fimg2y.addPreviewFigure(d)
cf=fimg2y.getInputConnectionFigure(fimg)
d.addFigure(cf)
fimg4=pyHImageFilterFigure(1350,60,80,40,"OpticalFlowLK")
d.addFigure(fimg4)
fimg4.setFilter(OpticalFlowPyrLK())
fimg4.addPreviewFigure(d)
cf=fimg4.getInputConnectionFigure(fimg)
d.addFigure(cf)
fimg4=pyHImageFilterFigure(1700,330,80,40," OpticalFlow ")
d.addFigure(fimg4)
fimg4.setFilter(OpticalFlow())
fimg4.addPreviewFigure(d)
cf=fimg4.getInputConnectionFigure(fimg2y)
d.addFigure(cf)
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 __init__(self):
pyHConnectionFigure.__init__(self)
self._forward_info_figure = pyHTextFigure(0, 0, 0, 0, "")
self._backward_info_figure = pyHTextFigure(0, 0, 0, 0, "")
self.set_default_style()
def __init__(self):
print("Python version")
print (sys.version)
print("Version info.")
print (sys.version_info)
super(pyHVisionEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
d=self.getView().getDrawing()
txt=pyHTextFigure(0,400,20,20,"Hola Caracola")
d.addFigure(txt)
#cam1=pyHCameraFigure(0,200,50,50,0)
#d.addFigure(cam1)
camD=pyHCameraFigure(200,500,80,50,0,width=640*2)
d.addFigure(camD)
camD.addPreviewFigure(d,2)
camL=pyHImageFilterFigure(240,500,80,50,"Camera Left")
d.addFigure(camL)
camL.setFilter(StereoSplitLeft())
camL.addPreviewFigure(d)
cf=camL.getInputConnectionFigure(camD)
d.addFigure(cf)
camR=pyHImageFilterFigure(240,500,80,50,"Camera Right")
d.addFigure(camR)
camR.setFilter(StereoSplitRight())
camR.addPreviewFigure(d)
cf=camR.getInputConnectionFigure(camD)
d.addFigure(cf)
#CALIBRATIOM INFO
KL=np.array([[917.407239 , 0. , 328.52914613],
[ 0. , 919.1541511 , 224.78093398],
[ 0. , 0. , 1. ]])
distL=np.array([[-4.51526228e-01, 3.58762955e-01, 1.17374106e-04, -1.83499141e-03, -1.87366908e-01]])
KR=np.array([[918.22817005, 0. , 302.54145304],
[ 0. , 920.02185944, 242.24430011],
[ 0. , 0. , 1. ]])
distR=np.array([[-4.56781581e-01, 6.13760382e-01, 4.19512761e-04, 4.46799077e-04, -1.92449009e+00]])
R=np.array([[ 9.99720798e-01, 5.23223467e-04, -2.36231162e-02],
[-1.85366840e-04, 9.99897706e-01, 1.43018752e-02],
[ 2.36281828e-02, -1.42935031e-02, 9.99618630e-01]])
T=np.array([[-59.30552545],
[ 0.09056016],
[ 0.42453778]])
E=np.array([[ 2.21846721e-03, -4.25788770e-01, 8.44539355e-02],
[ 1.82570104e+00, -8.47461585e-01, 5.92728792e+01],
[-7.95415963e-02, -5.92995062e+01, -8.46040909e-01]])
F=np.array([[-3.40628961e-09, 6.52508079e-07, -2.70309936e-04],
[-2.79783019e-06, 1.29620844e-06, -8.27887809e-02],
[ 7.66496661e-04, 8.28977953e-02, 1.00000000e+00]])
KL=np.array([[922.45198566, 0., 317.57070143],
[ 0., 924.16244459, 228.66603252],
[ 0., 0., 1., ]])
distL=np.array([[-0.44497071, 0.20006589, 0., 0., 0.51140312]])
KR=np.array([[922.45198566, 0., 306.07490755],
[ 0., 924.16244459, 240.69430766],
[ 0., 0., 1., ]])
distR=np.array([[-0.47532508, 0.81778056, 0., 0., -2.89338016]])
R=np.array([[ 9.99942658e-01, 3.81501933e-04, -1.07021197e-02],
[-3.69180797e-04, 9.99999267e-01, 1.15323195e-03],
[ 1.07025518e-02, -1.14921481e-03, 9.99942066e-01]])
T=np.array([[-5.93499276e+01],
[ 3.76647220e-02],
[ 5.29152796e-01]])
#T=np.array([[-60],
# [ 0],
# [ 5.29152796e-01]])
E=np.array([[ 5.98461691e-04, -5.29195693e-01, 3.70523040e-02],
[ 1.16431813e+00, -6.80039429e-02, 5.93408262e+01],
[-1.57517087e-02, -5.93498985e+01, -6.80411407e-02]])
F=np.array([[-8.92108636e-10, 7.87395889e-07, -2.30716939e-04],
[-1.73240131e-06, 1.00996515e-07, -8.09197674e-02],
[ 4.38911905e-04, 8.11939734e-02, 1.00000000e+00]])
ucamL=pyHImageFilterFigure(240,500,80,50,"Rectify Left")
d.addFigure(ucamL)
#ucamL.setFilter(Undistorter(KL,distL))
ucamL.setFilter(Rectify(KL,distL,KR,distR,R,T,left=True))
ucamL.addPreviewFigure(d)
cf=ucamL.getInputConnectionFigure(camL)
d.addFigure(cf)
ucamR=pyHImageFilterFigure(240,500,80,50,"Rectify Right")
d.addFigure(ucamR)
#ucamR.setFilter(Undistorter(K,dist))
ucamR.setFilter(Rectify(KL,distL,KR,distR,R,T,left=False))
ucamR.addPreviewFigure(d)
cf=ucamR.getInputConnectionFigure(camR)
d.addFigure(cf)
fmf=pyHImages2I1OFilterFigure(400,500,80,50,"FundamentalMatrixStereo")
d.addFigure(fmf)
fmf.setImageSourceFigure1(ucamL)
fmf.setImageSourceFigure2(ucamR)
fmf.setFilter(FundamentalMatrixStereo())
fmf.addPreviewFigure(d,2)
self.getView().setTransformFitToDrawing()
def __init__(self,x,y,w=80,h=40,folder="./",text=" Folder: ",width=640,hight=480):
super(pyHCameraFigure,self).__init__(x,y,w,h)
self.textFigure=pyHTextFigure(x,y,w,h,text+folder,border=False)
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)
#imgCm3=pyHImage('../images/googleEarth_CampusMostoles15031101.png')
#ifcm3=pyHImageFigure(0,0,320*2,240*2,imgCm3)
#d.addFigure(ifcm3)
#imgCm4=pyHImage('../images/googleEarth_CampusMostolesZoom15031101.png')
#ifcm4=pyHImageFigure(0,0,320*2,240*2,imgCm4)
#d.addFigure(ifcm4)
self.getView().setTransformFitToDrawing()
def __init__(self):
super(pyHVisionEditor, self).__init__()
pyHAbstractEditor.__init__(self)
self.initActionMenuToolBars()
self.statusBar()
self.initUI()
d = self.getView().getDrawing()
txt = pyHTextFigure(0, 400, 20, 20, "Hola Caracola")
d.addFigure(txt)
#cam1=pyHCameraFigure(0,200,50,50,0)
#d.addFigure(cam1)
cam = pyHCameraFigure(200, 500, 50, 50, 0)
d.addFigure(cam)
cam.addPreviewFigure(d)
fimg = pyHImageFilterFigure(500, 500, 40, 40)
d.addFigure(fimg)
fimg.setFilter(Gaussian())
fimg.addPreviewFigure(d)
cf = fimg.getInputConnectionFigure(cam)
d.addFigure(cf)
fimg1 = pyHImageFilterFigure(800, 500, 40, 40)
d.addFigure(fimg1)
fimg1.addPreviewFigure(d)
#cam.addChangedImageObserver(fimg1)
cf = fimg1.getInputConnectionFigure(cam)
d.addFigure(cf)
#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(800, 800, 40, 40)
d.addFigure(fimg2)
fimg2.setFilter(SobelX())
fimg2.addPreviewFigure(d)
cf = fimg2.getInputConnectionFigure(fimg)
d.addFigure(cf)
fimg2y = pyHImageFilterFigure(800, 300, 40, 40)
d.addFigure(fimg2y)
fimg2y.setFilter(SobelY())
fimg2y.addPreviewFigure(d)
cf = fimg2y.getInputConnectionFigure(fimg)
d.addFigure(cf)
fimg4 = pyHImageFilterFigure(0, 600, 40, 40)
d.addFigure(fimg4)
fimg4.setFilter(OpticalFlow())
fimg4.addPreviewFigure(d)
cf = fimg4.getInputConnectionFigure(fimg)
d.addFigure(cf)
fimg4 = pyHImageFilterFigure(800, 600, 40, 40)
d.addFigure(fimg4)
fimg4.setFilter(OpticalFlow())
fimg4.addPreviewFigure(d)
cf = fimg4.getInputConnectionFigure(fimg2y)
d.addFigure(cf)
self.getView().setTransformFitToDrawing()
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()
