q3dviewer 1.0.8__py3-none-any.whl → 1.0.9__py3-none-any.whl

q3dviewer/custom_items/grid_item.py

@@ -9,80 +9,131 @@ from PySide6.QtWidgets import QLabel, QDoubleSpinBox, QLineEdit
 from PySide6.QtCore import QRegularExpression
 from PySide6.QtGui import QRegularExpressionValidator
 import numpy as np
+from q3dviewer.utils.maths import hex_to_rgba
 
 
 class GridItem(BaseItem):
-    def __init__(self, size, spacing, color=np.array([255, 255, 255, 76.5]), offset=np.array([0, 0, 0])):
+    def __init__(self, size=100, spacing=20, color='#ffffff40', offset=np.array([0., 0., 0.])):
         super().__init__()
         self.size = size
         self.spacing = spacing
-        self.color = color
         self.offset = offset
+        self.need_update_grid = True
+        self.set_color(color)
+        self.vertices = self.generate_grid_vertices()
+
+    def set_color(self, color):
+        try:
+            self.rgba = hex_to_rgba(color)
+        except ValueError:
+            pass
+
+    def generate_grid_vertices(self):
+        vertices = []
+        x, y, z = self.offset
+        half_size = self.size / 2  # Keep as float
+        for i in np.arange(-half_size, half_size + self.spacing, self.spacing):
+            vertices.extend([i + x, -half_size + y, z, i + x, half_size + y, z])  # Grid lines parallel to Y axis
+            vertices.extend([-half_size + x, i + y, z, half_size + x, i + y, z])  # Grid lines parallel to X axis
+        vertices = np.array(vertices, dtype=np.float32)
+        return vertices
+
+    def initialize_gl(self):
+        self.vao = glGenVertexArrays(1)
+        vbo = glGenBuffers(1)
+
+        glBindVertexArray(self.vao)
+
+        glBindBuffer(GL_ARRAY_BUFFER, vbo)
+        glBufferData(GL_ARRAY_BUFFER, self.vertices.nbytes, self.vertices, GL_STATIC_DRAW)
+
+        glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, ctypes.c_void_p(0))
+        glEnableVertexAttribArray(0)
+
+        glBindVertexArray(0)
 
     def add_setting(self, layout):
-        label_size = QLabel("Set size:")
-        layout.addWidget(label_size)
         spinbox_size = QDoubleSpinBox()
+        spinbox_size.setPrefix("Size: ")
         spinbox_size.setSingleStep(1.0)
-        layout.addWidget(spinbox_size)
         spinbox_size.setValue(self.size)
-        spinbox_size.valueChanged.connect(self.set_size)
         spinbox_size.setRange(0, 100000)
+        spinbox_size.valueChanged.connect(self.set_size)
+        layout.addWidget(spinbox_size)
 
-        label_spacing = QLabel("Set spacing:")
-        layout.addWidget(label_spacing)
         spinbox_spacing = QDoubleSpinBox()
-        layout.addWidget(spinbox_spacing)
+        spinbox_spacing.setPrefix("Spacing: ")
         spinbox_spacing.setSingleStep(0.1)
         spinbox_spacing.setValue(self.spacing)
-        spinbox_spacing.valueChanged.connect(self._on_spacing)
         spinbox_spacing.setRange(0, 1000)
+        spinbox_spacing.valueChanged.connect(self._on_spacing)
+        layout.addWidget(spinbox_spacing)
 
-        label_offset = QLabel("Set offset (x;y;z):")
-        layout.addWidget(label_offset)
-        self.edit_offset = QLineEdit()
-        regex = QRegularExpression(r"^-?\d+(\.\d+)?;-?\d+(\.\d+)?;-?\d+(\.\d+)?$")
-        validator = QRegularExpressionValidator(regex)
-        self.edit_offset.setValidator(validator)
-        self.edit_offset.setText(f"{self.offset[0]};{self.offset[1]};{self.offset[2]}")
-        self.edit_offset.textChanged.connect(self._on_offset)
-        layout.addWidget(self.edit_offset)
+        spinbox_offset_x = QDoubleSpinBox()
+        spinbox_offset_x.setPrefix("Offset X: ")
+        spinbox_offset_x.setSingleStep(0.1)
+        spinbox_offset_x.setValue(self.offset[0])
+        spinbox_offset_x.valueChanged.connect(self._on_offset_x)
+        layout.addWidget(spinbox_offset_x)
+        
+        spinbox_offset_y = QDoubleSpinBox()
+        spinbox_offset_y.setPrefix("Offset Y: ")
+        spinbox_offset_y.setSingleStep(0.1)
+        spinbox_offset_y.setValue(self.offset[1])
+        spinbox_offset_y.valueChanged.connect(self._on_offset_y)
+        layout.addWidget(spinbox_offset_y)
+        
+        spinbox_offset_z = QDoubleSpinBox()
+        spinbox_offset_z.setPrefix("Offset Z: ")
+        spinbox_offset_z.setSingleStep(0.1)
+        spinbox_offset_z.setValue(self.offset[2])
+        spinbox_offset_z.valueChanged.connect(self._on_offset_z)
+        layout.addWidget(spinbox_offset_z)
 
     def set_size(self, size):
         self.size = size
+        self.need_update_grid = True
 
     def _on_spacing(self, spacing):
         if spacing > 0:
             self.spacing = spacing
+            self.need_update_grid = True
 
-    def _on_offset(self, text):
-        try:
-            values = list(map(float, text.split(';')))
-            if len(values) == 3:
-                self.offset = np.array(values)
-            else:
-                raise ValueError("Offset must have 3 values separated by ';'")
-        except ValueError:
-            pass
+    def _on_offset_x(self, value):
+        self.offset[0] = value
+        print(self.offset)
+        self.need_update_grid = True
+
+    def _on_offset_y(self, value):
+        self.offset[1] = value
+        self.need_update_grid = True
+
+    def _on_offset_z(self, value):
+        self.offset[2] = value
+        self.need_update_grid = True
 
     def set_offset(self, offset):
         if isinstance(offset, np.ndarray) and offset.shape == (3,):
             self.offset = offset
-            self.edit_offset.setText(f"{self.offset[0]};{self.offset[1]};{self.offset[2]}")
+            self.need_update_grid = True
         else:
             raise ValueError("Offset must be a numpy array with shape (3,)")
 
     def paint(self):
+        if self.need_update_grid:
+            self.vertices = self.generate_grid_vertices()
+            self.initialize_gl()
+            self.need_update_grid = False
+        
         glEnable(GL_BLEND)
         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
-        glColor4f(self.color[0] / 255.0, self.color[1] / 255.0, self.color[2] / 255.0, self.color[3] / 255.0)
-        glBegin(GL_LINES)
-        for i in np.arange(-self.size, self.size + self.spacing, self.spacing):
-            glVertex3f(i + self.offset[0], -self.size + self.offset[1], self.offset[2])
-            glVertex3f(i + self.offset[0], self.size + self.offset[1], self.offset[2])
-            glVertex3f(-self.size + self.offset[0], i + self.offset[1], self.offset[2])
-            glVertex3f(self.size + self.offset[0], i + self.offset[1], self.offset[2])
-        glEnd()
+
+        glLineWidth(1)
+        glColor4f(*self.rgba)
+        glBindVertexArray(self.vao)
+        glDrawArrays(GL_LINES, 0, len(self.vertices) // 3)
+        glBindVertexArray(0)
+        glLineWidth(1)
         glDisable(GL_BLEND)
 
 

q3dviewer/custom_items/image_item.py

@@ -154,9 +154,8 @@ class ImageItem(BaseItem):
         glDisable(GL_BLEND)
 
     def add_setting(self, layout):
-        alpha_label = QLabel("Set Alpha:")
-        layout.addWidget(alpha_label)
         spinbox_alpha = QSpinBox()
+        spinbox_alpha.setPrefix("Alpha: ")
         spinbox_alpha.setSingleStep(1)
         spinbox_alpha.setRange(0, 255)
         spinbox_alpha.setValue(self.alpha)

q3dviewer/custom_items/line_item.py

@@ -27,7 +27,7 @@ class LineItem(BaseItem):
         self.line_type = GL_LINE_STRIP if line_type == 'LINE_STRIP' else GL_LINES
 
     def add_setting(self, layout):
-        label_color = QLabel("Set trajectory color:")
+        label_color = QLabel("Color:")
         layout.addWidget(label_color)
         self.color_edit = QLineEdit()
         self.color_edit.setToolTip("Hex number, i.e. #FF4500")
@@ -38,14 +38,13 @@ class LineItem(BaseItem):
         self.color_edit.setValidator(validator)
         layout.addWidget(self.color_edit)
 
-        label_width = QLabel("Set width:")
-        layout.addWidget(label_width)
         spinbox_width = QDoubleSpinBox()
+        spinbox_width.setPrefix("Width: ")
         spinbox_width.setSingleStep(0.1)
-        layout.addWidget(spinbox_width)
         spinbox_width.setValue(self.width)
-        spinbox_width.valueChanged.connect(self.set_width)
         spinbox_width.setRange(0.1, 10.0)
+        spinbox_width.valueChanged.connect(self.set_width)
+        layout.addWidget(spinbox_width)
 
     def _on_color(self, color):
         try:

q3dviewer/gau_io.py

@@ -1,168 +0,0 @@
-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/glwidget.py

@@ -4,7 +4,7 @@ Distributed under MIT license. See LICENSE for more information.
 """
 
 from PySide6 import QtCore
-from PySide6.QtWidgets import QWidget, QComboBox, QVBoxLayout, QHBoxLayout, QSizePolicy, QSpacerItem, QLabel, QLineEdit, QCheckBox
+from PySide6.QtWidgets import QWidget, QComboBox, QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QCheckBox, QGroupBox
 from PySide6.QtGui import QKeyEvent, QVector3D, QRegularExpressionValidator
 from PySide6.QtCore import QRegularExpression
 from OpenGL.GL import *
@@ -17,11 +17,9 @@ class SettingWindow(QWidget):
         self.combo_items = QComboBox()
         self.combo_items.currentIndexChanged.connect(self.on_combo_selection)
         main_layout = QVBoxLayout()
-        self.stretch = QSpacerItem(
-            10, 10, QSizePolicy.Minimum, QSizePolicy.Expanding)
+        main_layout.setAlignment(QtCore.Qt.AlignTop)
         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")
@@ -39,17 +37,15 @@ class SettingWindow(QWidget):
                 child.widget().deleteLater()
 
     def on_combo_selection(self, index):
-        self.layout.removeItem(self.stretch)
         # remove all setting of previous widget
         self.clear_setting()
-
         key = list(self.items.keys())
         item = self.items[key[index]]
-        if hasattr(item, "add_setting"):
-            item.add_setting(self.layout)
-            self.layout.addItem(self.stretch)
-        else:
-            print("%s: No setting." % (item.__class__.__name__))
+        group_box = QGroupBox()
+        group_layout = QVBoxLayout()
+        item.add_setting(group_layout)
+        group_box.setLayout(group_layout)
+        self.layout.addWidget(group_box)
 
 
 class GLWidget(BaseGLWidget):
@@ -57,7 +53,7 @@ class GLWidget(BaseGLWidget):
         self.followed_name = 'none'
         self.named_items = {}
         self.color_str = '#000000'
-        self.followable_item_name = ['none']
+        self.followable_item_name = None
         self.setting_window = SettingWindow()
         self.enable_show_center = True
         super(GLWidget, self).__init__()
@@ -73,7 +69,8 @@ class GLWidget(BaseGLWidget):
 
     def update(self):
         if self.followed_name != 'none':
-            self.center = self.named_items[self.followed_name].T[:3, 3]
+            new_center = self.named_items[self.followed_name].T[:3, 3]
+            self.set_center(new_center)
         super().update()
 
     def add_setting(self, layout):
@@ -90,6 +87,10 @@ class GLWidget(BaseGLWidget):
         
         label_focus = QLabel("Set Focus:")
         combo_focus = QComboBox()
+    
+        if self.followable_item_name is None:
+            self.initial_followable()
+
         for name in self.followable_item_name:
             combo_focus.addItem(name)
         combo_focus.currentIndexChanged.connect(self.on_followable_selection)
@@ -101,6 +102,12 @@ class GLWidget(BaseGLWidget):
         checkbox_show_center.stateChanged.connect(self.change_show_center)
         layout.addWidget(checkbox_show_center)
 
+    def initial_followable(self):
+        self.followable_item_name = ['none']
+        for name, item in self.named_items.items():
+            if item.__class__.__name__ == 'AxisItem' and not item._disable_setting:
+                self.followable_item_name.append(name)
+
     def set_bg_color(self, color_str):
         try:
             color_flat = int(color_str[1:], 16)
@@ -114,9 +121,8 @@ class GLWidget(BaseGLWidget):
 
     def add_item_with_name(self, name, item):
         self.named_items.update({name: item})
-        if (item.__class__.__name__ == 'AxisItem'):
-            self.followable_item_name.append(name)
-        self.setting_window.add_setting(name, item)
+        if not item._disable_setting:
+            self.setting_window.add_setting(name, item)
         super().add_item(item)
 
     def open_setting_window(self):

q3dviewer/test/test_interpolation.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+"""
+this script tests interpolation of 3D poses.
+"""
+
+
+import numpy as np
+from q3dviewer.utils.maths import expSO3, logSO3, makeT, makeRt
+
+
+def interpolate_pose(T1, T2, v_max, omega_max, dt=0.1):
+    R1, t1 = makeRt(T1)
+    R2, t2 = makeRt(T2)
+    
+    # Get transform time based on linear velocity
+    d = np.linalg.norm(t2 - t1)
+    t_lin = d / v_max
+    
+    # Get transform time based on angular velocity
+    omega = logSO3(R2 @ R1.T)
+    theta = np.linalg.norm(omega)
+    t_ang = theta / omega_max
+    
+    # Get total time based on the linear and angular time
+    t_total = max(t_lin, t_ang)
+    num_steps = int(np.ceil(t_total / dt))
+    
+    # Generate interpolated transforms
+    interpolated_Ts = []
+    for i in range(num_steps + 1):
+        s = i / num_steps
+        t_interp = (1 - s) * t1 + s * t2
+        # Interpolate rotation using SO3
+        R_interp = expSO3(s * omega) @ R1
+        T_interp = makeT(R_interp, t_interp)
+        interpolated_Ts.append(T_interp)
+    
+    return interpolated_Ts
+
+if __name__ == "__main__":
+    T1 = np.eye(4)  # Identity transformation
+    T2 = np.array([[0, -1, 0, 1], [1, 0, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]])  # Target transformation
+
+    v_max = 1.0  # Maximum linear velocity (m/s)
+    omega_max = np.pi / 4  # Maximum angular velocity (rad/s)
+
+    # Perform interpolation
+    interpolated_poses = interpolate_pose(T1, T2, v_max, omega_max)
+    for i, T in enumerate(interpolated_poses):
+        print(f"Step {i}:\n{T}\n")
+
+

q3dviewer/test/test_rendering.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+"""
+this script tests the rendering of a cloud in a camera frame based on the 
+camera pose and intrinsic matrix
+"""
+
+import numpy as np
+import q3dviewer as q3d
+import cv2
+
+cloud, _ = q3d.load_pcd('/home/liu/lab.pcd')
+
+
+Tcw = np.array([[7.07106781e-01,  7.07106781e-01,  0.00000000e+00,
+                 0.00000000e+00],
+                [-3.53553391e-01,  3.53553391e-01,  8.66025404e-01,
+                 3.55271368e-15],
+                [6.12372436e-01, -6.12372436e-01,  5.00000000e-01,
+                 -4.00000000e+01],
+                [0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
+                 1.00000000e+00]])
+# convert the opengl camera coordinate to the opencv camera coordinate
+Tconv = np.array([[1, 0, 0, 0],
+                  [0, -1, 0, 0],
+                  [0, 0, -1, 0],
+                  [0, 0, 0, 1]])
+
+Tcw = Tconv @ Tcw
+
+K = np.array([[1.64718029e+03, 0.00000000e+00, 9.51000000e+02],
+              [0.00000000e+00, 1.64718036e+03, 5.31000000e+02],
+              [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
+
+
+def render_frame(cloud, Tcw, K, width, height):
+    image = np.zeros((height, width, 3), dtype=np.uint8)
+    Rcw, tcw = Tcw[:3, :3], Tcw[:3, 3]
+    pc = (Rcw @ cloud['xyz'].T).T + tcw
+    uv = (K @ pc.T).T
+    uv = uv[:, :2] / uv[:, 2][:, np.newaxis]
+    mask = (pc[:, 2] > 0) & (uv[:, 0] > 0) & (
+        uv[:, 0] < width) & (uv[:, 1] > 0) & (uv[:, 1] < height)
+    uv = uv[mask]
+    u = uv[:, 0].astype(int)
+    v = uv[:, 1].astype(int)
+    rgb = cloud['irgb'][mask]
+    r = rgb >> 16 & 0xff
+    g = rgb >> 8 & 0xff
+    b = rgb & 0xff
+
+    # Sort by depth to ensure front points are drawn first
+    depth = pc[mask, 2]
+    sorted_indices = np.argsort(depth)
+    u = u[sorted_indices]
+    v = v[sorted_indices]
+    r = r[sorted_indices]
+    g = g[sorted_indices]
+    b = b[sorted_indices]
+
+    image[v, u] = np.stack([b, g, r], axis=1)
+    return image
+
+
+image = render_frame(cloud, Tcw, K, 1902, 1062)
+cv2.imshow('image', image)
+cv2.waitKey(0)