3dtof_viewer/Viewer/CustomFrame.py

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