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

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/tools/film_maker.py

@@ -269,8 +269,8 @@ class CMMViewer(q3d.Viewer):
             self.current_frame_index = 0
             self.timer.start(self.update_interval)  # Adjust the interval as needed
             self.is_playing = True
-            self.play_button.setStyleSheet("")
-            self.play_button.setText("Stop")
+            self.play_button.setStyleSheet("background-color: red")
+            self.play_button.setText("Playing")
             self.record_checkbox.setEnabled(False)
             if self.is_recording is True:
                 self.start_recording()

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.1.0.dist-info → q3dviewer-1.1.1.dist-info}/METADATA

@@ -1,23 +1,25 @@
 Metadata-Version: 2.1
 Name: q3dviewer
-Version: 1.1.0
+Version: 1.1.1
 Summary: A library designed for quickly deploying a 3D viewer.
 Home-page: https://github.com/scomup/q3dviewer
 Author: Liu Yang
+License: UNKNOWN
+Platform: UNKNOWN
 Classifier: Programming Language :: Python :: 3
 Classifier: License :: OSI Approved :: MIT License
 Classifier: Operating System :: OS Independent
 Description-Content-Type: text/markdown
-License-File: LICENSE
-Requires-Dist: numpy
-Requires-Dist: pyside6
 Requires-Dist: PyOpenGL
-Requires-Dist: pillow
+Requires-Dist: imageio
+Requires-Dist: imageio[ffmpeg]
+Requires-Dist: laspy
 Requires-Dist: meshio
-Requires-Dist: pypcd4
+Requires-Dist: numpy
+Requires-Dist: pillow
 Requires-Dist: pye57
-Requires-Dist: laspy
-Requires-Dist: imageio
+Requires-Dist: pypcd4
+Requires-Dist: pyside6
 
 ## q3dviewer
 
@@ -150,7 +152,7 @@ def main():
 
     # Create a viewer
     viewer = q3d.Viewer(name='example')
-    
+
     # Add items to the viewer
     viewer.add_items({
         'grid': grid_item,
@@ -212,3 +214,4 @@ class YourItem(q3d.BaseItem):
 ```
 
 Enjoy using `q3dviewer`!
+

{q3dviewer-1.1.0.dist-info → q3dviewer-1.1.1.dist-info}/RECORD

@@ -1,32 +1,34 @@
 q3dviewer/__init__.py,sha256=rP5XX_x8g7hxIMqNHlU89BN4dt5MSvoYYwip68fCmhc,173
-q3dviewer/base_glwidget.py,sha256=k4fVwV2A_0CS7zZgHargbKE7push-l2UN7rmr2Z9vFo,10916
+q3dviewer/base_glwidget.py,sha256=tk8icQsRZ3OrSLkHvywmrxElk6jpMooLDZdbZ3sRLnk,11313
 q3dviewer/base_item.py,sha256=lzb04oRaS4rRJrAP6C1Bu4ugK237FgupMTB97zjNVFw,1768
-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=im8hjVYEL0Zl7fOIHTQMJdWu7WNOHlvTdIDYjebz9WA,4940
+q3dviewer/utils.py,sha256=evF0d-v17hbTmquC24fmMIp9CsXpUnSQZr4MVy2sfao,2426
 q3dviewer/viewer.py,sha256=LH1INLFhi6pRjzazzQJ0AWT4hgyXI6GnmqoJFUwUZVE,2517
 q3dviewer/custom_items/__init__.py,sha256=gOiAxdjDaAnFL8YbqSEWWWOwUrJfvzP9JLR34sCB9-4,434
 q3dviewer/custom_items/axis_item.py,sha256=PTBSf5DmQI8ieSinYjY_aC7P8q1nzE-2Vc2GNd1O3Os,2568
+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=7r6s_j1qEUfLe70t-M68-oIrGRrPOUcZp18ML6jST0s,12542
+q3dviewer/custom_items/cloud_item.py,sha256=s46TxNDw7E6DW3V-ek_PF7egB3kRbnqpOBNUKdaC8IY,12620
 q3dviewer/custom_items/frame_item.py,sha256=6BOM3MXi-Akv6KUXDC3QYEAXKxwk0Eo1KQn-F7jkqrQ,7559
 q3dviewer/custom_items/gaussian_item.py,sha256=CZoXMmj2JPFfMqu7v4q4dLUI2cg_WfE1DHWGXjEYn6M,9869
 q3dviewer/custom_items/grid_item.py,sha256=20n4TGm5YEaudhnEOCOk-HtsKwxVxaPr8YV36kO04yU,4802
 q3dviewer/custom_items/image_item.py,sha256=ctNR81fVgxkdl2n3U_TPFL6yn086UhNI_A9fFHgkc-4,5491
 q3dviewer/custom_items/line_item.py,sha256=u0oFN2iHzsRHtnbvyvC_iglEkCwEU2NnTuE536sKUAE,4348
 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
 q3dviewer/shaders/gau_prep.glsl,sha256=eCT9nm65uz32w8NaDjeGKhyAZh42Aea-QTwr3yQVr9U,7218
 q3dviewer/shaders/gau_vert.glsl,sha256=NNbVhv_JyqZDK9iXAyBAcIHAtim7G9yWbC9IaUfTL1w,1666
 q3dviewer/shaders/sort_by_key.glsl,sha256=CA2zOcbyDGYAJSJEUvgjUqNshg9NAehf8ipL3Jsv4qE,1097
-q3dviewer/test/test_interpolation.py,sha256=rR_CXsYFLpn0zO0mHf_jL-naluDBMSky--FviOQga0Q,1657
-q3dviewer/test/test_rendering.py,sha256=gbTcu7-cg20DgC5Zoi17C1s5lBGLfAE1rW9biqPjRsA,2164
 q3dviewer/tools/__init__.py,sha256=01wG7BGM6VX0QyFBKsqPmyf2e-vrmV_N3-mo-VQ1VBg,20
-q3dviewer/tools/cinematographer.py,sha256=o_24SSQ4mF062QQ7Gv3i90v7fA79PcHLB03UHXufuEA,13950
 q3dviewer/tools/cloud_viewer.py,sha256=10f2LSWpmsXzxrGobXw188doVjJbgBfPoqZPUi35EtI,3867
 q3dviewer/tools/example_viewer.py,sha256=yeVXT0k4-h1vTLKnGzWADZD3our6XUaYUTy0p5daTkE,959
-q3dviewer/tools/film_maker.py,sha256=CC4RuK0bE_-0kcdU6xyzqR47pB4TEcm0g4LlDrFs3qY,16004
+q3dviewer/tools/film_maker.py,sha256=PkkMW6__ml7850U2UV2G1Pn4Ahk4EWa7edcvI_5-Z4Y,16028
 q3dviewer/tools/gaussian_viewer.py,sha256=vIwWmiFhjNmknrEkBLzt2yiegeH7LP3OeNjnGM6GzaI,1633
 q3dviewer/tools/lidar_calib.py,sha256=M01bGg2mT8LwVcYybolr4UW_UUaR-f-BFciEHtjeK-w,10488
 q3dviewer/tools/lidar_cam_calib.py,sha256=SYNLDvi15MX7Q3aGn771fvu1cES9xeXgP0_WmDq33w4,11200
@@ -37,9 +39,9 @@ q3dviewer/utils/convert_ros_msg.py,sha256=sAoQfy3qLQKsIArBAVm8H--wlQXOcmkKK3-Ox9
 q3dviewer/utils/gl_helper.py,sha256=dRY_kUqyPMr7NTcupUr6_VTvgnj53iE2C0Lk0-oFYsI,1435
 q3dviewer/utils/maths.py,sha256=5TmjWUX1K3UjygXxrUsydjbo7tPzu0gD-yy7qtQUGBU,10588
 q3dviewer/utils/range_slider.py,sha256=jZJQL-uQgnpgLvtYSWpKTrJlLkt3aqNpaRQAePEpNd0,3174
-q3dviewer-1.1.0.dist-info/LICENSE,sha256=81cMOyNfw8KLb1JnPYngGHJ5W83gSbZEBU9MEP3tl-E,1124
-q3dviewer-1.1.0.dist-info/METADATA,sha256=hiWxWsKDWmCaXB-lJYhi-kutcydCeafxfzDSgeQv5Y8,6991
-q3dviewer-1.1.0.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
-q3dviewer-1.1.0.dist-info/entry_points.txt,sha256=aeUdGH7UIgMZEMFUc-0xPZWspY95GoPdZcZuLceq85g,361
-q3dviewer-1.1.0.dist-info/top_level.txt,sha256=HFFDCbGu28txcGe2HPc46A7EPaguBa_b5oH7bufmxHM,10
-q3dviewer-1.1.0.dist-info/RECORD,,
+q3dviewer-1.1.1.dist-info/LICENSE,sha256=81cMOyNfw8KLb1JnPYngGHJ5W83gSbZEBU9MEP3tl-E,1124
+q3dviewer-1.1.1.dist-info/METADATA,sha256=wSqlVSSanm719PxnxIE7VOStyogcvsVwK1QobHCOLQI,7032
+q3dviewer-1.1.1.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
+q3dviewer-1.1.1.dist-info/entry_points.txt,sha256=EOjker7XYaBk70ffvNB_knPcfA33Bnlg21ZjEeM1EyI,362
+q3dviewer-1.1.1.dist-info/top_level.txt,sha256=HFFDCbGu28txcGe2HPc46A7EPaguBa_b5oH7bufmxHM,10
+q3dviewer-1.1.1.dist-info/RECORD,,

{q3dviewer-1.1.0.dist-info → q3dviewer-1.1.1.dist-info}/entry_points.txt

@@ -6,3 +6,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
+

q3dviewer/test/test_interpolation.py

@@ -1,58 +0,0 @@
-#!/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

@@ -1,72 +0,0 @@
-#!/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)