q3dviewer 1.1.0__py3-none-any.whl → 1.1.2__py3-none-any.whl

q3dviewer/base_glwidget.py

@@ -195,33 +195,42 @@ class BaseGLWidget(QtOpenGLWidgets.QOpenGLWidget):
         # Handle rotation keys
         if QtCore.Qt.Key_Up in self.active_keys:
             self.rotate(radians(rot_speed), 0, 0)
+            self.view_need_update = True
         if QtCore.Qt.Key_Down in self.active_keys:
             self.rotate(radians(-rot_speed), 0, 0)
+            self.view_need_update = True
         if QtCore.Qt.Key_Left in self.active_keys:
             self.rotate(0, 0, radians(rot_speed))
+            self.view_need_update = True
         if QtCore.Qt.Key_Right in self.active_keys:
             self.rotate(0, 0, radians(-rot_speed))
+            self.view_need_update = True
         # Handle zoom keys
         xz_keys = {QtCore.Qt.Key_Z, QtCore.Qt.Key_X}
         if self.active_keys & xz_keys:
             Rwc = euler_to_matrix(self.euler)
             if QtCore.Qt.Key_Z in self.active_keys:
                 self.center += Rwc @ np.array([0, 0, -trans_speed])
+                self.view_need_update = True
             if QtCore.Qt.Key_X in self.active_keys:
                 self.center += Rwc @ np.array([0, 0, trans_speed])
+                self.view_need_update = True
         # Handle translation keys on the z plane
         dir_keys = {QtCore.Qt.Key_W, QtCore.Qt.Key_S, QtCore.Qt.Key_A, QtCore.Qt.Key_D}
         if self.active_keys & dir_keys:
             Rz = euler_to_matrix([0, 0, self.euler[2]])
             if QtCore.Qt.Key_W in self.active_keys:
                 self.center += Rz @ np.array([0, trans_speed, 0])
+                self.view_need_update = True
             if QtCore.Qt.Key_S in self.active_keys:
                 self.center += Rz @ np.array([0, -trans_speed, 0])
+                self.view_need_update = True
             if QtCore.Qt.Key_A in self.active_keys:
                 self.center += Rz @ np.array([-trans_speed, 0, 0])
+                self.view_need_update = True
             if QtCore.Qt.Key_D in self.active_keys:
                 self.center += Rz @ np.array([trans_speed, 0, 0])
-        self.view_need_update = True
+                self.view_need_update = True
 
     def update_model_view(self):
         glMatrixMode(GL_MODELVIEW)

q3dviewer/basic_window.py

@@ -0,0 +1,228 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import pyqtgraph.opengl as gl
+from pyqtgraph.Qt import QtCore
+from PyQt5.QtWidgets import QWidget, QComboBox, QVBoxLayout, QHBoxLayout, QSizePolicy,\
+      QSpacerItem, QMainWindow
+from OpenGL.GL import *
+from PyQt5.QtGui import QKeyEvent, QVector3D
+from PyQt5.QtWidgets import QApplication, QWidget
+import numpy as np
+import signal
+import sys
+from PyQt5.QtWidgets import QLabel, QLineEdit, QDoubleSpinBox, QSpinBox
+
+
+class SettingWindow(QWidget):
+    def __init__(self):
+        super().__init__()
+        self.combo_items = QComboBox()
+        self.combo_items.currentIndexChanged.connect(self.onComboboxSelection)
+        main_layout = QVBoxLayout()
+        self.stretch = QSpacerItem(
+            10, 10, QSizePolicy.Minimum, QSizePolicy.Expanding)
+        main_layout.addWidget(self.combo_items)
+        self.layout = QVBoxLayout()
+        self.layout.addItem(self.stretch)
+        main_layout.addLayout(self.layout)
+        self.setLayout(main_layout)
+        self.setWindowTitle("Setting Window")
+        self.setGeometry(200, 200, 300, 200)
+        self.items = {}
+
+    def addSetting(self, name, item):
+        self.items.update({name: item})
+        self.combo_items.addItem("%s(%s)" % (name, item.__class__.__name__))
+
+    def clearSetting(self):
+        while self.layout.count():
+            child = self.layout.takeAt(0)
+            if child.widget():
+                child.widget().deleteLater()
+
+    def onComboboxSelection(self, index):
+        self.layout.removeItem(self.stretch)
+        # remove all setting of previous widget
+        self.clearSetting()
+
+        key = list(self.items.keys())
+        try:
+            item = self.items[key[index]]
+            item.addSetting(self.layout)
+            self.layout.addItem(self.stretch)
+        except AttributeError:
+            print("%s: No setting." % (item.__class__.__name__))
+
+
+class ViewWidget(gl.GLViewWidget):
+    def __init__(self):
+        self.followed_name = 'none'
+        self.named_items = {}
+        self.color = '#000000'
+        self.followable_item_name = ['none']
+        self.setting_window = SettingWindow()
+        super(ViewWidget, self).__init__()
+
+    def onFollowableSelection(self, index):
+        self.followed_name = self.followable_item_name[index]
+
+    def update(self):
+        if self.followed_name != 'none':
+            pos = self.named_items[self.followed_name].T[:3, 3]
+            self.opts['center'] = QVector3D(pos[0], pos[1], pos[2])
+        super().update()
+
+    def addSetting(self, layout):
+        label1 = QLabel("Set background color:")
+        label1.setToolTip("using '#xxxxxx', i.e. #FF4500")
+        box1 = QLineEdit()
+        box1.setToolTip("'using '#xxxxxx', i.e. #FF4500")
+        box1.setText(str(self.color))
+        box1.textChanged.connect(self.setBKColor)
+        layout.addWidget(label1)
+        layout.addWidget(box1)
+        label2 = QLabel("Set Focus:")
+        combo2 = QComboBox()
+        for name in self.followable_item_name:
+            combo2.addItem(name)
+        combo2.currentIndexChanged.connect(self.onFollowableSelection)
+        layout.addWidget(label2)
+        layout.addWidget(combo2)
+
+    def setBKColor(self, color):
+        if (type(color) != str):
+            return
+        if color.startswith("#"):
+            try:
+                self.setBackgroundColor(color)
+                self.color = color
+            except ValueError:
+                return
+
+    def addItem(self, name, item):
+        self.named_items.update({name: item})
+        if (item.__class__.__name__ == 'GLAxisItem'):
+            self.followable_item_name.append(name)
+        self.setting_window.addSetting(name, item)
+        super().addItem(item)
+
+    def mouseReleaseEvent(self, ev):
+        if hasattr(self, 'mousePos'):
+            delattr(self, 'mousePos')
+
+    def mouseMoveEvent(self, ev):
+        lpos = ev.localPos()
+        if not hasattr(self, 'mousePos'):
+            self.mousePos = lpos
+        diff = lpos - self.mousePos
+        self.mousePos = lpos
+        if ev.buttons() == QtCore.Qt.MouseButton.RightButton:
+            self.orbit(-diff.x(), diff.y())
+        elif ev.buttons() == QtCore.Qt.MouseButton.LeftButton:
+            pitch_abs = np.abs(self.opts['elevation'])
+            camera_mode = 'view-upright'
+            if(pitch_abs <= 45.0 or pitch_abs == 90):
+                camera_mode = 'view'
+            self.pan(diff.x(), diff.y(), 0, relative=camera_mode)
+
+    def keyPressEvent(self, ev: QKeyEvent):
+        step = 10
+        zoom_delta = 20
+        speed = 2
+        self.projectionMatrix().data()
+
+        pitch_abs = np.abs(self.opts['elevation'])
+        camera_mode = 'view-upright'
+        if(pitch_abs <= 45.0 or pitch_abs == 90):
+            camera_mode = 'view'
+
+        if ev.key() == QtCore.Qt.Key_M:  # setting meun
+            print("Open setting windows")
+            self.openSettingWindow()
+        elif ev.key() == QtCore.Qt.Key_R:
+            print("Clear viewer")
+            for item in self.named_items.values():
+                try:
+                    item.clear()
+                except:
+                    pass
+        elif ev.key() == QtCore.Qt.Key_Up:
+            if ev.modifiers() & QtCore.Qt.KeyboardModifier.ControlModifier:
+                self.pan(0, +step, 0, relative=camera_mode)
+            else:
+                self.orbit(azim=0, elev=-speed)
+        elif ev.key() == QtCore.Qt.Key_Down:
+            if ev.modifiers() & QtCore.Qt.KeyboardModifier.ControlModifier:
+                self.pan(0, -step, 0, relative=camera_mode)
+            else:
+                self.orbit(azim=0, elev=speed)
+        elif ev.key() == QtCore.Qt.Key_Left:
+            if ev.modifiers() & QtCore.Qt.KeyboardModifier.ControlModifier:
+                self.pan(+step, 0, 0, relative=camera_mode)
+            else:
+                self.orbit(azim=speed, elev=0)
+
+        elif ev.key() == QtCore.Qt.Key_Right:
+            if ev.modifiers() & QtCore.Qt.KeyboardModifier.ControlModifier:
+                self.pan(-step, 0, 0, relative=camera_mode)
+            else:
+                self.orbit(azim=-speed, elev=0)
+
+        elif ev.key() == QtCore.Qt.Key_Z:
+            self.opts['distance'] *= 0.999**(+zoom_delta)
+        elif ev.key() == QtCore.Qt.Key_X:
+            self.opts['distance'] *= 0.999**(-zoom_delta)
+        else:
+            super().keyPressEvent(ev)
+
+    def openSettingWindow(self):
+        if self.setting_window.isVisible():
+            self.setting_window.raise_()
+
+        else:
+            self.setting_window.show()
+
+
+class Viewer(QMainWindow):
+    def __init__(self, name='Viewer', win_size=[1920, 1080], vw=ViewWidget):
+        signal.signal(signal.SIGINT, signal.SIG_DFL)
+        super(Viewer, self).__init__()
+        self.vw = vw
+        self.setGeometry(0, 0, win_size[0], win_size[1])
+        self.initUI()
+        self.setWindowTitle(name)
+
+    def initUI(self):
+        centerWidget = QWidget()
+        self.setCentralWidget(centerWidget)
+        layout = QVBoxLayout()
+        centerWidget.setLayout(layout)
+        self.viewerWidget = self.vw()
+        layout.addWidget(self.viewerWidget, 1)
+        timer = QtCore.QTimer(self)
+        timer.setInterval(20)  # period, in milliseconds
+        timer.timeout.connect(self.update)
+        self.viewerWidget.setCameraPosition(distance=40)
+        timer.start()
+
+    def addItems(self, named_items: dict):
+        for name, item in named_items.items():
+            self.viewerWidget.addItem(name, item)
+
+    def __getitem__(self, name: str):
+        if name in self.viewerWidget.named_items:
+            return self.viewerWidget.named_items[name]
+        else:
+            return None
+
+    def update(self):
+        # force update by timer
+        self.viewerWidget.update()
+
+    def closeEvent(self, _):
+        sys.exit(0)
+
+    def show(self):
+        self.viewerWidget.setting_window.addSetting("main win", self.viewerWidget)
+        super().show()

q3dviewer/cloud_viewer.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+import numpy as np
+from q3dviewer.custom_items import *
+from q3dviewer.basic_window import *
+from pypcd4 import PointCloud
+
+class CloudViewer(Viewer):
+    def __init__(self):
+        super(CloudViewer, self).__init__(name="Cloud Viewer")
+        self.setAcceptDrops(True)
+
+    def dragEnterEvent(self, event):
+        if event.mimeData().hasUrls():
+            event.accept()
+        else:
+            event.ignore()
+
+    def dropEvent(self, event):
+        for url in event.mimeData().urls():
+            file_path = url.toLocalFile()
+            self.openCloudFile(file_path)
+
+    def openCloudFile(self, file):
+        cloud_item = self['cloud']
+        if cloud_item is None:
+            print("Can't find clouditem.")
+            return
+        if file.endswith('.pcd'):
+            pc = PointCloud.from_path(file).pc_data
+            if 'rgb' in pc.dtype.names:
+                color = pc["rgb"].astype(np.uint32)
+                cloud_item.setColorMode('RGB')
+            elif 'intensity' in pc.dtype.names:
+                color = pc["intensity"].astype(np.uint32)
+                cloud_item.setColorMode('I')
+            else:
+                color = pc['z'].astype(np.uint32)
+                cloud_item.setColorMode('#FFFFFF')
+            cloud = np.rec.fromarrays(
+                [np.stack([pc["x"], pc["y"], pc["z"]], axis=1), color],
+                dtype=cloud_item.data_type)
+        elif file.endswith('.npy'):
+            pc = np.load(file)
+            cloud = np.rec.fromarrays(
+                [np.stack([pc[:, 0], pc[:, 1], pc[:, 2]], axis=1), pc[:, 3].astype(np.uint32)],
+                dtype=cloud_item.data_type)
+        cloud_item.setData(data=cloud)
+
+
+def main():
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--pcd", help="the pcd path")
+    args = parser.parse_args()
+    app = QApplication([])
+    viewer = CloudViewer()
+    cloud_item = CloudIOItem(size=1, alpha=0.1)
+    axis_item = GLAxisItem(size=0.5, width=5)
+    gird_item = GridItem(size=1000, spacing=20)
+    # viewer.viewerWidget.setBackgroundColor(255, 255, 255, 255)
+
+    viewer.addItems({'cloud': cloud_item, 'grid': gird_item, 'axis': axis_item})
+
+    if args.pcd:
+        pcd_fn = args.pcd
+        viewer.openCloudFile(pcd_fn)
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/custom_items/camera_frame_item.py

@@ -0,0 +1,173 @@
+import pyqtgraph.opengl as gl
+from OpenGL.GL import *
+import numpy as np
+from OpenGL.GL import shaders
+from PIL import Image
+
+
+# Vertex and Fragment shader source code
+vertex_shader_source = """
+#version 330 core
+layout(location = 0) in vec3 position;
+layout(location = 1) in vec2 texCoord;
+
+out vec2 TexCoord;
+
+uniform mat4 view_matrix;
+uniform mat4 project_matrix;
+
+void main()
+{
+    gl_Position = project_matrix * view_matrix * vec4(position, 1.0);
+    TexCoord = texCoord;
+}
+"""
+
+fragment_shader_source = """
+#version 330 core
+in vec2 TexCoord;
+out vec4 color;
+uniform sampler2D ourTexture;
+void main()
+{
+    color = texture(ourTexture, TexCoord);
+}
+"""
+
+
+def set_uniform_mat4(shader, content, name):
+    content = content.T
+    glUniformMatrix4fv(
+        glGetUniformLocation(shader, name),
+        1,
+        GL_FALSE,
+        content.astype(np.float32)
+    )
+
+
+class GLCameraFrameItem(gl.GLGraphicsItem.GLGraphicsItem):
+    def __init__(self, T=np.eye(4), size=1, width=3, path=None):
+        gl.GLGraphicsItem.GLGraphicsItem.__init__(self)
+        self.size = size
+        self.width = width
+        self.T = T
+        self.path = path
+
+    def initializeGL(self):
+        # Rectangle vertices and texture coordinates
+        hsize = self.size / 2
+        self.vertices = np.array([
+            # positions          # texture coords
+            [-hsize, -hsize,  0.0,  0.0, 0.0],  # bottom-left
+            [hsize, -hsize,  0.0,  1.0, 0.0],  # bottom-right
+            [hsize,  hsize,  0.0,  1.0, 1.0],  # top-right
+            [-hsize,  hsize,  0.0,  0.0, 1.0],   # top-left
+            [0,  0, -hsize * 0.66, 0.0, 0.0],   # top-left
+        ], dtype=np.float32)
+
+        R = self.T[:3, :3]
+        t = self.T[:3, 3]
+        self.vertices[:, :3] = (
+            R @ self.vertices[:, :3].T + t[:, np.newaxis]).T
+
+        self.focal_p = np.array([0, 0, hsize * 0.66])
+
+        indices = np.array([
+            0, 1, 2,  # first triangle
+            2, 3, 0   # second triangle
+        ], dtype=np.uint32)
+
+        self.vao = glGenVertexArrays(1)
+        vbo = glGenBuffers(1)
+        ebo = glGenBuffers(1)
+
+        glBindVertexArray(self.vao)
+
+        glBindBuffer(GL_ARRAY_BUFFER, vbo)
+        glBufferData(GL_ARRAY_BUFFER, self.vertices.itemsize *
+                     5 * 4, self.vertices, GL_STATIC_DRAW)
+
+        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo)
+        glBufferData(GL_ELEMENT_ARRAY_BUFFER,
+                     indices.nbytes, indices, GL_STATIC_DRAW)
+
+        # Vertex positions
+
+        glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE,
+                              20, ctypes.c_void_p(0))
+        glEnableVertexAttribArray(0)
+        # Texture coordinates
+        glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE,
+                              20, ctypes.c_void_p(12))
+        glEnableVertexAttribArray(1)
+
+        project_matrix = np.array(self._GLGraphicsItem__view.projectionMatrix().data(),
+                                  np.float32).reshape([4, 4]).T
+        # Compile shaders and create shader program
+        self.program = shaders.compileProgram(
+            shaders.compileShader(vertex_shader_source, GL_VERTEX_SHADER),
+            shaders.compileShader(fragment_shader_source, GL_FRAGMENT_SHADER),
+        )
+        glUseProgram(self.program)
+        set_uniform_mat4(self.program, project_matrix, 'project_matrix')
+        glUseProgram(0)
+
+        self.texture = glGenTextures(1)
+        glBindTexture(GL_TEXTURE_2D, self.texture)
+
+        # Load image
+        image = Image.open(self.path)
+        # image = image.transpose(Image.FLIP_TOP_BOTTOM)
+        img_data = image.convert("RGBA").tobytes()
+        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, image.width,
+                     image.height, 0, GL_RGBA, GL_UNSIGNED_BYTE, img_data)
+        glGenerateMipmap(GL_TEXTURE_2D)
+        glBindTexture(GL_TEXTURE_2D, 0)
+        glBindVertexArray(0)
+
+    def setTransform(self, T):
+        self.T = T
+
+    def paint(self):
+        self.view_matrix = np.array(
+            self._GLGraphicsItem__view.viewMatrix().data(), np.float32).reshape([4, 4]).T
+        project_matrix = np.array(self._GLGraphicsItem__view.projectionMatrix(
+        ).data(), np.float32).reshape([4, 4]).T
+
+        glEnable(GL_DEPTH_TEST)
+        glEnable(GL_BLEND)
+        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
+
+        glUseProgram(self.program)
+        set_uniform_mat4(self.program, self.view_matrix, 'view_matrix')
+        set_uniform_mat4(self.program, project_matrix, 'project_matrix')
+        glBindVertexArray(self.vao)
+        glBindTexture(GL_TEXTURE_2D, self.texture)
+        glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, None)
+        glBindTexture(GL_TEXTURE_2D, 0)
+        glBindVertexArray(0)
+        glUseProgram(0)
+
+        glLineWidth(self.width)
+        glBegin(GL_LINES)
+        glColor4f(1, 1, 1, 1)  # z is blue
+        glVertex3f(*self.vertices[0, :3])
+        glVertex3f(*self.vertices[1, :3])
+        glVertex3f(*self.vertices[1, :3])
+        glVertex3f(*self.vertices[2, :3])
+        glVertex3f(*self.vertices[2, :3])
+        glVertex3f(*self.vertices[3, :3])
+        glVertex3f(*self.vertices[3, :3])
+        glVertex3f(*self.vertices[0, :3])
+        glVertex3f(*self.vertices[4, :3])
+        glVertex3f(*self.vertices[0, :3])
+        glVertex3f(*self.vertices[4, :3])
+        glVertex3f(*self.vertices[1, :3])
+        glVertex3f(*self.vertices[4, :3])
+        glVertex3f(*self.vertices[2, :3])
+        glVertex3f(*self.vertices[4, :3])
+        glVertex3f(*self.vertices[3, :3])
+        glEnd()
+
+        glDisable(GL_DEPTH_TEST)
+        glDisable(GL_BLEND)

q3dviewer/custom_items/cloud_item.py

@@ -67,6 +67,7 @@ class CloudItem(BaseItem):
         self.box_size.setValue(self.size)
         self.box_size.setRange(0, 100)
         self.box_size.valueChanged.connect(self.set_size)
+        self._on_point_type_selection(self.point_type_table[self.point_type])
         layout.addWidget(self.box_size)
 
         box_alpha = QDoubleSpinBox()

q3dviewer/custom_items/trajectory_item.py

@@ -0,0 +1,79 @@
+import pyqtgraph.opengl as gl
+from OpenGL.GL import *
+import numpy as np
+import threading
+
+
+class TrajectoryItem(gl.GLGridItem):
+    def __init__(self, width=1, color=(0, 1, 0, 1)):
+        super(TrajectoryItem, self).__init__()
+        self.width = width
+        self.buff = np.empty((0, 3), np.float32)
+        self.wait_add_data = None
+        self.mutex = threading.Lock()
+        self.CAPACITY = 100000
+        self.valid_buff_top = 0
+        self.color = color
+
+    def addSetting(self, layout):
+        pass
+
+    def setData(self, data, append=True):
+        self.mutex.acquire()
+        data = data.astype(np.float32).reshape(-1, 3)
+        if (append is False):
+            self.wait_add_data = data
+            self.add_buff_loc = 0
+        else:
+            if (self.wait_add_data is None):
+                self.wait_add_data = data
+            else:
+                self.wait_add_data = np.concatenate([self.wait_add_data, data])
+            self.add_buff_loc = self.valid_buff_top
+        self.mutex.release()
+
+
+    def updateRenderBuffer(self):
+        if(self.wait_add_data is None):
+            return
+        self.mutex.acquire()
+
+        new_buff_top = self.add_buff_loc + self.wait_add_data.shape[0]
+        if new_buff_top > self.buff.shape[0]:
+            buff_capacity = self.buff.shape[0]
+            while (new_buff_top > buff_capacity):
+                buff_capacity += self.CAPACITY
+            self.buff = np.empty((buff_capacity, 3), np.float32)
+            self.buff[self.add_buff_loc:new_buff_top] = self.wait_add_data
+            glBindBuffer(GL_ARRAY_BUFFER, self.vbo)
+            glBufferData(GL_ARRAY_BUFFER, self.buff.nbytes, self.buff, GL_DYNAMIC_DRAW)
+            glBindBuffer(GL_ARRAY_BUFFER, 0)
+        else:
+            self.buff[self.add_buff_loc:new_buff_top] = self.wait_add_data
+            glBindBuffer(GL_ARRAY_BUFFER, self.vbo)
+            glBufferSubData(GL_ARRAY_BUFFER, self.add_buff_loc * 12,
+                            self.wait_add_data.shape[0] * 12, self.wait_add_data)
+        self.valid_buff_top = new_buff_top
+        self.wait_add_data = None
+        self.mutex.release()
+
+    def initializeGL(self):
+        self.vbo = glGenBuffers(1)
+
+
+    def paint(self):
+        self.setupGLState()
+        self.updateRenderBuffer()
+        glEnable(GL_BLEND)
+        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
+        glBindBuffer(GL_ARRAY_BUFFER, self.vbo)
+        glEnableClientState(GL_VERTEX_ARRAY)
+        glVertexPointer(3, GL_FLOAT, 0, None)
+        glLineWidth(self.width)
+        glColor4f(*self.color)  # z is blue
+
+        glDrawArrays(GL_LINE_STRIP, 0, self.valid_buff_top)
+        glDisableClientState(GL_VERTEX_ARRAY)
+
+        glBindBuffer(GL_ARRAY_BUFFER, 0)
+        glUseProgram(0)