vtk显示点云

Viewer/CustomFrame.py

@@ -1,13 +1,15 @@
 
-from PyQt5.QtWidgets import QTabWidget,QSplitter, QLineEdit,QCheckBox,QLabel,QFrame,QPushButton,QMenu,QAction
-from PyQt5.QtGui import QPixmap,QImage,QPainter,QResizeEvent,QCloseEvent,QPaintEvent,QFont
+from PyQt5.QtWidgets import (QWidget,QVBoxLayout,QTabWidget,QSplitter, QFileDialog,
+                             QLineEdit,QCheckBox,QLabel,QFrame,QPushButton,QMenu,QAction)
+from PyQt5.QtGui import QPixmap,QImage,QPainter,QResizeEvent,QCloseEvent,QPaintEvent
 from PyQt5.QtCore import QSize,QTimer,QRect,Qt
 import numpy as np
 import def_pb2 as pb
 from concurrent.futures import ThreadPoolExecutor, as_completed, wait
 import GrpcClient as rpc
-from PCViewer import PCLViewer
-
+import vtk
+from vtk.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor
+import threading
 class ControlFrame(QFrame):
     def __init__(self):
         QFrame.__init__(self)
@@ -163,20 +165,133 @@ class ImageViewer(QLabel):
                 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 displayPCL(self,cloud:np.array):
+        colors = vtk.vtkUnsignedCharArray()
+        colors.SetNumberOfComponents(3)
+        colors.SetName("Colors")
+
+        self._point_cloud.clearPoints()
+        for point in cloud:
+            self._point_cloud.addPoint(point)
+            if point[-1]>0.85:
+                colors.InsertNextTypedTuple((0,255,0))
+            else:
+                colors.InsertNextTypedTuple((255,255,255))
+        self._point_cloud.vtkPolyData.GetPointData().SetScalars(colors)
+        self._vtk_widget.update()
+        #self._vtk_widget.GetRenderWindow().Render()
+    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=PCLViewer()
+        self.pclViewer=VtkPointCloudCanvas(self)
         self.addWidget(self.pclViewer)
         self.setStretchFactor(0,4)
     def save(self):
-        print("save")
+        fileName2, ok2 = QFileDialog.getSaveFileName(self, "文件保存", "C:/","All Files (*);;Text Files (*.txt)")
+        print(fileName2 ,ok2)
+        with open(fileName2,"w+") as f:
+            for point in self.pointCloud:
+                if point[3]>0.85:
+                    f.write("%f %f %f 0 255 0\n"%(point[0],point[1],point[2]))
+                else:
+                    f.write("%f %f %f 255 255 255\n"%(point[0],point[1],point[2]))
+        print("Save Cloud to %s"%(fileName2))
+
     def displayCloud(self,points:np.array):
+        self.pointCloud=points
         self.pclViewer.displayPCL(points)
     def resetViewer(self):
         self.pclViewer.resetViewer()
@@ -188,9 +303,9 @@ class PointCLViwer(QSplitter):
         updata_act.triggered.connect(self.updataData)
         menu.addAction(updata_act)
 
-        act=QAction("还原视野")
-        act.triggered.connect(self.resetViewer)
-        menu.addAction(act)
+        #act=QAction("还原视野")
+        #act.triggered.connect(self.resetViewer)
+        #menu.addAction(act)
 
         actSave=QAction("保存")
         actSave.triggered.connect(self.save)

Viewer/PCViewer.py

@@ -1,385 +0,0 @@
-from OpenGL import GLUT
-# -*-coding: utf-8 -*-
-#===============================================================================
-
-from PyQt5 import QtCore, QtGui, QtOpenGL
-import math
-import numpy
-import numpy.linalg as linalg
-import OpenGL
-OpenGL.ERROR_CHECKING = True
-from OpenGL.GL import *
-from OpenGL.GLU import *
-
-class PyGLWidget(QtOpenGL.QGLWidget):
-
-    # Qt signals
-    signalGLMatrixChanged = QtCore.pyqtSignal()
-    rotationBeginEvent = QtCore.pyqtSignal()
-    rotationEndEvent = QtCore.pyqtSignal()
-
-    def __init__(self, parent = None):
-        format = QtOpenGL.QGLFormat()
-        format.setSampleBuffers(True)
-        QtOpenGL.QGLWidget.__init__(self, format, parent)
-        self.setCursor(QtCore.Qt.OpenHandCursor)
-        self.setMouseTracking(True)
-
-        self.modelview_matrix_  = [
-            [1,0,0,0],
-            [0,1,0,0],
-            [0,0,1,0],
-            [-10.53889084,-2.01535511,-25.95999146,1. ]]
-        self.translate_vector_  = [0, 0, 0.0]
-        self.viewport_matrix_   = []
-        self.projection_matrix_ = []
-        self.near_   = 0.1
-        self.far_    = 100.0
-        self.fovy_   = 45.0
-        self.radius_ = 10.0
-        self.last_point_2D_ = QtCore.QPoint()
-        self.last_point_ok_ = False
-        self.last_point_3D_ = [1.0, 0.0, 0.0]
-        self.isInRotation_  = False
-
-
-
-        # connections
-        #self.signalGLMatrixChanged.connect(self.printModelViewMatrix)
-
-    @QtCore.pyqtSlot()
-    def printModelViewMatrix(self):
-        print (self.modelview_matrix_)
-
-    def initializeGL(self):
-        # OpenGL state
-        glClearColor(0.3, 0.3, 0.3, 0.5)
-        glEnable(GL_DEPTH_TEST)
-        self.reset_view()
-        self.translate([0,0,-5])
-
-    def resizeGL(self, width, height):
-        glViewport(0, 0, width, height );
-        self.set_projection( self.near_, self.far_, self.fovy_ );
-        self.updateGL()
-
-    def paintGL(self):
-        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
-
-        glMatrixMode(GL_MODELVIEW)
-        glLoadMatrixd(self.modelview_matrix_)
-        #self.printModelViewMatrix()
-
-    def set_projection(self, _near, _far, _fovy):
-        self.near_ = _near
-        self.far_ = _far
-        self.fovy_ = _fovy
-        self.makeCurrent()
-        glMatrixMode( GL_PROJECTION )
-        glLoadIdentity()
-        gluPerspective( self.fovy_, float(self.width()) / float(self.height()),
-                        self.near_, self.far_ )
-        self.updateGL()
-
-    def set_center(self, _cog):
-        self.center_ = _cog
-        self.view_all()
-
-    def set_radius(self, _radius):
-        self.radius_ = _radius
-        self.set_projection(_radius / 100.0, _radius * 100.0, self.fovy_)
-        self.reset_view()
-        self.translate([0, 0, -_radius * 2.0])
-        self.view_all()
-        self.updateGL()
-
-    def reset_view(self):
-        # scene pos and size
-        glMatrixMode( GL_MODELVIEW )
-        glLoadIdentity();
-        self.modelview_matrix_ = glGetDoublev( GL_MODELVIEW_MATRIX )
-        self.set_center([0.0, 0.0, 0.0])
-
-
-    def reset_rotation(self):
-        self.modelview_matrix_[0] = [1.0, 0.0, 0.0, 0.0]
-        self.modelview_matrix_[1] = [0.0, 1.0, 0.0, 0.0]
-        self.modelview_matrix_[2] = [0.0, 0.0, 1.0, 0.0]
-        glMatrixMode(GL_MODELVIEW)
-        glLoadMatrixd(self.modelview_matrix_)
-        self.updateGL()
-
-    def translate(self, _trans):
-        # Translate the object by _trans
-        # Update modelview_matrix_
-        self.makeCurrent()
-        glMatrixMode(GL_MODELVIEW)
-        glLoadIdentity()
-        glTranslated(_trans[0], _trans[1], _trans[2])
-        glMultMatrixd(self.modelview_matrix_)
-        self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
-        self.translate_vector_[0] = self.modelview_matrix_[3][0]
-        self.translate_vector_[1] = self.modelview_matrix_[3][1]
-        self.translate_vector_[2] = self.modelview_matrix_[3][2]
-        self.signalGLMatrixChanged.emit()
-
-    def rotate(self, _axis, _angle):
-        t = [self.modelview_matrix_[0][0] * self.center_[0] +
-             self.modelview_matrix_[1][0] * self.center_[1] +
-             self.modelview_matrix_[2][0] * self.center_[2] +
-             self.modelview_matrix_[3][0],
-             self.modelview_matrix_[0][1] * self.center_[0] +
-             self.modelview_matrix_[1][1] * self.center_[1] +
-             self.modelview_matrix_[2][1] * self.center_[2] +
-             self.modelview_matrix_[3][1],
-             self.modelview_matrix_[0][2] * self.center_[0] +
-             self.modelview_matrix_[1][2] * self.center_[1] +
-             self.modelview_matrix_[2][2] * self.center_[2] +
-             self.modelview_matrix_[3][2]]
-
-        self.makeCurrent()
-        glLoadIdentity()
-        glTranslatef(t[0], t[1], t[2])
-        glRotated(_angle, _axis[0], _axis[1], _axis[2])
-        glTranslatef(-t[0], -t[1], -t[2])
-        glMultMatrixd(self.modelview_matrix_)
-        self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
-        self.signalGLMatrixChanged.emit()
-
-    def view_all(self):
-        self.translate( [ -( self.modelview_matrix_[0][0] * self.center_[0] +
-                             self.modelview_matrix_[0][1] * self.center_[1] +
-                             self.modelview_matrix_[0][2] * self.center_[2] +
-                             self.modelview_matrix_[0][3]),
-                          -( self.modelview_matrix_[1][0] * self.center_[0] +
-                             self.modelview_matrix_[1][1] * self.center_[1] +
-                             self.modelview_matrix_[1][2] * self.center_[2] +
-                             self.modelview_matrix_[1][3]),
-                          -( self.modelview_matrix_[2][0] * self.center_[0] +
-                             self.modelview_matrix_[2][1] * self.center_[1] +
-                             self.modelview_matrix_[2][2] * self.center_[2] +
-                             self.modelview_matrix_[2][3] +
-                             self.radius_ / 2.0 )])
-
-    def map_to_sphere(self, _v2D):
-        _v3D = [0.0, 0.0, 0.0]
-        # inside Widget?
-        if (( _v2D.x() >= 0 ) and ( _v2D.x() <= self.width() ) and
-                ( _v2D.y() >= 0 ) and ( _v2D.y() <= self.height() ) ):
-            # map Qt Coordinates to the centered unit square [-0.5..0.5]x[-0.5..0.5]
-            x  = float( _v2D.x() - 0.5 * self.width())  / self.width()
-            y  = float( 0.5 * self.height() - _v2D.y()) / self.height()
-
-            _v3D[0] = x;
-            _v3D[1] = y;
-            # use Pythagoras to comp z-coord (the sphere has radius sqrt(2.0*0.5*0.5))
-            z2 = 2.0*0.5*0.5-x*x-y*y;
-            # numerical robust sqrt
-            _v3D[2] = math.sqrt(max( z2, 0.0 ))
-
-            # normalize direction to unit sphere
-            n = linalg.norm(_v3D)
-            _v3D = numpy.array(_v3D) / n
-
-            return True, _v3D
-        else:
-            return False, _v3D
-
-    def wheelEvent(self, _event):
-        # Use the mouse wheel to zoom in/out
-
-        d =  (_event.angleDelta().y()) / 1000.0 * self.radius_
-
-        self.translate([0.0, 0.0, d])
-        self.updateGL()
-        _event.accept()
-
-    def mousePressEvent(self, _event):
-        self.last_point_2D_ = _event.pos()
-        self.last_point_ok_, self.last_point_3D_ = self.map_to_sphere(self.last_point_2D_)
-
-    def mouseMoveEvent(self, _event):
-        newPoint2D = _event.pos()
-
-        if ((newPoint2D.x() < 0) or (newPoint2D.x() > self.width()) or
-                (newPoint2D.y() < 0) or (newPoint2D.y() > self.height())):
-            return
-
-        # Left button: rotate around center_
-        # Middle button: translate object
-        # Left & middle button: zoom in/out
-
-        value_y = 0
-        newPoint_hitSphere, newPoint3D = self.map_to_sphere(newPoint2D)
-
-        dx = float(newPoint2D.x() - self.last_point_2D_.x())
-        dy = float(newPoint2D.y() - self.last_point_2D_.y())
-
-        w  = float(self.width())
-        h  = float(self.height())
-
-        # enable GL context
-        self.makeCurrent()
-
-        # move in z direction
-        if (((_event.buttons() & QtCore.Qt.LeftButton) and (_event.buttons() & QtCore.Qt.MidButton))
-                or (_event.buttons() & QtCore.Qt.LeftButton and _event.modifiers() & QtCore.Qt.ControlModifier)):
-            value_y = self.radius_ * dy * 2.0 / h;
-            self.translate([0.0, 0.0, value_y])
-
-        #rotate
-        elif (_event.buttons() & QtCore.Qt.LeftButton):
-            #左键  旋转
-            if (not self.isInRotation_):
-                self.isInRotation_ = True
-                self.rotationBeginEvent.emit()
-
-            axis = [0.0, 0.0, 0.0]
-            angle = 0.0
-
-            if (self.last_point_ok_ and newPoint_hitSphere):
-                axis = numpy.cross(self.last_point_3D_, newPoint3D)
-                cos_angle = numpy.dot(self.last_point_3D_, newPoint3D)
-                if (abs(cos_angle) < 1.0):
-                    angle = math.acos(cos_angle) * 180.0 / math.pi
-                    angle *= 2.0
-                self.rotate(axis, angle)
-
-        #  move in x,y direction
-        elif (_event.buttons() & QtCore.Qt.MidButton
-              or (_event.buttons() & QtCore.Qt.LeftButton and _event.modifiers() & QtCore.Qt.ShiftModifier)):
-            #中间键移动
-
-            z = - (self.modelview_matrix_[0][2] * self.center_[0] +
-                   self.modelview_matrix_[1][2] * self.center_[1] +
-                   self.modelview_matrix_[2][2] * self.center_[2] +
-                   self.modelview_matrix_[3][2]) / (self.modelview_matrix_[0][3] * self.center_[0] +
-                                                    self.modelview_matrix_[1][3] * self.center_[1] +
-                                                    self.modelview_matrix_[2][3] * self.center_[2] +
-                                                    self.modelview_matrix_[3][3])
-
-            fovy   = 45.0
-            aspect = w / h
-            n      = 0.01 * self.radius_
-            up     = math.tan(fovy / 2.0 * math.pi / 180.0) * n
-            right  = aspect * up
-
-            self.translate( [2.0 * dx / w * right / n * z,
-                             -2.0 * dy / h * up / n * z,
-                             0.0] )
-        # remember this point
-        self.last_point_2D_ = newPoint2D
-        self.last_point_3D_ = newPoint3D
-        self.last_point_ok_ = newPoint_hitSphere
-
-        # trigger redraw
-        self.updateGL()
-
-    def mouseReleaseEvent(self, _event):
-        if (self.isInRotation_):
-            self.isInRotation_ = False
-            self.rotationEndEvent.emit()
-        last_point_ok_ = False
-
-#===============================================================================
-
-
-class PCLViewer(PyGLWidget):
-    def __init__(self):
-        PyGLWidget.__init__(self)
-        GLUT.glutInit()
-        self.points=[]
-    def resetViewer(self):
-        self.reset_view()
-        self.translate([0,0,-5])
-        self.update()
-    def displayPCL(self,points):
-        self.points=points
-        self.update()
-    def drawTrajWithPoint(self, poses, ptsize, color):
-        glPointSize(ptsize)
-        glDepthMask(GL_FALSE)
-        glBegin(GL_POINTS)
-        glColor3f(color[0], color[1], color[2])
-        for pt in poses:
-            [x, y, _] = pt
-            glVertex2d(x, y)
-        glEnd()
-
-    @staticmethod
-    def RotatePoint(point, yaw):
-        [x, y] = point
-        nx = math.cos(yaw) * x - y * math.sin(yaw)
-        ny = x * math.sin(yaw) + y * math.cos(yaw)
-        return [nx, ny]
-    @staticmethod
-    def Transform(point,yaw,dt=None):
-        [x,y]=point
-        nx = math.cos(yaw) * x - y * math.sin(yaw)
-        ny = x * math.sin(yaw) + y * math.cos(yaw)
-        if dt is not None:
-            [dx,dy]=dt
-            nx+=dx
-            ny+=dy
-        return [nx, ny]
-    def DrawText(self,pt,text,width,size,rgb):
-        glDisable(GL_TEXTURE_2D)
-        glLineWidth(width)
-        glPointSize(width)
-        r, g, b = rgb
-        glPushMatrix()
-        glColor3f(r, g, b)
-        glTranslatef(pt[0],pt[1],0)
-        s = size*0.005
-        glScalef(s, s, s)
-        for char in text:
-            GLUT.glutStrokeCharacter(GLUT.GLUT_STROKE_ROMAN, ord(char))
-        glPopMatrix()
-        glEnable(GL_TEXTURE_2D)
-
-    def drawAxis(self, len, width):
-
-        glDepthMask(GL_FALSE)
-        glColor3f(1, 0, 0)
-        glLineWidth(width)
-        glBegin(GL_LINES)
-        glVertex3f(0, 0,0)
-        glVertex3f(len, 0,0)
-        glEnd()
-
-        glColor3f(0, 1, 0)
-        glBegin(GL_LINES)
-        glVertex3f(0, 0,0)
-        glVertex3f(0, len,0)
-        glEnd()
-
-        glColor3f(0, 0, 1)
-        glBegin(GL_LINES)
-        glVertex3f(0, 0,0)
-        glVertex3f(0, 0,len)
-        glEnd()
-
-    def drawPCL(self,points):
-        if len(points)==0:
-            return
-        glPointSize(0.5)
-        glBegin(GL_POINTS)
-
-        for pt in points:
-            [x, y, z,p] = pt
-            if p<0.85:
-                glColor3f(1, 1, 1)
-            else:
-                glColor3f(0, 1, 0)
-            glVertex3f(x, y,z)
-        glEnd()
-
-    def paintGL(self):
-        PyGLWidget.paintGL(self)
-
-        self.drawPCL(self.points)
-
-        self.drawAxis( 1, 2)
-
-
-