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