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

q3dviewer/utils/cloud_io.py

@@ -0,0 +1,323 @@
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+import numpy as np
+import meshio
+from pypcd4 import PointCloud, MetaData
+from pye57 import E57
+import laspy
+
+
+def save_ply(cloud, save_path):
+    xyz = cloud['xyz']
+    i = (cloud['irgb'] & 0xFF000000) >> 24
+    rgb = cloud['irgb'] & 0x00FFFFFF
+    mesh = meshio.Mesh(points=xyz, cells=[], point_data={
+                       "rgb": rgb, "intensity": i})
+    mesh.write(save_path, file_format="ply")
+
+
+def load_ply(file):
+    mesh = meshio.read(file)
+    xyz = mesh.points
+    rgb = np.zeros([xyz.shape[0]], dtype=np.uint32)
+    intensity = np.zeros([xyz.shape[0]], dtype=np.uint32)
+    color_mode = 'FLAT'
+    if "intensity" in mesh.point_data:
+        intensity = mesh.point_data["intensity"]
+        color_mode = 'I'
+    if "rgb" in mesh.point_data:
+        rgb = mesh.point_data["rgb"]
+        color_mode = 'RGB'
+    irgb = (intensity << 24) | rgb
+    dtype = [('xyz', '<f4', (3,)), ('irgb', '<u4')]
+    cloud = np.rec.fromarrays([xyz, irgb], dtype=dtype)
+    return cloud, color_mode
+
+
+def save_pcd(cloud, save_path):
+    fields = ('x', 'y', 'z', 'intensity', 'rgb')
+    metadata = MetaData.model_validate(
+        {
+            "fields": fields,
+            "size": [4, 4, 4, 4, 4],
+            "type": ['F', 'F', 'F', 'U', 'U'],
+            "count": [1, 1, 1, 1, 1],
+            "width": cloud.shape[0],
+            "points": cloud.shape[0],
+        })
+    i = (cloud['irgb'] & 0xFF000000) >> 24
+    rgb = cloud['irgb'] & 0x00FFFFFF
+
+    dtype = [('xyz', '<f4', (3,)), ('intensity', '<u4'), ('rgb', '<u4')]
+    tmp = np.rec.fromarrays([cloud['xyz'], i, rgb], dtype=dtype)
+
+    PointCloud(metadata, tmp).save(save_path)
+
+
+def load_pcd(file):
+    dtype = [('xyz', '<f4', (3,)), ('irgb', '<u4')]
+    pc = PointCloud.from_path(file).pc_data
+    rgb = np.zeros([pc.shape[0]], dtype=np.uint32)
+    intensity = np.zeros([pc.shape[0]], dtype=np.uint32)
+    color_mode = 'FLAT'
+    if 'intensity' in pc.dtype.names:
+        intensity = pc['intensity'].astype(np.uint32)
+        color_mode = 'I'
+    if 'rgb' in pc.dtype.names:
+        rgb = pc['rgb'].astype(np.uint32)
+        color_mode = 'RGB'
+    irgb = (intensity << 24) | rgb
+    xyz = np.stack([pc['x'], pc['y'], pc['z']], axis=1)
+    cloud = np.rec.fromarrays([xyz, irgb], dtype=dtype)
+    return cloud, color_mode
+
+
+def save_e57(cloud, save_path):
+    e57 = E57(save_path, mode='w')
+    x = cloud['xyz'][:, 0]
+    y = cloud['xyz'][:, 1]
+    z = cloud['xyz'][:, 2]
+    i = (cloud['irgb'] & 0xFF000000) >> 24
+    r = (cloud['irgb'] & 0x00FF0000) >> 16
+    g = (cloud['irgb'] & 0x0000FF00) >> 8
+    b = (cloud['irgb'] & 0x000000ff)
+    data = {"cartesianX": x, "cartesianY": y, "cartesianZ": z,
+            "intensity": i,
+            "colorRed": r, "colorGreen": g, "colorBlue": b}
+    e57.write_scan_raw(data)
+    e57.close()
+
+
+def load_e57(file_path):
+    e57 = E57(file_path, mode="r")
+    scans = e57.read_scan(0, ignore_missing_fields=True,
+                          intensity=True, colors=True)
+    x = scans["cartesianX"]
+    y = scans["cartesianY"]
+    z = scans["cartesianZ"]
+    rgb = np.zeros([x.shape[0]], dtype=np.uint32)
+    intensity = np.zeros([x.shape[0]], dtype=np.uint32)
+    color_mode = 'FLAT'
+    if "intensity" in scans:
+        intensity = scans["intensity"].astype(np.uint32)
+        color_mode = 'I'
+    if all([x in scans for x in ["colorRed", "colorGreen", "colorBlue"]]):
+        r = scans["colorRed"].astype(np.uint32)
+        g = scans["colorGreen"].astype(np.uint32)
+        b = scans["colorBlue"].astype(np.uint32)
+        rgb = (r << 16) | (g << 8) | b
+        color_mode = 'RGB'
+    irgb = (intensity << 24) | rgb
+    dtype = [('xyz', '<f4', (3,)), ('irgb', '<u4')]
+    cloud = np.rec.fromarrays(
+        [np.stack([x, y, z], axis=1), irgb],
+        dtype=dtype)
+    e57.close()
+    return cloud, color_mode
+
+
+def load_las(file):
+    with laspy.open(file) as f:
+        las = f.read()
+        xyz = np.vstack((las.x, las.y, las.z)).transpose()
+        dimensions = list(las.point_format.dimension_names)
+        color_mode = 'FLAT'
+        rgb = np.zeros([las.x.shape[0]], dtype=np.uint32)
+        intensity = np.zeros([las.x.shape[0]], dtype=np.uint32)
+        if 'intensity' in dimensions:
+            intensity = las.intensity.astype(np.uint32)
+            color_mode = 'I'
+        if 'red' in dimensions and 'green' in dimensions and 'blue' in dimensions:
+            red = las.red
+            green = las.green
+            blue = las.blue
+            max_val = np.max([red, green, blue])
+            if red.dtype == np.dtype('uint16') and max_val > 255:
+                red = (red / 65535.0 * 255).astype(np.uint32)
+                green = (green / 65535.0 * 255).astype(np.uint32)
+                blue = (blue / 65535.0 * 255).astype(np.uint32)
+            rgb = (red << 16) | (green << 8) | blue
+            color_mode = 'RGB'
+        color = (intensity << 24) | rgb
+        dtype = [('xyz', '<f4', (3,)), ('irgb', '<u4')]
+        cloud = np.rec.fromarrays([xyz, color], dtype=dtype)
+    return cloud, color_mode
+
+def save_las(cloud, save_path):
+    header = laspy.LasHeader(point_format=3, version="1.2")
+    las = laspy.LasData(header)
+    las.x = cloud['xyz'][:, 0]
+    las.y = cloud['xyz'][:, 1]
+    las.z = cloud['xyz'][:, 2]
+    las.red = (cloud['irgb'] >> 16) & 0xFF
+    las.green = (cloud['irgb'] >> 8) & 0xFF
+    las.blue = cloud['irgb'] & 0xFF
+    las.intensity = cloud['irgb'] >> 24
+    las.write(save_path)
+
+
+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_wxyz(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_gs_ply(path, T=None):
+    mesh = meshio.read(path)
+    vertices = mesh.points
+    pws = vertices[:, :3]
+
+    alphas = mesh.point_data['opacity']
+    alphas = 1 / (1 + np.exp(-alphas))
+
+    scales = np.vstack((mesh.point_data['scale_0'],
+                        mesh.point_data['scale_1'],
+                        mesh.point_data['scale_2'])).T
+
+    rots = np.vstack((mesh.point_data['rot_0'],
+                      mesh.point_data['rot_1'],
+                      mesh.point_data['rot_2'],
+                      mesh.point_data['rot_3'])).T
+    rots /= np.linalg.norm(rots, axis=1)[:, np.newaxis]
+
+    sh_dim = len(mesh.point_data) - 11
+    shs = np.zeros([pws.shape[0], sh_dim])
+    shs[:, 0] = mesh.point_data['f_dc_0']
+    shs[:, 1] = mesh.point_data['f_dc_1']
+    shs[:, 2] = mesh.point_data['f_dc_2']
+
+    sh_rest_dim = sh_dim - 3
+    if sh_rest_dim > 0:
+        for i in range(sh_rest_dim):
+            name = f"f_rest_{i}"
+            shs[:, 3 + i] = mesh.point_data[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).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_wxyz(R_new)
+    gs['pw'] = pws
+    gs['rot'] = rots
+    return gs
+
+
+def load_gs(fn):
+    if fn.endswith('.ply'):
+        return load_gs_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/tmp.ply")
+    print(gs.shape)

q3dviewer/utils/convert_ros_msg.py

@@ -0,0 +1,49 @@
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+import numpy as np
+from pypcd4 import PointCloud
+from q3dviewer.utils.maths import make_transform
+
+
+def convert_pointcloud2_msg(msg):
+    pc = PointCloud.from_msg(msg).pc_data
+    data_type = [('xyz', '<f4', (3,)), ('irgb', '<u4')]
+    rgb = np.zeros([pc.shape[0]], dtype=np.uint32)
+    intensity = np.zeros([pc.shape[0]], dtype=np.uint32)
+    fields = ['xyz']
+    if 'intensity' in pc.dtype.names:
+        intensity = pc['intensity'].astype(np.uint32)
+        fields.append('intensity')
+    if 'rgb' in pc.dtype.names:
+        rgb = pc['rgb'].view(np.uint32)
+        fields.append('rgb')
+    irgb = (intensity << 24) | rgb
+    xyz = np.stack([pc['x'], pc['y'], pc['z']], axis=1)
+    cloud = np.rec.fromarrays([xyz, irgb], dtype=data_type)
+    stamp = msg.header.stamp.to_sec()
+    return cloud, fields, stamp
+
+def convert_odometry_msg(msg):
+    pose = np.array(
+        [msg.pose.pose.position.x,
+         msg.pose.pose.position.y,
+         msg.pose.pose.position.z])
+    rotation = np.array([
+        msg.pose.pose.orientation.x,
+        msg.pose.pose.orientation.y,
+        msg.pose.pose.orientation.z,
+        msg.pose.pose.orientation.w])
+    transform = make_transform(pose, rotation)
+    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)
+    if (msg.encoding == 'bgr8'):
+        image = image[:, :, ::-1]  # convert bgr to rgb
+    stamp = msg.header.stamp.to_sec()
+    return image, stamp

q3dviewer/utils/gl_helper.py

@@ -0,0 +1,40 @@
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+from OpenGL.GL import *
+import numpy as np
+
+
+def set_uniform(shader, content, name):
+    location = glGetUniformLocation(shader, name)
+    if location == -1:
+        raise ValueError(
+            f"Uniform '{name}' not found in shader program {shader}.")
+
+    if isinstance(content, int):
+        glUniform1i(location, content)
+    elif isinstance(content, float):
+        glUniform1f(location, content)
+    elif isinstance(content, np.ndarray):
+        if content.ndim == 1:
+            if content.shape[0] == 2:
+                glUniform2f(location, *content)
+            elif content.shape[0] == 3:
+                glUniform3f(location, *content)
+            else:
+                raise ValueError(
+                    f"Unsupported 1D array size: {content.shape}.")
+        elif content.ndim == 2:
+            if content.shape == (4, 4):
+                glUniformMatrix4fv(location, 1, GL_FALSE,
+                                   content.T.astype(np.float32))
+            else:
+                raise ValueError(
+                    f"Unsupported 2D array size: {content.shape}.")
+        else:
+            raise ValueError(f"Unsupported array dimension: {content.ndim}.")
+    else:
+        raise TypeError(
+            f"Unsupported type for uniform '{name}': {type(content)}.")

q3dviewer/utils/maths.py

@@ -0,0 +1,168 @@
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+import numpy as np
+
+
+def frustum(left, right, bottom, top, near, far):
+    # see https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glFrustum.xml
+    if near <= 0 or far <= 0 or near >= far or left == right or bottom == top:
+        print("Invalid frustum parameters.")
+        return None
+    matrix = np.zeros((4, 4), dtype=np.float32)
+    matrix[0, 0] = 2.0 * near / (right - left)
+    matrix[0, 2] = (right + left) / (right - left)
+    matrix[1, 1] = 2.0 * near / (top - bottom)
+    matrix[1, 2] = (top + bottom) / (top - bottom)
+    matrix[2, 2] = -(far + near) / (far - near)
+    matrix[2, 3] = -2.0 * far * near / (far - near)
+    matrix[3, 2] = -1.0
+    return matrix
+
+
+def rainbow(scalars, scalar_min=0, scalar_max=255):
+    range = scalar_max - scalar_min
+    values = 1.0 - (scalars - scalar_min) / range
+    # values = (scalars - scalar_min) / range  # using inverted color
+    colors = np.zeros([scalars.shape[0], 3], dtype=np.float32)
+    values = np.clip(values, 0, 1)
+
+    h = values * 5.0 + 1.0
+    i = np.floor(h).astype(int)
+    f = h - i
+    f[np.logical_not(i % 2)] = 1 - f[np.logical_not(i % 2)]
+    n = 1 - f
+
+    # idx = i <= 1
+    colors[i <= 1, 0] = n[i <= 1] * 255
+    colors[i <= 1, 1] = 0
+    colors[i <= 1, 2] = 255
+
+    colors[i == 2, 0] = 0
+    colors[i == 2, 1] = n[i == 2] * 255
+    colors[i == 2, 2] = 255
+
+    colors[i == 3, 0] = 0
+    colors[i == 3, 1] = 255
+    colors[i == 3, 2] = n[i == 3] * 255
+
+    colors[i == 4, 0] = n[i == 4] * 255
+    colors[i == 4, 1] = 255
+    colors[i == 4, 2] = 0
+
+    colors[i >= 5, 0] = 255
+    colors[i >= 5, 1] = n[i >= 5] * 255
+    colors[i >= 5, 2] = 0
+    return colors
+
+
+def euler_to_matrix(rpy):
+    roll, pitch, yaw = rpy
+    Rx = np.array([[1, 0, 0],
+                   [0, np.cos(roll), -np.sin(roll)],
+                   [0, np.sin(roll), np.cos(roll)]])
+    Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)],
+                   [0, 1, 0],
+                   [-np.sin(pitch), 0, np.cos(pitch)]])
+    Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
+                   [np.sin(yaw), np.cos(yaw), 0],
+                   [0, 0, 1]])
+    R = Rz @ Ry @ Rx
+    return R
+
+
+def matrix_to_euler(R):
+    sy = np.sqrt(R[0, 0]**2 + R[1, 0]**2)
+    singular = sy < 1e-6  # Check for gimbal lock
+    if not singular:
+        roll = np.arctan2(R[2, 1], R[2, 2])   # X-axis rotation
+        pitch = np.arctan2(-R[2, 0], sy)      # Y-axis rotation
+        yaw = np.arctan2(R[1, 0], R[0, 0])    # Z-axis rotation
+    else:
+        # Gimbal lock case
+        roll = np.arctan2(-R[1, 2], R[1, 1])
+        pitch = np.arctan2(-R[2, 0], sy)
+        yaw = 0  # Arbitrarily set yaw to 0
+
+    return np.array([roll, pitch, yaw])
+
+
+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([x, y, z, w])
+
+
+def quaternion_to_matrix(quaternion):
+    x, y, z, w = quaternion
+    q = np.array(quaternion[:4], dtype=np.float64, copy=True)
+    n = np.linalg.norm(q)
+    if np.any(n == 0.0):
+        raise ZeroDivisionError("bad quaternion input")
+    else:
+        m = np.empty((3, 3))
+        m[0, 0] = 1.0 - 2*(y**2 + z**2)/n
+        m[0, 1] = 2*(x*y - z*w)/n
+        m[0, 2] = 2*(x*z + y*w)/n
+        m[1, 0] = 2*(x*y + z*w)/n
+        m[1, 1] = 1.0 - 2*(x**2 + z**2)/n
+        m[1, 2] = 2*(y*z - x*w)/n
+        m[2, 0] = 2*(x*z - y*w)/n
+        m[2, 1] = 2*(y*z + x*w)/n
+        m[2, 2] = 1.0 - 2*(x**2 + y**2)/n
+        return m
+
+
+def make_transform(pose, rotation):
+    transform = np.eye(4)
+    transform[0:3, 0:3] = quaternion_to_matrix(rotation)
+    transform[0:3, 3] = pose
+    return transform
+
+def makeT(R, t):
+    T = np.eye(4)
+    T[0:3, 0:3] = R
+    T[0:3, 3] = t
+    return T
+
+def makeRt(T):
+    R = T[0:3, 0:3]
+    t = T[0:3, 3]
+    return R, t
+
+def hex_to_rgba(hex_color):
+    color_flat = int(hex_color[1:], 16)
+    red = (color_flat >> 16) & 0xFF
+    green = (color_flat >> 8) & 0xFF
+    blue = color_flat & 0xFF
+    return (red / 255.0, green / 255.0, blue / 255.0, 1.0)
+
+# euler = np.array([1, 0.1, 0.1])
+# euler_angles = matrix_to_euler(euler_to_matrix(euler))
+# print("Euler Angles:", euler_angles)

q3dviewer/utils/range_slider.py

@@ -0,0 +1,86 @@
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+from PySide6.QtCore import Qt, Signal
+from PySide6.QtGui import QPainter, QColor
+from PySide6.QtWidgets import QSlider
+
+
+class RangeSlider(QSlider):
+    # Signal emitted when the range changes
+    rangeChanged = Signal(int, int)
+
+    def __init__(self, orientation=Qt.Horizontal,
+                 parent=None, vmin=0, vmax=255):
+        super().__init__(orientation, parent)
+        self.setMinimum(vmin)
+        self.setMaximum(vmax)
+        self.lower_value = vmin
+        self.upper_value = vmax
+        self.active_handle = None
+        self.setTickPosition(QSlider.NoTicks)  # Hide original ticks
+        # Hide slider handle
+        self.setStyleSheet("QSlider::handle { background: transparent; }")
+
+    def mousePressEvent(self, event):
+        """Override to handle which handle is selected."""
+        pos = self.pixelPosToValue(event.pos())
+        if abs(pos - self.lower_value) < abs(pos - self.upper_value):
+            self.active_handle = "lower"
+        else:
+            self.active_handle = "upper"
+
+    def mouseMoveEvent(self, event):
+        """Override to update handle positions."""
+        if event.buttons() != Qt.LeftButton:
+            return
+
+        pos = self.pixelPosToValue(event.pos())
+        if self.active_handle == "lower":
+            self.lower_value = max(
+                self.minimum(), min(pos, self.upper_value - 1))
+        elif self.active_handle == "upper":
+            self.upper_value = min(
+                self.maximum(), max(pos, self.lower_value + 1))
+        self.rangeChanged.emit(self.lower_value, self.upper_value)
+        self.update()
+
+    def paintEvent(self, event):
+        """Override to paint custom range handles."""
+        painter = QPainter(self)
+
+        # Draw the range bar
+        bar_color = QColor(200, 200, 200)  # Gray bar
+        highlight_color = QColor(100, 100, 255)  # Blue for selected range
+        painter.setPen(Qt.NoPen)
+
+        bar_height = 6
+        bar_y = self.height() // 2 - bar_height // 2
+        painter.setBrush(bar_color)
+        painter.drawRect(0, bar_y, self.width(), bar_height)
+
+        # Draw the selected range
+        lower_x = int(self.valueToPixelPos(self.lower_value))
+        upper_x = int(self.valueToPixelPos(self.upper_value))
+        painter.setBrush(highlight_color)
+        painter.drawRect(lower_x, bar_y, upper_x - lower_x, bar_height)
+
+        # Draw the range handles
+        handle_color = QColor(50, 50, 255)  # Blue handles
+        painter.setBrush(handle_color)
+        painter.drawEllipse(lower_x - 5, bar_y - 4, 12, 12)
+        painter.drawEllipse(upper_x - 5, bar_y - 4, 12, 12)
+
+        painter.end()
+
+    def pixelPosToValue(self, pos):
+        """Convert pixel position to slider value."""
+        return self.minimum() + (self.maximum() - self.minimum()) \
+            * pos.x() / self.width()
+
+    def valueToPixelPos(self, value):
+        """Convert slider value to pixel position."""
+        return self.width() * (value - self.minimum()) /\
+            (self.maximum() - self.minimum())