q3dviewer 1.0.0__py3-none-any.whl → 1.0.2__py3-none-any.whl

q3dviewer/__init__.py

@@ -2,3 +2,18 @@ from q3dviewer.utils import *
 from q3dviewer.custom_items import *
 from q3dviewer.viewer_widget import *
 from q3dviewer.basic_viewer import *
+from q3dviewer.tools.cloud_viewer import main as cloud_viewer
+from q3dviewer.tools.ros_viewer import main as ros_viewer
+from q3dviewer.tools.mesh_viewer import main as mesh_viewer
+from q3dviewer.tools.gaussian_viewer import main as gaussian_viewer
+from q3dviewer.tools.lidar_cam_calib import main as lidar_cam_calib
+from q3dviewer.tools.lidar_calib import main as lidar_calib
+
+__all__ = [
+    'cloud_viewer',
+    'ros_viewer',
+    'mesh_viewer',
+    'gaussian_viewer',
+    'lidar_cam_calib',
+    'lidar_calib'
+]

q3dviewer/tools/__init__.py

@@ -0,0 +1 @@
+# tools/__init__.py

q3dviewer/tools/cloud_viewer.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+
+import numpy as np
+from pypcd4 import PointCloud
+import q3dviewer as q3d
+
+
+class CloudViewer(q3d.Viewer):
+    def __init__(self, **kwargs):
+        super(CloudViewer, self).__init__(**kwargs)
+        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 = q3d.QApplication(['Cloud Viewer'])
+    viewer = CloudViewer(name='Cloud Viewer')
+    cloud_item = q3d.CloudIOItem(size=1, alpha=0.1)
+    axis_item = q3d.GLAxisItem(size=0.5, width=5)
+    grid_item = q3d.GridItem(size=1000, spacing=20)
+
+    viewer.addItems(
+        {'cloud': cloud_item, 'grid': grid_item, 'axis': axis_item})
+
+    if args.pcd:
+        pcd_fn = args.pcd
+        viewer.openCloudFile(pcd_fn)
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/example_viewer.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import threading
+import time
+import q3dviewer as q3d
+
+
+def update(viewer):
+    i = 0.0
+    while True:
+        i += 0.05
+        time.sleep(0.1)
+        viewer['traj'].setData(np.array([np.sin(i) * i, np.cos(i) * i, i]))
+
+
+def main():
+    app = q3d.QApplication([])
+
+    axis_item = q3d.GLAxisItem(size=0.5, width=5)
+    grid_item = q3d.GridItem(size=10, spacing=1)
+    traj_item = q3d.TrajectoryItem(width=2)
+
+    viewer = q3d.Viewer(name='example')
+    th = threading.Thread(target=update, args=(viewer, ))
+    th.start()
+
+    viewer.addItems({
+        'grid': grid_item,
+        'axis': axis_item,
+        'traj': traj_item})
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/gaussian_viewer.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import q3dviewer as q3d
+from q3dviewer.gau_io import load_gs, rotate_gaussian
+
+
+class GuassianViewer(q3d.Viewer):
+    def __init__(self):
+        super(GuassianViewer, self).__init__(name="Guassian 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.openGuassianFile(file_path)
+
+    def openGuassianFile(self, file):
+        gau_item = self['gaussian']
+        if gau_item is None:
+            print("Can't find gaussianitem")
+            return
+
+        print("Try to load %s ..." % file)
+        gs = load_gs(file)
+        # convert camera optical frame (b) to camera frame (c).
+        Rcb = np.array([[0, -1, 0],
+                        [0, 0, -1],
+                        [1, 0, 0]]).T
+        gs = rotate_gaussian(Rcb, gs)
+        gs_data = gs.view(np.float32).reshape(gs.shape[0], -1)
+        gau_item.setData(gs_data=gs_data)
+
+
+def main():
+    app = q3d.QApplication(['Guassian Viewer'])
+    viewer = GuassianViewer(name='Guassian Viewer')
+
+    grid_item = q3d.GridItem(size=1000, spacing=20)
+    gau_item = q3d.GaussianItem()
+
+    viewer.addItems({'grid': grid_item, 'gaussian': gau_item})
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/lidar_calib.py

@@ -0,0 +1,274 @@
+#!/usr/bin/env python3
+
+from sensor_msgs.msg import PointCloud2
+from q3dviewer import *
+import rospy
+import numpy as np
+import argparse
+import open3d as o3d
+
+viewer = None
+cloud0_accum = None
+clouds0 = []
+cloud1_accum = None
+clouds1 = []
+
+
+class CustomDoubleSpinBox(QDoubleSpinBox):
+    def __init__(self, decimals=4, *args, **kwargs):
+        self.decimals = decimals
+        super().__init__(*args, **kwargs)
+        self.setDecimals(self.decimals)  # Set the number of decimal places
+
+    def textFromValue(self, value):
+        return f"{value:.{self.decimals}f}"
+
+    def valueFromText(self, text):
+        return float(text)
+
+
+class ViewerWithPanel(Viewer):
+    def __init__(self, **kwargs):
+        self.t01 = np.array([0, 0, 0])
+        self.R01 = np.eye(3)
+        self.cloud_num = 10
+        self.radius = 0.2
+        super().__init__(**kwargs)
+
+    def initUI(self):
+        centerWidget = QWidget()
+        self.setCentralWidget(centerWidget)
+        main_layout = QHBoxLayout()
+        centerWidget.setLayout(main_layout)
+
+        # Create a vertical layout for the settings
+        setting_layout = QVBoxLayout()
+
+        # Add XYZ spin boxes
+        label_xyz = QLabel("Set XYZ:")
+        setting_layout.addWidget(label_xyz)
+        self.box_x = CustomDoubleSpinBox()
+        self.box_x.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_x)
+        self.box_y = CustomDoubleSpinBox()
+        self.box_y.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_y)
+        self.box_z = CustomDoubleSpinBox()
+        self.box_z.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_z)
+        self.box_x.setRange(-100.0, 100.0)
+        self.box_y.setRange(-100.0, 100.0)
+        self.box_z.setRange(-100.0, 100.0)
+
+        # Add RPY spin boxes
+        label_rpy = QLabel("Set Roll-Pitch-Yaw:")
+        setting_layout.addWidget(label_rpy)
+        self.box_roll = CustomDoubleSpinBox()
+        self.box_roll.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_roll)
+        self.box_pitch = CustomDoubleSpinBox()
+        self.box_pitch.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_pitch)
+        self.box_yaw = CustomDoubleSpinBox()
+        self.box_yaw.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_yaw)
+        self.box_roll.setRange(-np.pi, np.pi)
+        self.box_pitch.setRange(-np.pi, np.pi)
+        self.box_yaw.setRange(-np.pi, np.pi)
+
+        label_cloud_num = QLabel("Set cloud frame number:")
+        setting_layout.addWidget(label_cloud_num)
+        self.box_cloud_num = QSpinBox()
+        self.box_cloud_num.setValue(self.cloud_num)
+        self.box_cloud_num.setRange(1, 100)
+        self.box_cloud_num.valueChanged.connect(self.update_cloud_num)
+        setting_layout.addWidget(self.box_cloud_num)
+
+        label_res = QLabel("The lidar0-lidar1 trans and quat:")
+        setting_layout.addWidget(label_res)
+        self.line_trans = QLineEdit()
+        self.line_trans.setReadOnly(True)
+        setting_layout.addWidget(self.line_trans)
+        self.line_quat = QLineEdit()
+        self.line_quat.setReadOnly(True)
+        setting_layout.addWidget(self.line_quat)
+
+        label_matching_setting = QLabel("Set matching radius:")
+        setting_layout.addWidget(label_matching_setting)
+        self.box_radius = CustomDoubleSpinBox(decimals=2)
+        self.box_radius.setSingleStep(0.1)
+        self.box_radius.setRange(0.1, 3.0)
+        self.box_radius.setValue(self.radius)
+        setting_layout.addWidget(self.box_radius)
+
+        self.icp_button = QPushButton("Auto Scan Matching")
+        self.icp_button.clicked.connect(self.perform_icp)
+        setting_layout.addWidget(self.icp_button)
+
+        self.line_trans.setText(np.array2string(self.t01, formatter={
+                                'float_kind': lambda x: "%.4f" % x}, separator=', '))
+        quat = matrix_to_quaternion(self.R01)
+        self.line_quat.setText(np.array2string(
+            quat, formatter={'float_kind': lambda x: "%.4f" % x}, separator=', '))
+
+        # Connect spin boxes to methods
+        self.box_x.valueChanged.connect(self.update_xyz)
+        self.box_y.valueChanged.connect(self.update_xyz)
+        self.box_z.valueChanged.connect(self.update_xyz)
+        self.box_roll.valueChanged.connect(self.update_rpy)
+        self.box_pitch.valueChanged.connect(self.update_rpy)
+        self.box_yaw.valueChanged.connect(self.update_rpy)
+
+        # Add a stretch to push the widgets to the top
+        setting_layout.addStretch(1)
+
+        self.viewerWidget = self.vw()
+        main_layout.addLayout(setting_layout)
+        main_layout.addWidget(self.viewerWidget, 1)
+
+        timer = QtCore.QTimer(self)
+        timer.setInterval(20)  # period, in milliseconds
+        timer.timeout.connect(self.update)
+        self.viewerWidget.setCameraPosition(distance=5)
+        self.viewerWidget.setBKColor('#ffffff')
+        timer.start()
+
+    def update_radius(self):
+        self.radius = self.box_radius.value()
+
+    def update_cloud_num(self):
+        self.cloud_num = self.box_cloud_num.value()
+
+    def update_xyz(self):
+        x = self.box_x.value()
+        y = self.box_y.value()
+        z = self.box_z.value()
+        self.t01 = np.array([x, y, z])
+        self.line_trans.setText(f"[{x:.6f}, {y:.6f}, {z:.6f}]")
+
+    def update_rpy(self):
+        roll = self.box_roll.value()
+        pitch = self.box_pitch.value()
+        yaw = self.box_yaw.value()
+        self.R01 = euler_to_matrix(np.array([roll, pitch, yaw]))
+        quat = matrix_to_quaternion(self.R01)
+        self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
+
+    def perform_icp(self):
+        global cloud0_accum, cloud1_accum
+
+        if cloud0_accum is not None and cloud1_accum is not None:
+            # Convert to Open3D point clouds
+            cloud0_o3d = o3d.geometry.PointCloud()
+            cloud0_o3d.points = o3d.utility.Vector3dVector(cloud0_accum['xyz'])
+            cloud1_o3d = o3d.geometry.PointCloud()
+            cloud1_o3d.points = o3d.utility.Vector3dVector(cloud1_accum['xyz'])
+            voxel_size = 0.1
+            cloud0_o3d = cloud0_o3d.voxel_down_sample(voxel_size)
+            cloud1_o3d = cloud1_o3d.voxel_down_sample(voxel_size)
+            cloud0_o3d.estimate_normals(
+                search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=self.radius, max_nn=30))
+            trans_init = np.eye(4)
+            trans_init[:3, :3] = self.R01
+            trans_init[:3, 3] = self.t01
+
+            criteria = o3d.pipelines.registration.ICPConvergenceCriteria(
+                relative_fitness=1e-4,
+                max_iteration=50)
+
+            # Auto Scan Matching
+            reg_p2p = o3d.pipelines.registration.registration_icp(
+                cloud1_o3d, cloud0_o3d, self.radius, trans_init,
+                o3d.pipelines.registration.TransformationEstimationPointToPlane(),
+                criteria)
+
+            transformation_icp = reg_p2p.transformation
+            # print("ICP Transformation:", transformation_icp)
+
+            # Update R01 and t01 with ICP result
+            R01 = transformation_icp[:3, :3]
+            t01 = transformation_icp[:3, 3]
+
+            # Update the UI with new values
+            quat = matrix_to_quaternion(R01)
+            rpy = matrix_to_euler(R01)
+            self.box_roll.setValue(rpy[0])
+            self.box_pitch.setValue(rpy[1])
+            self.box_yaw.setValue(rpy[2])
+            self.box_x.setValue(t01[0])
+            self.box_y.setValue(t01[1])
+            self.box_z.setValue(t01[2])
+            self.line_trans.setText(f"[{t01[0]:.6f}, {t01[1]:.6f}, {t01[2]:.6f}]")
+            self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
+            self.t01 = t01
+            self.R01 = R01
+            print("Matching results")
+            print(f"translation: [{t01[0]:.6f}, {t01[1]:.6f}, {t01[2]:.6f}]")
+            print(f"Roll-Pitch-Yaw: [{rpy[0]:.6f}, {rpy[1]:.6f}, {rpy[2]:.6f}]")
+            print(f"Quaternion: [{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
+
+def msg2Cloud(data):
+    pc = PointCloud.from_msg(data).pc_data
+    data_type = [('xyz', '<f4', (3,)), ('color', '<u4')]
+    color = pc['intensity'].astype(np.uint32)
+    cloud = np.rec.fromarrays(
+        [np.stack([pc['x'], pc['y'], pc['z']], axis=1), color],
+        dtype=data_type)
+    return cloud
+
+
+def scan0CB(data):
+    global viewer, clouds0, cloud0_accum
+    cloud = msg2Cloud(data)
+    while len(clouds0) > viewer.cloud_num:
+        clouds0.pop(0)
+    clouds0.append(cloud)
+    cloud0_accum = np.concatenate(clouds0)
+    viewer['scan0'].setData(data=cloud0_accum)
+
+
+def scan1CB(data):
+    global viewer, clouds1, cloud1_accum
+    cloud = msg2Cloud(data)
+    while len(clouds1) > viewer.cloud_num:
+        clouds1.pop(0)
+    clouds1.append(cloud)
+    cloud1_accum = np.concatenate(clouds1)
+    cloud0_accum_new = cloud1_accum.copy()
+    cloud0_accum_new['xyz'] = (
+        viewer.R01 @ cloud1_accum['xyz'].T + viewer.t01[:, np.newaxis]).T
+
+    viewer['scan1'].setData(data=cloud0_accum_new)
+
+
+def main():
+    global viewer
+    # Set up argument parser
+    parser = argparse.ArgumentParser(
+        description="Configure topic names for LiDAR, image, and camera info.")
+    parser.add_argument('--lidar0', type=str, default='/lidar0',
+                        help="Topic name for LiDAR data")
+    parser.add_argument('--lidar1', type=str, default='/lidar1',
+                        help="Topic name for camera image data")
+    args = parser.parse_args()
+
+    app = QApplication(["LiDAR Calib"])
+    viewer = ViewerWithPanel(name='LiDAR Calib')
+    grid_item = GridItem(size=10, spacing=1, color=(0, 0, 0, 70))
+    scan0_item = CloudItem(size=2, alpha=1, color_mode='#FF0000')
+    scan1_item = CloudItem(size=2, alpha=1, color_mode='#00FF00')
+    viewer.addItems(
+        {'scan0': scan0_item, 'scan1': scan1_item, 'grid': grid_item})
+
+    rospy.init_node('lidar_calib', anonymous=True)
+
+    # Use topic names from arguments
+    rospy.Subscriber(args.lidar0, PointCloud2, scan0CB, queue_size=1)
+    rospy.Subscriber(args.lidar1, PointCloud2, scan1CB, queue_size=1)
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/lidar_cam_calib.py

@@ -0,0 +1,299 @@
+#!/usr/bin/env python3
+
+from sensor_msgs.msg import PointCloud2, Image, CameraInfo
+from q3dviewer import *
+import rospy
+import numpy as np
+import argparse
+import cv2
+
+viewer = None
+cloud_accum = None
+cloud_accum_color = None
+clouds = []
+remap_info = None
+K = None
+
+
+class CustomDoubleSpinBox(QDoubleSpinBox):
+    def __init__(self, decimals=4, *args, **kwargs):
+        self.decimals = decimals
+        super().__init__(*args, **kwargs)
+        self.setDecimals(self.decimals)  # Set the number of decimal places
+
+    def textFromValue(self, value):
+        return f"{value:.{self.decimals}f}"
+
+    def valueFromText(self, text):
+        return float(text)
+
+
+class ViewerWithPanel(Viewer):
+    def __init__(self, **kwargs):
+        # b: camera body frame
+        # c: camera image frame
+        # l: lidar frame
+        self.Rbl = np.eye(3)
+        self.Rcb = np.array([[0, -1, 0],
+                             [0, 0, -1],
+                             [1, 0, 0]])
+        self.tcl = np.array([0, 0, 0])
+        self.Rcl = self.Rcb @ self.Rbl
+        self.psize = 2
+        self.cloud_num = 1
+        self.en_rgb = False
+        super().__init__(**kwargs)
+
+    def initUI(self):
+        centerWidget = QWidget()
+        self.setCentralWidget(centerWidget)
+        main_layout = QHBoxLayout()
+        centerWidget.setLayout(main_layout)
+
+        # Create a vertical layout for the settings
+        setting_layout = QVBoxLayout()
+
+        # Add a checkbox for RGB
+        self.checkbox_rgb = QCheckBox("Enable RGB Cloud")
+        self.checkbox_rgb.setChecked(False)
+        setting_layout.addWidget(self.checkbox_rgb)
+        self.checkbox_rgb.stateChanged.connect(
+            self.checkbox_changed)
+
+        # Add XYZ spin boxes
+        label_xyz = QLabel("Set XYZ:")
+        setting_layout.addWidget(label_xyz)
+        self.box_x = CustomDoubleSpinBox()
+        self.box_x.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_x)
+        self.box_y = CustomDoubleSpinBox()
+        self.box_y.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_y)
+        self.box_z = CustomDoubleSpinBox()
+        self.box_z.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_z)
+        self.box_x.setRange(-100.0, 100.0)
+        self.box_y.setRange(-100.0, 100.0)
+        self.box_y.setRange(-100.0, 100.0)
+
+        # Add RPY spin boxes
+        label_rpy = QLabel("Set Roll-Pitch-Yaw:")
+        setting_layout.addWidget(label_rpy)
+        self.box_roll = CustomDoubleSpinBox()
+        self.box_roll.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_roll)
+        self.box_pitch = CustomDoubleSpinBox()
+        self.box_pitch.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_pitch)
+        self.box_yaw = CustomDoubleSpinBox()
+        self.box_yaw.setSingleStep(0.01)
+        setting_layout.addWidget(self.box_yaw)
+        self.box_roll.setRange(-np.pi, np.pi)
+        self.box_pitch.setRange(-np.pi, np.pi)
+        self.box_yaw.setRange(-np.pi, np.pi)
+
+        label_psize = QLabel("Set point size:")
+        setting_layout.addWidget(label_psize)
+        self.box_psize = QSpinBox()
+        self.box_psize.setValue(self.psize)
+        self.box_psize.setRange(0, 5)
+        self.box_psize.valueChanged.connect(self.update_psize)
+        setting_layout.addWidget(self.box_psize)
+
+        label_cloud_num = QLabel("Set cloud frame number:")
+        setting_layout.addWidget(label_cloud_num)
+        self.box_cloud_num = QSpinBox()
+        self.box_cloud_num.setValue(self.cloud_num)
+        self.box_cloud_num.setRange(1, 100)
+        self.box_cloud_num.valueChanged.connect(self.update_cloud_num)
+        setting_layout.addWidget(self.box_cloud_num)
+
+        label_res = QLabel("The cam-lidar quat and translation:")
+        setting_layout.addWidget(label_res)
+        self.line_quat = QLineEdit()
+        self.line_quat.setReadOnly(True)
+        setting_layout.addWidget(self.line_quat)
+        self.line_trans = QLineEdit()
+        self.line_trans.setReadOnly(True)
+        setting_layout.addWidget(self.line_trans)
+
+        self.line_trans.setText(f"[{self.tcl[0]:.6f}, {self.tcl[1]:.6f}, {self.tcl[2]:.6f}]")
+        quat = matrix_to_quaternion(self.Rcl)
+        self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
+
+        # Connect spin boxes to methods
+        self.box_x.valueChanged.connect(self.update_xyz)
+        self.box_y.valueChanged.connect(self.update_xyz)
+        self.box_z.valueChanged.connect(self.update_xyz)
+        self.box_roll.valueChanged.connect(self.update_rpy)
+        self.box_pitch.valueChanged.connect(self.update_rpy)
+        self.box_yaw.valueChanged.connect(self.update_rpy)
+
+        # Add a stretch to push the widgets to the top
+        setting_layout.addStretch(1)
+
+        self.viewerWidget = self.vw()
+        main_layout.addLayout(setting_layout)
+        main_layout.addWidget(self.viewerWidget, 1)
+
+        timer = QtCore.QTimer(self)
+        timer.setInterval(20)  # period, in milliseconds
+        timer.timeout.connect(self.update)
+        self.viewerWidget.setCameraPosition(distance=5)
+        self.viewerWidget.setBKColor('#ffffff')
+        timer.start()
+
+    def update_psize(self):
+        self.psize = self.box_psize.value()
+
+    def update_cloud_num(self):
+        self.cloud_num = self.box_cloud_num.value()
+
+    def update_xyz(self):
+        x = self.box_x.value()
+        y = self.box_y.value()
+        z = self.box_z.value()
+        self.tcl = np.array([x, y, z])
+        self.line_trans.setText(f"[{x:.6f}, {y:.6f}, {x:.6f}]")
+
+    def update_rpy(self):
+        roll = self.box_roll.value()
+        pitch = self.box_pitch.value()
+        yaw = self.box_yaw.value()
+        self.Rbl = euler_to_matrix(np.array([roll, pitch, yaw]))
+        self.Rcl = self.Rcb @ self.Rbl
+        quat = matrix_to_quaternion(self.Rcl)
+        self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
+
+    def checkbox_changed(self, state):
+        if state == QtCore.Qt.Checked:
+            self.en_rgb = True
+        else:
+            self.en_rgb = False
+
+
+def cameraInfoCB(data):
+    global remap_info, K
+    if remap_info is None:  # Initialize only once
+        K = np.array(data.K).reshape(3, 3)
+        D = np.array(data.D)
+        rospy.loginfo("Camera intrinsic parameters set")
+        height = data.height
+        width = data.width
+        mapx, mapy = cv2.initUndistortRectifyMap(
+            K, D, None, K, (width, height), cv2.CV_32FC1)
+        remap_info = [mapx, mapy]
+
+
+def scanCB(data):
+    global viewer, clouds, cloud_accum, cloud_accum_color
+    pc = PointCloud.from_msg(data).pc_data
+    data_type = viewer['scan'].data_type
+    color = pc['intensity'].astype(np.uint32)
+    cloud = np.rec.fromarrays(
+        [np.stack([pc['x'], pc['y'], pc['z']], axis=1), color],
+        dtype=data_type)
+    while len(clouds) > viewer.cloud_num:
+        clouds.pop(0)
+    clouds.append(cloud)
+    cloud_accum = np.concatenate(clouds)
+
+    if cloud_accum_color is not None and viewer.en_rgb:
+        viewer['scan'].setData(data=cloud_accum_color)
+        viewer['scan'].setColorMode('RGB')
+    else:
+        viewer['scan'].setData(data=cloud_accum)
+        viewer['scan'].setColorMode('I')
+
+
+def draw_larger_points(image, points, colors, radius):
+    for dx in range(-radius + 1, radius):
+        for dy in range(-radius + 1, radius):
+            distance = np.sqrt(dx**2 + dy**2)
+            if distance <= radius:
+                x_indices = points[:, 0] + dx
+                y_indices = points[:, 1] + dy
+                image[y_indices, x_indices] = colors
+    return image
+
+
+def imageCB(data):
+    global cloud_accum, cloud_accum_color, remap_info, K
+    if remap_info is None:
+        rospy.logwarn("Camera parameters not yet received.")
+        return
+
+    image = np.frombuffer(data.data, dtype=np.uint8).reshape(
+        data.height, data.width, -1)
+    if data.encoding == 'bgr8':
+        image = image[:, :, ::-1]  # convert BGR to RGB
+
+    # Undistort the image
+    image_un = cv2.remap(image, remap_info[0], remap_info[1], cv2.INTER_LINEAR)
+
+    if cloud_accum is not None:
+        cloud_local = cloud_accum.copy()
+        tcl = viewer.tcl
+        Rcl = viewer.Rcl
+        pl = cloud_local['xyz']
+        po = (Rcl @ pl.T + tcl[:, np.newaxis]).T
+        u = (K @ po.T).T
+        u_mask = u[:, 2] != 0
+        u = u[:, :2][u_mask] / u[:, 2][u_mask][:, np.newaxis]
+
+        radius = viewer.psize  # Radius of the points to be drawn
+        u = np.round(u).astype(np.int32)
+        valid_x = (u[:, 0] >= radius) & (u[:, 0] < image_un.shape[1]-radius)
+        valid_y = (u[:, 1] >= radius) & (u[:, 1] < image_un.shape[0]-radius)
+        valid_points = valid_x & valid_y
+        u = u[valid_points]
+
+        intensity = cloud_local['color'][u_mask][valid_points]
+        intensity_color = rainbow(intensity).astype(np.uint8)
+        draw_image = image_un.copy()
+        draw_image = draw_larger_points(draw_image, u, intensity_color, radius)
+        rgb = image_un[u[:, 1], u[:, 0]]
+        xyz = cloud_local['xyz'][u_mask][valid_points]
+        color = (intensity.astype(np.uint32) << 24) | \
+                (rgb[:, 0].astype(np.uint32) << 16) | \
+                (rgb[:, 1].astype(np.uint32) << 8) | \
+                (rgb[:, 2].astype(np.uint32) << 0)
+        cloud_accum_color = np.rec.fromarrays(
+            [xyz, color],
+            dtype=viewer['scan'].data_type)
+        viewer['img'].setData(data=draw_image)
+
+
+def main():
+    global viewer
+    # Set up argument parser
+    parser = argparse.ArgumentParser(
+        description="Configure topic names for LiDAR, image, and camera info.")
+    parser.add_argument(
+        '--lidar', type=str, default='lidar', help="Topic of LiDAR data")
+    parser.add_argument('--camera', type=str, default='image_raw',
+                        help="Topic of camera image data")
+    parser.add_argument('--camera_info', type=str,
+                        default='camera_info', help="Topic of camera info")
+    args = parser.parse_args()
+
+    app = QApplication(['LiDAR Cam Calib'])
+    viewer = ViewerWithPanel(name='LiDAR Cam Calib')
+    grid_item = GridItem(size=10, spacing=1, color=(0, 0, 0, 70))
+    scan_item = CloudItem(size=2, alpha=1, color_mode='I')
+    img_item = ImageItem(pos=np.array([0, 0]), size=np.array([800, 600]))
+    viewer.addItems({'scan': scan_item, 'grid': grid_item, 'img': img_item})
+
+    rospy.init_node('lidar_cam_calib', anonymous=True)
+
+    # Use topic names from arguments
+    rospy.Subscriber(args.lidar, PointCloud2, scanCB, queue_size=1)
+    rospy.Subscriber(args.camera, Image, imageCB, queue_size=1)
+    rospy.Subscriber(args.camera_info, CameraInfo, cameraInfoCB, queue_size=1)
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/mesh_viewer.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import q3dviewer as q3d
+from pyqtgraph.opengl import GLMeshItem
+from stl import mesh
+
+
+class MeshViewer(q3d.Viewer):
+    def __init__(self):
+        super(MeshViewer, self).__init__(name="Mesh 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.openMeshFile(file_path)
+
+    def openMeshFile(self, file):
+        mesh_item = self['mesh']
+        if mesh_item is None:
+            print("Can't find meshitem")
+            return
+
+        print("Try to load %s ..." % file)
+        stl_mesh = mesh.Mesh.from_file(file)
+
+        vertices = stl_mesh.points.reshape(-1, 3)
+        faces = np.arange(vertices.shape[0]).reshape(-1, 3)
+        mesh_item.setMeshData(vertexes=vertices, faces=faces)
+
+
+def main():
+    app = q3d.QApplication([])
+    viewer = MeshViewer()
+
+    grid_item = q3d.GridItem(size=1000, spacing=20)
+    # 'glOptions', 'opaque', 'additive' 'translucent'
+    mesh_item = GLMeshItem(smooth=True, drawFaces=True, drawEdges=True,
+                           color=(0, 1, 0, 0.2),
+                           edgeColor=(1, 1, 1, 1),
+                           glOptions='translucent')
+
+    viewer.addItems({'grid': grid_item, 'mesh': mesh_item})
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/ros_viewer.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+
+
+from q3dviewer.utils import make_transform
+import q3dviewer as q3d
+import rospy
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import PointCloud2
+import numpy as np
+import random
+from pypcd4 import PointCloud
+from sensor_msgs.msg import Image
+
+viewer = None
+point_num_per_scan = None
+color_mode = None
+
+
+def odomCB(data):
+    global viewer
+    pose = np.array(
+        [data.pose.pose.position.x,
+         data.pose.pose.position.y,
+         data.pose.pose.position.z])
+    rotation = np.array([
+        data.pose.pose.orientation.x,
+        data.pose.pose.orientation.y,
+        data.pose.pose.orientation.z,
+        data.pose.pose.orientation.w])
+    transform = make_transform(pose, rotation)
+    viewer['odom'].setTransform(transform)
+
+
+def scanCB(data):
+    global viewer
+    global point_num_per_scan
+    global color_mode
+    pc = PointCloud.from_msg(data).pc_data
+    data_type = viewer['scan'].data_type
+    if 'rgb' in pc.dtype.names:
+        color = pc['rgb'].view(np.uint32)
+    else:
+        color = pc['intensity']
+    
+    if color_mode is None:
+        if 'rgb' in pc.dtype.names:
+            color_mode = 'RGB'
+        else:
+            color_mode = 'I'
+        viewer['map'].setColorMode(color_mode)
+    
+    cloud = np.rec.fromarrays(
+        [np.stack([pc['x'], pc['y'], pc['z']], axis=1), color],
+        dtype=data_type)
+    if (cloud.shape[0] > point_num_per_scan):
+        idx = random.sample(range(cloud.shape[0]), point_num_per_scan)
+        cloud = cloud[idx]
+    viewer['map'].setData(data=cloud, append=True)
+    viewer['scan'].setData(data=cloud)
+
+
+def imageCB(data):
+    image = np.frombuffer(data.data, dtype=np.uint8).reshape(
+        data.height, data.width, -1)
+    if (data.encoding == 'bgr8'):
+        image = image[:, :, ::-1]  # convert bgr to rgb
+    viewer['img'].setData(data=image)
+
+
+def main():
+    rospy.init_node('ros_viewer', anonymous=True)
+
+    global viewer
+    global point_num_per_scan
+    point_num_per_scan = 10000
+    map_item = q3d.CloudIOItem(size=1, alpha=0.1, color_mode='RGB')
+    scan_item = q3d.CloudItem(size=2, alpha=1, color_mode='#FFFFFF')
+    odom_item = q3d.GLAxisItem(size=0.5, width=5)
+    grid_item = q3d.GridItem(size=1000, spacing=20)
+    img_item = q3d.ImageItem(pos=np.array([0, 0]), size=np.array([800, 600]))
+
+    app = q3d.QApplication(['ROS Viewer'])
+    viewer = q3d.Viewer(name='ROS Viewer')
+
+    viewer.addItems({'map': map_item, 'scan': scan_item,
+                    'odom': odom_item, 'grid': grid_item, 'img': img_item})
+
+    point_num_per_scan = rospy.get_param("scan_num", 100000)
+    print("point_num_per_scan: %d" % point_num_per_scan)
+    rospy.Subscriber(
+        "/cloud_registered", PointCloud2, scanCB,
+        queue_size=1, buff_size=2**24)
+    rospy.Subscriber(
+        "/odometry", Odometry, odomCB, queue_size=1, buff_size=2**24)
+    rospy.Subscriber('/image', Image, imageCB)
+
+    viewer.show()
+    app.exec_()
+
+
+if __name__ == "__main__":
+    main()

{q3dviewer-1.0.0.dist-info → q3dviewer-1.0.2.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: q3dviewer
-Version: 1.0.0
+Version: 1.0.2
 Requires-Dist: numpy
 Requires-Dist: pyqtgraph
 Requires-Dist: pyqt5

{q3dviewer-1.0.0.dist-info → q3dviewer-1.0.2.dist-info}/RECORD

@@ -1,4 +1,4 @@
-q3dviewer/__init__.py,sha256=BCrGWEtwjdvK4CHgp1e5Wkyhabk6WdvY_7FKJdGJwGE,142
+q3dviewer/__init__.py,sha256=sWX8RiMSEBX4Ebi5SN9Oyij_ruLX1FdY-ZM56_55_Ak,654
 q3dviewer/basic_viewer.py,sha256=A7bMxutP1grnqE5A4tjODGrzNfFq6-c6MlNYj8t54WU,1609
 q3dviewer/gau_io.py,sha256=fKry1eyeFdw_JO5JZqU3l66dzt43NGpHhX89ER_y38A,5900
 q3dviewer/utils.py,sha256=xzUAXUhgxoJyPrnivnyrJWoQQg9LrDPhxgZo1Cp35kQ,4596
@@ -13,8 +13,16 @@ q3dviewer/custom_items/grid_item.py,sha256=WuNzIHBhjAQ6e4fP5aBp5jSgYbrY7wrbP9PhR
 q3dviewer/custom_items/image_item.py,sha256=qOT0r6p8_NcF5GVeJrrR8rJNe68U_jW1qrUDZ05O5hI,5312
 q3dviewer/custom_items/text_item.py,sha256=lbJaXfpeyyxr6lhlMjypk4a079-aUpsrXFjGi-BTzPU,1872
 q3dviewer/custom_items/trajectory_item.py,sha256=9gM5voiYk0j4JMEp3drF7VFYSPsJcEK7mibdx45OQDs,2778
-q3dviewer-1.0.0.dist-info/METADATA,sha256=YkgPKWnfC-R9Q26DXkrAwih_KwyqAeAPVbOgzRwiMIk,189
-q3dviewer-1.0.0.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
-q3dviewer-1.0.0.dist-info/entry_points.txt,sha256=6zSxtJ5cjcpc32sR_wkRqOaeiJFpEuLnJQckZE3niU0,316
-q3dviewer-1.0.0.dist-info/top_level.txt,sha256=HFFDCbGu28txcGe2HPc46A7EPaguBa_b5oH7bufmxHM,10
-q3dviewer-1.0.0.dist-info/RECORD,,
+q3dviewer/tools/__init__.py,sha256=01wG7BGM6VX0QyFBKsqPmyf2e-vrmV_N3-mo-VQ1VBg,20
+q3dviewer/tools/cloud_viewer.py,sha256=qqOLulsVBRnykbWxatHN0eto52sMwF9yOHz35-Ye_SQ,2322
+q3dviewer/tools/example_viewer.py,sha256=aKydEeM_h9AUvgeVvaWqGsL4l7YHzgxvXhXibQhsuqQ,747
+q3dviewer/tools/gaussian_viewer.py,sha256=o_Vu-kKGN6A0VffPcOBZCCDFGfg_56v94IoC0r7mv1Y,1489
+q3dviewer/tools/lidar_calib.py,sha256=f57xgFD2A-YqEwxCo0fgEAtiPnEvhT2NhM0b4lrTaFU,10238
+q3dviewer/tools/lidar_cam_calib.py,sha256=8LHe7rgr1WCXgcyy1dnFoENdfkANf_CDrwVUmZ85HfQ,10794
+q3dviewer/tools/mesh_viewer.py,sha256=QjahZzNy71xIwV-RzlxAtZs8cO57QlOSmMeqJnylS8g,1542
+q3dviewer/tools/ros_viewer.py,sha256=PZ6kod7QT5DdRXudYw_olFkd072GD8GTszovBjTL3Cs,2984
+q3dviewer-1.0.2.dist-info/METADATA,sha256=IicP6A0SV7jW_yVFO-Ejb610ZEgzPR30NPS2qf3U_U0,189
+q3dviewer-1.0.2.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
+q3dviewer-1.0.2.dist-info/entry_points.txt,sha256=6zSxtJ5cjcpc32sR_wkRqOaeiJFpEuLnJQckZE3niU0,316
+q3dviewer-1.0.2.dist-info/top_level.txt,sha256=HFFDCbGu28txcGe2HPc46A7EPaguBa_b5oH7bufmxHM,10
+q3dviewer-1.0.2.dist-info/RECORD,,