import os import subprocess from PyQt5.QtWidgets import (QWidget,QVBoxLayout,QTabWidget,QSplitter, QFileDialog,QMessageBox,QComboBox, QLineEdit,QTextEdit,QCheckBox,QLabel,QFrame,QPushButton,QMenu,QAction) from PyQt5.QtGui import QPixmap,QImage,QPainter,QResizeEvent,QCloseEvent,QPaintEvent from PyQt5.QtCore import QRect,Qt import numpy as np import math import def_pb2 as pb import GrpcClient as rpc import vtk import cv2 import time import google.protobuf.text_format as tf from vtkmodules.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor #from vtk.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor import threading class ControlFrame(QFrame): cameraMap={"相机1":0,"相机2":1,"相机3":2,"相机4":3} def __init__(self): QFrame.__init__(self) self.setGeometry(0, 0, 500, 500) self.images_=pb.ResImage() self.InitUI() self.setFrameShape(self.StyledPanel) #self.timer_ = QTimer() #self.timer_.timeout.connect(self.Update) #self.timer_.start(100) def OnImages(self,images:pb.ResImage): self.images_=images def DisplayMeasureInfo(self,info : pb.MeasureInfo): xstr="x\t: %.3f\n"%(info.x) if info.HasField("x") else "x\t: None\n" ystr="y\t: %.3f\n"%(info.y) if info.HasField("y") else "y\t: None\n" thetastr="角度\t: %.3f\n"%(info.theta*180.0/np.pi) if info.HasField("theta") else "角度\t: None\n" widthstr="宽\t: %.3f\n"%(info.width) if info.HasField("width") else "宽\t: None\n" wheelbasestr="轴距\t: %.3f\n"%(info.wheelbase) if info.HasField("wheelbase") else "轴距\t: None\n" fthetastr="前轮角\t: %.3f\n"%(info.ftheta*180.0/np.pi) if info.HasField("ftheta") else "前轮角\t: None\n" error=info.error self.ErrorInfo.setText(xstr+ystr+thetastr+widthstr+wheelbasestr+fthetastr+error) def OnAction(self,dtype,id): client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) try: if dtype==1: client.GrabImage(ip,port,id) if dtype==2: client.GrabCloud(ip,port,id) except Exception as e: QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok) def InitUI(self): self.begstatic = QLabel(self) self.begstatic.setText(" IP:") self.begstatic.setGeometry(20, 5, 80, 25) self.IPEdit = QLineEdit(self) self.IPEdit.setText("10.211.31.16") self.IPEdit.setGeometry(90, 5, 150, 25) self.endstatic = QLabel(self) self.endstatic.setText("Port:") self.endstatic.setGeometry(20, 35, 80, 25) self.PortEdit = QLineEdit(self) self.PortEdit.setText("9876") self.PortEdit.setGeometry(90, 35, 150, 25) self.btnImageStreamCheck = QCheckBox("实时图像", self) self.btnImageStreamCheck.setGeometry(20, 65, 120, 30) self.btnImageStreamCheck.clicked.connect(self.RealTimeImagecb) self.btnDataStreamCheck = QCheckBox("实时结果打印", self) self.btnDataStreamCheck.setGeometry(150, 65, 120, 30) self.btnDataStreamCheck.clicked.connect(self.RealTimeDatacb) self.btnGrab = QPushButton(self) self.btnGrab.setGeometry(20, 100, 100, 40) self.btnGrab.setText("抓取图像") self.btnGrab.clicked.connect(self.btnGrabImageClick) self.btnCancel = QPushButton(self) self.btnCancel.setGeometry(150, 100, 100, 30) self.btnCancel.setText("抓取点云") self.btnCancel.clicked.connect(self.btnGrabCloudClick) self.btnMergeSave = QPushButton(self) self.btnMergeSave.setGeometry(20, 160, 100, 40) self.btnMergeSave.setText("拼接保存") self.btnMergeSave.clicked.connect(self.btnMergeSaveClick) self.btnSaveAllCloud = QPushButton(self) self.btnSaveAllCloud.setGeometry(150, 135, 100, 30) self.btnSaveAllCloud.setText("合并保存点云") self.btnSaveWheelSampleCloud = QPushButton(self) self.btnSaveWheelSampleCloud.setGeometry(150, 170, 100, 30) self.btnSaveWheelSampleCloud.setText("保存轮胎样本") self.ParameterEdit = QTextEdit(self) self.ParameterEdit.setText("") self.ParameterEdit.setGeometry(20, 300, 130, 150) self.MatrixEdit = QTextEdit(self) self.MatrixEdit.setAlignment(Qt.AlignTop) self.MatrixEdit.setText("输入4x4变换矩阵") self.MatrixEdit.setGeometry(20, 205, 500, 90) self.MatrixEdit.textChanged.connect(self.matrix2rpy) self.CameraListEdit=QComboBox(self) self.CameraListEdit.addItems(self.cameraMap.keys()) self.CameraListEdit.setGeometry(155, 300, 100, 30) self.btnChange = QPushButton(self) self.btnChange.setGeometry(155, 350, 100, 40) self.btnChange.setText("修改参数") self.btnChange.clicked.connect(self.ChangeCameraParameter) self.btnSave = QPushButton(self) self.btnSave.setGeometry(155, 410, 100, 40) self.btnSave.setText("写入参数") self.btnSave.clicked.connect(self.SaveCameraParameter) self.ErrorInfo = QLabel(self) self.ErrorInfo.setGeometry(20, 455, 250, 400) self.ErrorInfo.setWordWrap(True) self.ErrorInfo.setAlignment(Qt.AlignTop) def RealTimeDatacb(self): try: if self.btnDataStreamCheck.checkState()==Qt.Checked: client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) client.OpenDataStream(ip,port) else: client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) client.CloseDataStream(ip,port) except Exception as e: QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok) def RealTimeImagecb(self): try: if self.btnImageStreamCheck.checkState()==Qt.Checked: self.btnGrab.setEnabled(False) client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) client.OpenImageStream(ip,port) else: self.btnGrab.setEnabled(True) client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) client.CloseImageStream(ip,port) except Exception as e: QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok) def closeEvent(self, a0: QCloseEvent) -> None: if self.btnImageStreamCheck.checkState()==Qt.Checked: client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) client.CloseImageStream(ip,port) if self.btnDataStreamCheck.checkState()==Qt.Checked: client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) client.CloseDataStream(ip,port) rpc.GrpcStream().close() def btnGrabImageClick(self): if self.btnImageStreamCheck.checkState()==Qt.Checked: print(" 先关闭实时显示") else : client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) try: client.GrabImage(ip,port,-1) except Exception as e: QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok) def btnMergeSaveClick(self): # if self.images_.HasField("img1") and self.images_.HasField("img2")\ and self.images_.HasField("img3") and self.images_.HasField("img4"): img1=self.PbImg2numpy(self.images_.img1) img2=self.PbImg2numpy(self.images_.img2) img3=self.PbImg2numpy(self.images_.img3) img4=self.PbImg2numpy(self.images_.img4) img_up=np.concatenate([img1,img2],axis=1) img_down=np.concatenate([img3,img4],axis=1) image=np.concatenate([img_up,img_down],axis=0) file=time.strftime("%Y%m%d-%H%M%S")+'.jpg' cv2.imwrite("./images/"+file,image) else: print(" ---------------") def PbImg2numpy(self,image:pb.Image): img1=image w=img1.width h=img1.height c=img1.channel btarry=bytearray(img1.data) npdata = np.frombuffer(btarry, dtype=np.uint8) if c==3: npdata = npdata.reshape([h,w,c]) if c==1: npdata = npdata.reshape([h,w]) return npdata def btnGrabCloudClick(self): client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) try: for id in range(4): client.GrabCloud(ip,port,id) except Exception as e: QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok) def rotationMatrixToEulerAngles(self,R) : sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) singular = sy < 1e-6 if not singular : x = math.atan2(R[2,1] , R[2,2]) y = math.atan2(-R[2,0], sy) z = math.atan2(R[1,0], R[0,0]) else : x = math.atan2(-R[1,2], R[1,1]) y = math.atan2(-R[2,0], sy) z = 0 return np.array([x, y, z]) def SaveCameraParameter(self): self.btnSave.setEnabled(False) camera=self.CameraListEdit.currentText() id=self.cameraMap[camera] text=self.ParameterEdit.toPlainText() transform=pb.CoordinateTransformation3D() try: tf.Parse(text, transform) except: QMessageBox.information(self, '格式错误',text,QMessageBox.Ok,QMessageBox.Ok) self.btnSave.setEnabled(True) print(" 保存相机%d参数:%s"%(id,transform)) client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) try: ret=client.SaveCameraParameter(ip,port,id,transform) QMessageBox.information(self, 'Info',ret.info,QMessageBox.Ok,QMessageBox.Ok) except Exception as e: QMessageBox.information(self, 'Info',str(e),QMessageBox.Ok,QMessageBox.Ok) self.btnSave.setEnabled(True) def ChangeCameraParameter(self): self.btnChange.setEnabled(False) camera=self.CameraListEdit.currentText() id=self.cameraMap[camera] text=self.ParameterEdit.toPlainText() transform=pb.CoordinateTransformation3D() try: tf.Parse(text, transform) except: QMessageBox.information(self, '格式错误',text,QMessageBox.Ok,QMessageBox.Ok) self.btnChange.setEnabled(True) return print(" 修改相机%d参数:%s"%(id,transform)) client=rpc.GrpcStream() ip=self.IPEdit.text() port=int(self.PortEdit.text()) try: ret=client.ChangeCameraParameter(ip,port,id,transform) QMessageBox.information(self, 'Info',ret.info,QMessageBox.Ok,QMessageBox.Ok) except Exception as e: QMessageBox.information(self, 'Info',str(e),QMessageBox.Ok,QMessageBox.Ok) self.btnChange.setEnabled(True) def matrix2rpy(self): text="" str=self.MatrixEdit.toPlainText() nums=str.split('\n') if not len(nums)==4: text="变换矩阵格式错误 需要4x4矩阵" else: matrix=np.zeros(shape=[4,4],dtype=np.float32) count=0 for line in nums: datas=line.split(' ') if not len(datas)==4: text="变换矩阵格式错误 需要4x4矩阵" break else: matrix[count,:]=[float(datas[0]),float(datas[1]),float(datas[2]),float(datas[3])] count+=1 rpy=self.rotationMatrixToEulerAngles(matrix) text="roll : %.3f,\npitch: %.3f,\nyaw: %.3f,\nx:%.3f,\ny:%.3f,\nz:%.3f,"%(math.degrees(rpy[0]),math.degrees(rpy[1]),math.degrees(rpy[2]), matrix[0,3],matrix[1,3],matrix[2,3]) self.ParameterEdit.setText(text) #QMessageBox.information(self, '旋转变换转rpy-t',text,QMessageBox.Ok,QMessageBox.Ok) def btnCancelClick(self): pass class ImageViewer(QLabel): def __init__(self,parent,OnAction,id): super(ImageViewer,self).__init__(parent=parent) self.OnAction=OnAction self.dtype=1 self.id=id self.image=None self.show = False self.setStyleSheet("border-width:1px;border-style:solid;border-color:rgb(150,150,150)") def ShowImg(self,pixmap): self.show=True self.image=pixmap self.update() def updataData(self): self.OnAction(self.dtype,self.id-1) print("%d,%d"%(self.dtype,self.id-1)) def contextMenuEvent(self, a0): menu=QMenu(self) updata_act=QAction("更新") updata_act.triggered.connect(self.updataData) menu.addAction(updata_act) ret=menu.exec_(a0.globalPos()) def paintEvent(self, a0: QPaintEvent) -> None: if not self.image == None: w, h = self.size().width(),self.size().height() iw, ih = self.image.width(), self.image.height() painter=QPainter(self) painter.drawPixmap(QRect(0,0,w,h),self.image,QRect(0,0,iw,ih)) class VtkPointCloud: def __init__(self, zMin=-10.0, zMax=10.0, maxNumPoints=1e7): self.maxNumPoints = maxNumPoints self.vtkPolyData = vtk.vtkPolyData() self.clearPoints() mapper = vtk.vtkPolyDataMapper() mapper.SetInputData(self.vtkPolyData) mapper.SetColorModeToDefault() mapper.SetScalarRange(zMin, zMax) mapper.SetScalarVisibility(1) self.vtkActor = vtk.vtkActor() self.vtkActor.SetMapper(mapper) self.vtkActor.GetProperty().SetPointSize(1) self.vtkActor.GetProperty().SetColor(1, 1, 1) def addPoint(self, point): if self.vtkPoints.GetNumberOfPoints() < self.maxNumPoints: pointId = self.vtkPoints.InsertNextPoint(point[0:-1]) self.vtkDepth.InsertNextValue(point[2]) self.vtkCells.InsertNextCell(1) self.vtkCells.InsertCellPoint(pointId) self.vtkCells.Modified() self.vtkPoints.Modified() self.vtkDepth.Modified() def clearPoints(self): self.vtkPoints = vtk.vtkPoints() self.vtkCells = vtk.vtkCellArray() self.vtkDepth = vtk.vtkDoubleArray() self.vtkDepth.SetName('DepthArray') self.vtkPolyData.SetPoints(self.vtkPoints) self.vtkPolyData.SetVerts(self.vtkCells) #self.vtkPolyData.GetPointData().SetScalars(self.vtkDepth) self.vtkPolyData.GetPointData().SetActiveScalars('DepthArray') class pclViewer(QVTKRenderWindowInteractor): def __init__(self,parent=None): super(pclViewer,self).__init__(parent=parent) def mousePressEvent(self, ev): btn = ev.button() if btn == Qt.RightButton: print(" r btn") else: QVTKRenderWindowInteractor.mousePressEvent(self,ev) class VtkPointCloudCanvas(QWidget): def __init__(self, *args, **kwargs): super(VtkPointCloudCanvas, self).__init__(*args, **kwargs) self.lock_=threading.RLock() self._layout = QVBoxLayout() self.setLayout(self._layout) self._vtk_widget = pclViewer(self) self._vtk_widget.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera()) # 设置交互方式==常用的方式 移动摄像机 self._layout.addWidget(self._vtk_widget) self._render = vtk.vtkRenderer() self._vtk_widget.GetRenderWindow().AddRenderer(self._render) self._iren = self._vtk_widget.GetRenderWindow().GetInteractor() self._point_cloud = VtkPointCloud() self._render.AddActor(self._point_cloud.vtkActor) transform = vtk.vtkTransform() transform.Translate(0, 0.0, 0.0) axes = vtk.vtkAxesActor() axes.SetUserTransform(transform) colors = vtk.vtkNamedColors() axes.GetXAxisCaptionActor2D().GetCaptionTextProperty().SetColor(colors.GetColor3d("Red")) axes.GetYAxisCaptionActor2D().GetCaptionTextProperty().SetColor(colors.GetColor3d("Green")) axes.GetZAxisCaptionActor2D().GetCaptionTextProperty().SetColor(colors.GetColor3d("Blue")) self._render.AddActor(axes) self.camera=self._render.GetActiveCamera() self.show() self._iren.Initialize() def close(self) -> bool: print(" tk close") self._point_cloud.clearPoints() def displayPCL(self,cloud:np.array,type): #type 1:只显示轮胎，其他：显示所有 colors = vtk.vtkUnsignedCharArray() colors.SetNumberOfComponents(3) colors.SetName("Colors") self._point_cloud.clearPoints() for point in cloud: prob=point[-1] if type==1: if prob>0.9: self._point_cloud.addPoint(point) colors.InsertNextTypedTuple((0,255,0)) else: self._point_cloud.addPoint(point) if prob>0.9: colors.InsertNextTypedTuple((0,255,0)) else: colors.InsertNextTypedTuple((255,0,0)) self._point_cloud.vtkPolyData.GetPointData().SetScalars(colors) self._vtk_widget.update() def resetViewer(self): self.update() class PointCLViwer(QSplitter): def __init__(self,OnAction,id): super(PointCLViwer, self).__init__() self.OnAction=OnAction self.dtype=2 self.id=id self.pointCloud=[] self.InitUI() def InitUI(self): self.pclViewer=VtkPointCloudCanvas(self) self.addWidget(self.pclViewer) self.setStretchFactor(0,4) def save(self): #fileName2, ok2 = QFileDialog.getSaveFileName(self, "文件保存", "C:/","All Files (*);;Text Files (*.txt)") #print(fileName2 ,ok2) fileName2="./cloud/%d.txt"%(self.id) with open(fileName2,"w+") as f: for point in self.pointCloud: prob=point[3] f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255))) print("Save Cloud to %s"%(fileName2)) subprocess.Popen(["cloudcompare.CloudCompare",fileName2]) #os.system("cloudcompare.CloudCompare %s"%(fileName2)) def saveWheel(self): #fileName2, ok2 = QFileDialog.getSaveFileName(self, "文件保存", "C:/","All Files (*);;Text Files (*.txt)") fileName2="./cloud/%d-wheel.txt"%(self.id) with open(fileName2,"w+") as f: for point in self.pointCloud: prob=point[3] if prob>0.9: f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255))) print("Save Cloud to %s"%(fileName2)) def displayCloud(self,points:np.array): self.pointCloud=points self.pclViewer.displayPCL(points,0) def resetViewer(self): self.pclViewer.resetViewer() def updataData(self): self.OnAction(self.dtype,self.id-1) def displayWheelPoints(self): self.pclViewer.displayPCL(self.pointCloud,1) def displayAllPoints(self): self.pclViewer.displayPCL(self.pointCloud,0) def contextMenuEvent(self, a0): menu=QMenu(self) updata_act=QAction("抓取点云") updata_act.triggered.connect(self.updataData) menu.addAction(updata_act) show_wheel=QAction("仅显示轮胎") show_wheel.triggered.connect(self.displayWheelPoints) menu.addAction(show_wheel) show_all=QAction("显示所有") show_all.triggered.connect(self.displayAllPoints) menu.addAction(show_all) #act=QAction("还原视野") #act.triggered.connect(self.resetViewer) #menu.addAction(act) actSave=QAction("保存所有点") actSave.triggered.connect(self.save) menu.addAction(actSave) actSaveWheel=QAction("保存轮胎点") actSaveWheel.triggered.connect(self.saveWheel) menu.addAction(actSaveWheel) ret=menu.exec_(a0.globalPos()) class ViewerFrame(QFrame): def __init__(self,OnAction): super(ViewerFrame, self).__init__() self.OnAction=OnAction self.InitUI() def InitUI(self): self.table1=QTabWidget(self) self.table2=QTabWidget(self) self.table3=QTabWidget(self) self.table4=QTabWidget(self) self.table1.setTabPosition(QTabWidget.TabPosition.North) self.table2.setTabPosition(QTabWidget.TabPosition.North) self.table3.setTabPosition(QTabWidget.TabPosition.North) self.table4.setTabPosition(QTabWidget.TabPosition.North) self.panel1=ImageViewer(self,self.OnAction,1) self.panel2=ImageViewer(self,self.OnAction,2) self.panel3=ImageViewer(self,self.OnAction,3) self.panel4=ImageViewer(self,self.OnAction,4) self.pcViewer1=PointCLViwer(self.OnAction,1) self.pcViewer2=PointCLViwer(self.OnAction,2) self.pcViewer3=PointCLViwer(self.OnAction,3) self.pcViewer4=PointCLViwer(self.OnAction,4) self.table1.addTab(self.panel1,"图像") self.table1.addTab(self.pcViewer1,"点云") self.table2.addTab(self.panel2,"图像") self.table2.addTab(self.pcViewer2,"点云") self.table3.addTab(self.panel3,"图像") self.table3.addTab(self.pcViewer3,"点云") self.table4.addTab(self.panel4,"图像") self.table4.addTab(self.pcViewer4,"点云") def closeEvent(self, a0: QCloseEvent) -> None: pass def resizeEvent(self, a0: QResizeEvent) -> None: w, h = self.size().width(), self.size().height() w=w-15 h=h-15 self.table1.setGeometry(5, 5, w/2, h/2) self.table2.setGeometry(10+w/2, 5, w/2, h/2) self.table3.setGeometry(5, 10+h/2, w/2, h/2) self.table4.setGeometry(10+w/2, 10+h/2, w/2, h/2) def PbImg2QPix(self,image:pb.Image): img1=image w=img1.width h=img1.height c=img1.channel btarry=bytearray(img1.data) npdata = np.frombuffer(btarry, dtype=np.uint8) if c==3: npdata = npdata.reshape([h,w,c]) qimg=QImage(npdata.data,w,h,QImage.Format_RGB888) if c==1: npdata = npdata.reshape([h,w]) qimg=QImage(npdata.data,w,h,QImage.Format_Grayscale8) pix=QPixmap(qimg) return pix def PbCloud2Pts(self,cloud:pb.PointCloud): size=cloud.size btarry=bytearray(cloud.data) npdata = np.frombuffer(btarry, dtype=np.short) points= npdata.reshape([size,4])/32750.0 *5.0 return points def DisplayFrame(self,frame : pb.ResFrame): if frame.HasField("images"): self.DisplayImage(frame.images) if frame.HasField("clouds"): self.DisplayCloud(frame.clouds) def DisplayImage(self,images:pb.ResImage): if images.HasField("img1"): self.panel1.ShowImg(self.PbImg2QPix(images.img1)) if images.HasField("img2"): self.panel2.ShowImg(self.PbImg2QPix(images.img2)) if images.HasField("img3"): self.panel3.ShowImg(self.PbImg2QPix(images.img3)) if images.HasField("img4"): self.panel4.ShowImg(self.PbImg2QPix(images.img4)) def DisplayCloud(self,clouds:pb.ResCloud): if clouds.HasField("cloud1"): self.pcViewer1.displayCloud(self.PbCloud2Pts(clouds.cloud1)) if clouds.HasField("cloud2"): self.pcViewer2.displayCloud(self.PbCloud2Pts(clouds.cloud2)) if clouds.HasField("cloud3"): self.pcViewer3.displayCloud(self.PbCloud2Pts(clouds.cloud3)) if clouds.HasField("cloud4"): self.pcViewer4.displayCloud(self.PbCloud2Pts(clouds.cloud4))