q3dviewer 1.0.5__py3-none-any.whl → 1.0.7__py3-none-any.whl

q3dviewer/base_glwidget.py

@@ -123,6 +123,7 @@ class BaseGLWidget(QtOpenGLWidgets.QOpenGLWidget):
         self.show_center = True
 
     def paintGL(self):
+        pass
         self.update_model_projection()
         self.update_model_view()
         bgcolor = self.color
@@ -134,10 +135,12 @@ class BaseGLWidget(QtOpenGLWidgets.QOpenGLWidget):
             glMatrixMode(GL_MODELVIEW)
             glPushMatrix()
             glPushAttrib(GL_ALL_ATTRIB_BITS)
-            item.paint()
-            glPopAttrib()
-            glMatrixMode(GL_MODELVIEW)
-            glPopMatrix()
+            try:
+                item.paint()
+            finally:
+                glPopAttrib()
+                glMatrixMode(GL_MODELVIEW)
+                glPopMatrix()
         
         # Show center as a point if updated by mouse move event
         if self.enable_show_center and self.show_center:
@@ -193,12 +196,14 @@ class BaseGLWidget(QtOpenGLWidgets.QOpenGLWidget):
         glLoadMatrixf(m.T)
         
     def get_view_matrix(self):
-        twc = self.center
-        tcw = np.array([0, 0, self.dist])
+        two = self.center # the origin(center) in the world frame
+        tco = np.array([0, 0, self.dist]) # the origin(center) in camera frame
         Rwc = euler_to_matrix(self.euler)
-        twc = twc + Rwc @ tcw
-        Twc = makeT(Rwc, twc)
-        return np.linalg.inv(Twc)
+        twc = two + Rwc @ tco
+        Rcw = Rwc.T
+        tcw = -Rcw @ twc
+        Tcw = makeT(Rcw, tcw)
+        return Tcw
 
     def set_cam_position(self, **kwargs):
         pos = kwargs.get('pos', None)

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

@@ -201,17 +201,6 @@ class CloudItem(BaseItem):
                 self.wait_add_data = data
                 self.add_buff_loc = 0
 
-            while self.add_buff_loc + self.wait_add_data.shape[0] > self.max_cloud_size:
-                # Randomly select half of the points
-                print(
-                    "Warning: Buffer size exceeds the maximum cloud size. Randomly selecting half of the points.")
-                indices = np.random.choice(
-                    self.add_buff_loc, self.add_buff_loc // 2, replace=False)
-                self.buff[:self.add_buff_loc // 2] = self.buff[indices]
-                self.add_buff_loc //= 2
-                indices = np.random.choice(
-                    self.wait_add_data.shape[0], self.wait_add_data.shape[0] // 2, replace=False)
-                self.wait_add_data = self.wait_add_data[indices]
 
     def update_setting(self):
         if (self.need_update_setting is False):
@@ -245,6 +234,16 @@ class CloudItem(BaseItem):
             new_buff[:self.add_buff_loc] = self.buff[:self.add_buff_loc]
             new_buff[self.add_buff_loc:new_buff_top] = self.wait_add_data
             self.buff = new_buff
+
+            # if exceed the maximum cloud size, randomly select half of the points
+            while self.buff.shape[0] >= self.max_cloud_size:
+                print("[Cloud Item] Exceed maximum cloud size, reduce to half")
+                new_buff = np.empty((self.buff.shape[0] // 2), self.data_type)
+                indices = np.random.choice(new_buff_top, new_buff_top // 2, replace=False)
+                new_buff = self.buff[indices]
+                new_buff_top = new_buff_top // 2
+                self.buff = new_buff
+
             glBindBuffer(GL_ARRAY_BUFFER, self.vbo)
             glBufferData(GL_ARRAY_BUFFER, self.buff.nbytes,
                          self.buff, GL_DYNAMIC_DRAW)

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)

q3dviewer/gau_io.py

@@ -0,0 +1,168 @@
+import numpy as np
+from plyfile import PlyData
+
+
+def gsdata_type(sh_dim):
+    return [('pw', '<f4', (3,)),
+            ('rot', '<f4', (4,)),
+            ('scale', '<f4', (3,)),
+            ('alpha', '<f4'),
+            ('sh', '<f4', (sh_dim))]
+
+
+def matrix_to_quaternion(matrices):
+    m00, m01, m02 = matrices[:, 0, 0], matrices[:, 0, 1], matrices[:, 0, 2]
+    m10, m11, m12 = matrices[:, 1, 0], matrices[:, 1, 1], matrices[:, 1, 2]
+    m20, m21, m22 = matrices[:, 2, 0], matrices[:, 2, 1], matrices[:, 2, 2]
+    t = 1 + m00 + m11 + m22
+    s = np.ones_like(m00)
+    w = np.ones_like(m00)
+    x = np.ones_like(m00)
+    y = np.ones_like(m00)
+    z = np.ones_like(m00)
+
+    t_positive = t > 0.0000001
+    s[t_positive] = 0.5 / np.sqrt(t[t_positive])
+    w[t_positive] = 0.25 / s[t_positive]
+    x[t_positive] = (m21[t_positive] - m12[t_positive]) * s[t_positive]
+    y[t_positive] = (m02[t_positive] - m20[t_positive]) * s[t_positive]
+    z[t_positive] = (m10[t_positive] - m01[t_positive]) * s[t_positive]
+
+    c1 = np.logical_and(m00 > m11, m00 > m22)
+    cond1 = np.logical_and(np.logical_not(t_positive), np.logical_and(m00 > m11, m00 > m22))
+
+    s[cond1] = 2.0 * np.sqrt(1.0 + m00[cond1] - m11[cond1] - m22[cond1])
+    w[cond1] = (m21[cond1] - m12[cond1]) / s[cond1]
+    x[cond1] = 0.25 * s[cond1]
+    y[cond1] = (m01[cond1] + m10[cond1]) / s[cond1]
+    z[cond1] = (m02[cond1] + m20[cond1]) / s[cond1]
+
+    c2 = np.logical_and(np.logical_not(c1), m11 > m22)
+    cond2 = np.logical_and(np.logical_not(t_positive), c2)
+    s[cond2] = 2.0 * np.sqrt(1.0 + m11[cond2] - m00[cond2] - m22[cond2])
+    w[cond2] = (m02[cond2] - m20[cond2]) / s[cond2]
+    x[cond2] = (m01[cond2] + m10[cond2]) / s[cond2]
+    y[cond2] = 0.25 * s[cond2]
+    z[cond2] = (m12[cond2] + m21[cond2]) / s[cond2]
+
+    c3 = np.logical_and(np.logical_not(c1), np.logical_not(c2))
+    cond3 = np.logical_and(np.logical_not(t_positive), c3)
+    s[cond3] = 2.0 * np.sqrt(1.0 + m22[cond3] - m00[cond3] - m11[cond3])
+    w[cond3] = (m10[cond3] - m01[cond3]) / s[cond3]
+    x[cond3] = (m02[cond3] + m20[cond3]) / s[cond3]
+    y[cond3] = (m12[cond3] + m21[cond3]) / s[cond3]
+    z[cond3] = 0.25 * s[cond3]
+    return np.array([w, x, y, z]).T
+
+
+def load_ply(path, T=None):
+    plydata = PlyData.read(path)
+    pws = np.stack((np.asarray(plydata.elements[0]["x"]),
+                    np.asarray(plydata.elements[0]["y"]),
+                    np.asarray(plydata.elements[0]["z"])),  axis=1)
+
+    alphas = np.asarray(plydata.elements[0]["opacity"])
+    alphas = 1/(1 + np.exp(-alphas))
+
+    scales = np.stack((np.asarray(plydata.elements[0]["scale_0"]),
+                       np.asarray(plydata.elements[0]["scale_1"]),
+                       np.asarray(plydata.elements[0]["scale_2"])),  axis=1)
+
+    rots = np.stack((np.asarray(plydata.elements[0]["rot_0"]),
+                     np.asarray(plydata.elements[0]["rot_1"]),
+                     np.asarray(plydata.elements[0]["rot_2"]),
+                     np.asarray(plydata.elements[0]["rot_3"])),  axis=1)
+
+    rots /= np.linalg.norm(rots, axis=1)[:, np.newaxis]
+
+    sh_dim = len(plydata.elements[0][0])-14
+    shs = np.zeros([pws.shape[0], sh_dim])
+    shs[:, 0] = np.asarray(plydata.elements[0]["f_dc_0"])
+    shs[:, 1] = np.asarray(plydata.elements[0]["f_dc_1"])
+    shs[:, 2] = np.asarray(plydata.elements[0]["f_dc_2"])
+
+    sh_rest_dim = sh_dim - 3
+    for i in range(sh_rest_dim):
+        name = "f_rest_%d" % i
+        shs[:, 3 + i] = np.asarray(plydata.elements[0][name])
+
+    shs[:, 3:] = shs[:, 3:].reshape(-1, 3, sh_rest_dim//3).transpose([0, 2, 1]).reshape(-1, sh_rest_dim)
+
+    pws = pws.astype(np.float32)
+    rots = rots.astype(np.float32)
+    scales = np.exp(scales)
+    scales = scales.astype(np.float32)
+    alphas = alphas.astype(np.float32)
+    shs = shs.astype(np.float32)
+
+    dtypes = gsdata_type(sh_dim)
+
+    gs = np.rec.fromarrays(
+        [pws, rots, scales, alphas, shs], dtype=dtypes)
+
+    return gs
+
+
+def rotate_gaussian(T, gs):
+    # Transform to world
+    pws = (T @ gs['pw'].T).T
+    w = gs['rot'][:, 0]
+    x = gs['rot'][:, 1]
+    y = gs['rot'][:, 2]
+    z = gs['rot'][:, 3]
+    R = np.array([
+        [1.0 - 2*(y**2 + z**2), 2*(x*y - z*w), 2*(x * z + y * w)],
+        [2*(x*y + z*w), 1.0 - 2*(x**2 + z**2), 2*(y*z - x*w)],
+        [2*(x*z - y*w), 2*(y*z + x*w), 1.0 - 2*(x**2 + y**2)]
+    ]).transpose(2, 0, 1)
+    R_new = T @ R
+    rots = matrix_to_quaternion(R_new)
+    gs['pw'] = pws
+    gs['rot'] = rots
+    return gs
+
+
+def load_gs(fn):
+    if fn.endswith('.ply'):
+        return load_ply(fn)
+    elif fn.endswith('.npy'):
+        return np.load(fn)
+    else:
+        print("%s is not a supported file." % fn)
+        exit(0)
+
+
+def save_gs(fn, gs):
+    np.save(fn, gs)
+
+
+def get_example_gs():
+    gs_data = np.array([[0.,  0.,  0.,  # xyz
+                         1.,  0.,  0., 0.,  # rot
+                         0.05,  0.05,  0.05,  # size
+                         1.,
+                         1.772484,  -1.772484,  1.772484],
+                        [1.,  0.,  0.,
+                         1.,  0.,  0., 0.,
+                         0.2,  0.05,  0.05,
+                         1.,
+                         1.772484,  -1.772484, -1.772484],
+                        [0.,  1.,  0.,
+                         1.,  0.,  0., 0.,
+                         0.05,  0.2,  0.05,
+                         1.,
+                         -1.772484, 1.772484, -1.772484],
+                        [0.,  0.,  1.,
+                         1.,  0.,  0., 0.,
+                         0.05,  0.05,  0.2,
+                         1.,
+                         -1.772484, -1.772484,  1.772484]
+                        ], dtype=np.float32)
+    dtypes = gsdata_type(3)
+    gs = np.frombuffer(gs_data.tobytes(), dtype=dtypes)
+    return gs
+
+
+if __name__ == "__main__":
+    gs = load_gs("/home/liu/workspace/EasyGaussianSplatting/data/final.npy")
+    print(gs.shape)

q3dviewer/utils/convert_ros_msg.py

@@ -26,6 +26,7 @@ def convert_pointcloud2_msg(msg):
     stamp = msg.header.stamp.to_sec()
     return cloud, fields, stamp
 
+
 def convert_odometry_msg(msg):
     pose = np.array(
         [msg.pose.pose.position.x,
@@ -40,6 +41,7 @@ def convert_odometry_msg(msg):
     stamp = msg.header.stamp.to_sec()
     return transform, stamp
 
+
 def convert_image_msg(msg):
     image = np.frombuffer(msg.data, dtype=np.uint8).reshape(
         msg.height, msg.width, -1)

q3dviewer/utils.py

@@ -0,0 +1,71 @@
+import numpy as np
+import time
+
+
+def matrix_to_quaternion(matrix):
+    trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2]
+    if trace > 0:
+        s = 0.5 / np.sqrt(trace + 1.0)
+        w = 0.25 / s
+        x = (matrix[2, 1] - matrix[1, 2]) * s
+        y = (matrix[0, 2] - matrix[2, 0]) * s
+        z = (matrix[1, 0] - matrix[0, 1]) * s
+    else:
+        if matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]:
+            s = 2.0 * np.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])
+            w = (matrix[2, 1] - matrix[1, 2]) / s
+            x = 0.25 * s
+            y = (matrix[0, 1] + matrix[1, 0]) / s
+            z = (matrix[0, 2] + matrix[2, 0]) / s
+        elif matrix[1, 1] > matrix[2, 2]:
+            s = 2.0 * np.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2])
+            w = (matrix[0, 2] - matrix[2, 0]) / s
+            x = (matrix[0, 1] + matrix[1, 0]) / s
+            y = 0.25 * s
+            z = (matrix[1, 2] + matrix[2, 1]) / s
+        else:
+            s = 2.0 * np.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1])
+            w = (matrix[1, 0] - matrix[0, 1]) / s
+            x = (matrix[0, 2] + matrix[2, 0]) / s
+            y = (matrix[1, 2] + matrix[2, 1]) / s
+            z = 0.25 * s
+    return np.array([w, x, y, z])
+
+
+def quaternion_to_matrix(quaternion):
+    _EPS = np.finfo(float).eps * 4.0
+    q = np.array(quaternion[:4], dtype=np.float64, copy=True)
+    nq = np.dot(q, q)
+    if nq < _EPS:
+        return np.identity(4)
+    q *= np.sqrt(2.0 / nq)
+    q = np.outer(q, q)
+    return np.array((
+        (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0),
+        (q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0),
+        (q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
+        (0.0, 0.0,  0.0, 1.0)
+        ), dtype=np.float64)
+
+
+def make_transform(pose, rotation):
+    transform = np.matrix(np.identity(4, dtype=np.float64))
+    transform = quaternion_to_matrix(rotation)
+    transform[0:3, 3] = np.transpose(pose)
+    return transform
+
+
+class FPSMonitor():
+    def __init__(self):
+        self.stamp_record = []
+
+    def count(self):
+        cur_stamp = time.time()
+        self.stamp_record.append(cur_stamp)
+        while len(self.stamp_record) > 0:
+            if(cur_stamp - self.stamp_record[0] > 1.):
+                self.stamp_record.pop(0)
+            else:
+                break
+        return len(self.stamp_record)
+

{q3dviewer-1.0.5.dist-info → q3dviewer-1.0.7.dist-info}/METADATA

@@ -1,13 +1,21 @@
 Metadata-Version: 2.1
 Name: q3dviewer
-Version: 1.0.5
-License-File: LICENSE
-Requires-Dist: numpy
-Requires-Dist: pyside6
+Version: 1.0.7
+Summary: UNKNOWN
+Home-page: UNKNOWN
+Author: UNKNOWN
+Author-email: UNKNOWN
+License: UNKNOWN
+Platform: UNKNOWN
 Requires-Dist: PyOpenGL
-Requires-Dist: pillow
+Requires-Dist: laspy
 Requires-Dist: meshio
-Requires-Dist: pypcd4
+Requires-Dist: numpy
+Requires-Dist: pillow
 Requires-Dist: pye57
-Requires-Dist: laspy
+Requires-Dist: pypcd4
+Requires-Dist: pyside6
+
+UNKNOWN
+
 

{q3dviewer-1.0.5.dist-info → q3dviewer-1.0.7.dist-info}/RECORD

@@ -1,19 +1,24 @@
 q3dviewer/__init__.py,sha256=rP5XX_x8g7hxIMqNHlU89BN4dt5MSvoYYwip68fCmhc,173
-q3dviewer/base_glwidget.py,sha256=lCYTT0ENrxZU31GRD0XeUOWFbDX_45zTQP8TH15PXTE,8742
+q3dviewer/base_glwidget.py,sha256=i1GOuPJMd_HeOu6-Y0mMe73tM4U0z1gdM-XOJszXGnM,8916
 q3dviewer/base_item.py,sha256=-6GogOKrjVRSTszio9ELPDhehBaUlTyNaag2bUl81l8,1337
-q3dviewer/gau_io.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+q3dviewer/basic_window.py,sha256=CFErOPRMysFcCqq3vhDsQ-xZzLArO3m1yABCTIhq5do,7946
+q3dviewer/cloud_viewer.py,sha256=IxxrB6Sl6aPCr9P9QzKHGkMcDP_DjsBWbkmIbsAoIM4,2358
+q3dviewer/gau_io.py,sha256=S6NmqL5vSMgHVxKR-eu4CWCqgZeWBJKYRoOMAwr8Xbo,5890
 q3dviewer/glwidget.py,sha256=ppkAJUCCJ4pnLX3j6jCFmJw8O-kCr7UphsEdkGSakpQ,4832
+q3dviewer/utils.py,sha256=evF0d-v17hbTmquC24fmMIp9CsXpUnSQZr4MVy2sfao,2426
 q3dviewer/viewer.py,sha256=Rv_YMo-N2kibh4Bnaz0g32hEtNyn8WXRmoh7GTSPcJg,1708
 q3dviewer/custom_items/__init__.py,sha256=gOiAxdjDaAnFL8YbqSEWWWOwUrJfvzP9JLR34sCB9-4,434
 q3dviewer/custom_items/axis_item.py,sha256=6dWGqBeA3DL-9xmsxdmTSIGLs6BcRbiysBKkkXVb5oI,4456
+q3dviewer/custom_items/camera_frame_item.py,sha256=VBsr3Avly_YWXViIh4DJkGc_HJt227GeOYLpGtbYTOw,5605
 q3dviewer/custom_items/cloud_io_item.py,sha256=gjK3n9WKB7JwxC93ijkweEHA5EezpgNJ8KO-PBaDKCs,2835
-q3dviewer/custom_items/cloud_item.py,sha256=BOjTqUdTrE2nxqjixfF8T70N_vhGntyIBQVh8C6mtWY,12637
+q3dviewer/custom_items/cloud_item.py,sha256=u9hHHFjK4Ls6ssFVbBFqdR2xg0QNmgsKOz9jGHSqrt8,12444
 q3dviewer/custom_items/frame_item.py,sha256=YTBOEeRWh95trhAlKoZ9LZ4I2x-A5JjOhNPp4OvD5E0,6983
 q3dviewer/custom_items/gaussian_item.py,sha256=B0mYjlW83Kr6FUtZ6P_f40pjjiASPB8g3CSjMTnjsvY,9857
 q3dviewer/custom_items/grid_item.py,sha256=U4nk16d5Dkbt6U9s23c9PIlG-wPwu15Ho0HsamDuyqM,3436
 q3dviewer/custom_items/image_item.py,sha256=tJryUj5qxaGx1j7W0lbzx-JJmehMPSEqJXPnSkG4FOI,5529
 q3dviewer/custom_items/line_item.py,sha256=ujl78wBlvcotghauAy7ILTa22P_fdrFzrH6RD4TvY3M,4401
 q3dviewer/custom_items/text_item.py,sha256=nuHMVMQrwy50lNk9hxB94criFxbJJK-SYiK2fSXWUMQ,2158
+q3dviewer/custom_items/trajectory_item.py,sha256=uoKQSrTs_m_m1M8iNAm3peiXnZ9uVPsYQLYas3Gksjg,2754
 q3dviewer/shaders/cloud_frag.glsl,sha256=tbCsDUp9YlPe0hRWlFS724SH6TtMeLO-GVYROzEElZg,609
 q3dviewer/shaders/cloud_vert.glsl,sha256=Vxgw-Zrr0knAK0z4qMXKML6IC4EbffKMwYN2TMXROoI,2117
 q3dviewer/shaders/gau_frag.glsl,sha256=5_UY84tWDts59bxP8x4I-wgnzY8aGeGuo28wX--LW7E,975
@@ -29,13 +34,13 @@ q3dviewer/tools/lidar_cam_calib.py,sha256=kYR41Eq3o7frHLIWqysHUvVbB29GC0sDLzUigw
 q3dviewer/tools/ros_viewer.py,sha256=Arpy5Y89h_1aSb8aFbeHJEYFFUOxCNm4QWLh5tKcPQI,2504
 q3dviewer/utils/__init__.py,sha256=irm8Z_bT8l9kzhoMlds2Dal8g4iw4vjmqNPZSs4W6e0,157
 q3dviewer/utils/cloud_io.py,sha256=ttD8FJExdDhXB1Z0Ej9S939i8gcq4JfyqwLXVh8CEFw,11094
-q3dviewer/utils/convert_ros_msg.py,sha256=VJeleSqp4rMYGCOyrwoXiJveBanTQMM6MEzRMjyXpCI,1684
+q3dviewer/utils/convert_ros_msg.py,sha256=sAoQfy3qLQKsIArBAVm8H--wlQXOcmkKK3-Ox9UCcrc,1686
 q3dviewer/utils/gl_helper.py,sha256=dRY_kUqyPMr7NTcupUr6_VTvgnj53iE2C0Lk0-oFYsI,1435
 q3dviewer/utils/maths.py,sha256=ghnZQYxQtAQonTqQOmfHAMdSluk0sO1Ps_9bL2vMeb8,5397
 q3dviewer/utils/range_slider.py,sha256=jZJQL-uQgnpgLvtYSWpKTrJlLkt3aqNpaRQAePEpNd0,3174
-q3dviewer-1.0.5.dist-info/LICENSE,sha256=81cMOyNfw8KLb1JnPYngGHJ5W83gSbZEBU9MEP3tl-E,1124
-q3dviewer-1.0.5.dist-info/METADATA,sha256=jYybJOLJmUxx8FbhOVJk6kfwHwBgAlOWWNK5zJezL-I,252
-q3dviewer-1.0.5.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
-q3dviewer-1.0.5.dist-info/entry_points.txt,sha256=6zSxtJ5cjcpc32sR_wkRqOaeiJFpEuLnJQckZE3niU0,316
-q3dviewer-1.0.5.dist-info/top_level.txt,sha256=HFFDCbGu28txcGe2HPc46A7EPaguBa_b5oH7bufmxHM,10
-q3dviewer-1.0.5.dist-info/RECORD,,
+q3dviewer-1.0.7.dist-info/LICENSE,sha256=81cMOyNfw8KLb1JnPYngGHJ5W83gSbZEBU9MEP3tl-E,1124
+q3dviewer-1.0.7.dist-info/METADATA,sha256=IP5D90vA9PSGbJAuw2N8a0mceD8YOzx7rIze_lqZGXQ,349
+q3dviewer-1.0.7.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
+q3dviewer-1.0.7.dist-info/entry_points.txt,sha256=2vsxH5_1WhM-DnDgY0TDbiAGFPFMfI0Wmfs_6iXEf4Y,317
+q3dviewer-1.0.7.dist-info/top_level.txt,sha256=HFFDCbGu28txcGe2HPc46A7EPaguBa_b5oH7bufmxHM,10
+q3dviewer-1.0.7.dist-info/RECORD,,

{q3dviewer-1.0.5.dist-info → q3dviewer-1.0.7.dist-info}/entry_points.txt

@@ -5,3 +5,4 @@ lidar_calib = q3dviewer.tools.lidar_calib:main
 lidar_cam_calib = q3dviewer.tools.lidar_cam_calib:main
 mesh_viewer = q3dviewer.tools.mesh_viewer:main
 ros_viewer = q3dviewer.tools.ros_viewer:main
+