q3dviewer 1.1.6__py3-none-any.whl → 1.1.8__py3-none-any.whl

q3dviewer/shaders/cloud_vert.glsl

@@ -1,9 +1,9 @@
+#version 330 core
 /*
 Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
 Distributed under MIT license. See LICENSE for more information.
 */
 
-#version 330 core
 
 layout (location = 0) in vec3 position;
 layout (location = 1) in uint value;
@@ -17,7 +17,7 @@ uniform float vmin = 0;
 uniform float vmax = 255;
 uniform float focal = 1000;
 uniform int point_type = 0; // 0 pixel, 1 flat square, 2 sphere
-uniform float point_size = 0.01;  // World size for each point (meter)
+uniform int point_size = 1;  // World size for each point (pixel or cm)
 out vec4 color;
 
 vec3 getRainbowColor(uint value_raw) {
@@ -47,9 +47,9 @@ void main()
 
     // Calculate point size in pixels based on distance
     if (point_type == 0)
-        gl_PointSize = int(point_size);
+        gl_PointSize = float(point_size);
     else
-        gl_PointSize = point_size / gl_Position.w * focal;
+        gl_PointSize = (float(point_size) * 0.01) / gl_Position.w * focal;
     vec3 c = vec3(1.0, 1.0, 1.0);
     if (color_mode == 1)
     {
@@ -62,6 +62,15 @@ void main()
         c.y = float((value & uint(0x0000FF00)) >> 8)/255.;
         c.x = float((value & uint(0x00FF0000)) >> 16)/255.;
     }
+    else if(color_mode == 3)
+    {
+        uint intensity = value >> 24;
+        float range = vmax - vmin;
+        float value = 1.0 - (float(intensity) - vmin) / range;
+        c.z = value;
+        c.y = value;
+        c.x = value;
+    }
     else
     {
         c.z = float( uint(flat_rgb) & uint(0x000000FF))/255.;

q3dviewer/shaders/gau_frag.glsl

@@ -1,9 +1,9 @@
+#version 430 core
 /*
 this file is modified from GaussianSplattingViewer licensed under the MIT License.
 see https://github.com/limacv/GaussianSplattingViewer/blob/main/shaders/gau_frag.glsl
 */
 
-#version 430 core
 
 in vec3 color;
 in float alpha;

q3dviewer/shaders/gau_prep.glsl

@@ -1,10 +1,10 @@
+#version 430 core
 /*
 We proprocess gaussian using compute shader.
 this file is modified from GaussianSplattingViewer licensed under the MIT License.
 see https://github.com/limacv/GaussianSplattingViewer/blob/main/shaders/gau_vert.glsl
 */
 
-#version 430 core
 
 layout(local_size_x = 256, local_size_y = 1, local_size_z = 1) in;
 

q3dviewer/shaders/gau_vert.glsl

@@ -1,3 +1,4 @@
+#version 430 core
 /*
 Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
 Distributed under MIT license. See LICENSE for more information.
@@ -5,7 +6,6 @@ Distributed under MIT license. See LICENSE for more information.
 
 //draw 2d gaussian using proprocess data.
 
-#version 430 core
 
 #define OFFSET_PREP_U 0
 #define OFFSET_PREP_COVINV 3

q3dviewer/shaders/sort_by_key.glsl

@@ -1,3 +1,4 @@
+#version 430 core
 /*
 Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
 Distributed under MIT license. See LICENSE for more information.
@@ -8,7 +9,6 @@ opengl compute shader.
 sort guassian by depth using bitonic sorter
 */
 
-#version 430 core
 
 layout(local_size_x = 256, local_size_y = 1, local_size_z = 1) in;
 

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,73 @@
+#!/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
+from q3dviewer.utils.cloud_io import load_pcd
+
+cloud, _ = 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)

q3dviewer/tools/cinematographer.py

@@ -0,0 +1,367 @@
+#!/usr/bin/env python3
+
+"""
+Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
+Distributed under MIT license. See LICENSE for more information.
+"""
+
+import numpy as np
+import q3dviewer as q3d
+from PySide6.QtWidgets import QVBoxLayout, QListWidget, QListWidgetItem, QPushButton, QDoubleSpinBox, QCheckBox, QLineEdit, QMessageBox, QLabel, QHBoxLayout
+from PySide6.QtCore import QTimer
+from cloud_viewer import ProgressDialog,  FileLoaderThread
+from PySide6 import QtCore
+from PySide6.QtGui import QKeyEvent
+from q3dviewer import GLWidget
+import imageio.v2 as imageio
+import os
+
+
+class KeyFrame:
+    def __init__(self, Twc):
+        self.Twc = Twc
+        self.linear_velocity = 10
+        self.angular_velocity = 1
+        self.stop_time = 0
+        self.item = q3d.FrameItem(Twc, width=3, color='#0000FF')
+
+
+class CustomGLWidget(GLWidget):
+    def __init__(self, viewer):
+        super().__init__()
+        self.viewer = viewer  # Add a viewer handle
+
+    def keyPressEvent(self, ev: QKeyEvent):
+        if ev.key() == QtCore.Qt.Key_Space:
+            self.viewer.add_key_frame()
+        elif ev.key() == QtCore.Qt.Key_Delete:
+            self.viewer.del_key_frame()
+        super().keyPressEvent(ev)
+
+class CMMViewer(q3d.Viewer):
+    """
+    This class is a subclass of Viewer, which is used to create a cloud movie maker.
+    """
+    def __init__(self, **kwargs):
+        self.key_frames = []
+        self.video_path = os.path.join(os.path.expanduser("~"), "output.mp4")
+        super().__init__(**kwargs, gl_widget_class=lambda: CustomGLWidget(self))
+        # for drop cloud file
+        self.setAcceptDrops(True)
+
+    def add_control_panel(self, main_layout):
+        """
+        Add a control panel to the viewer.
+        """
+        # Create a vertical layout for the settings
+        setting_layout = QVBoxLayout()
+
+        # Buttons to add and delete key frames
+        add_button = QPushButton("Add Key Frame")
+        add_button.clicked.connect(self.add_key_frame)
+        setting_layout.addWidget(add_button)
+        del_button = QPushButton("Delete Key Frame")
+        del_button.clicked.connect(self.del_key_frame)
+        setting_layout.addWidget(del_button)
+
+        # Add play/stop button
+        self.play_button = QPushButton("Play")
+        self.play_button.clicked.connect(self.toggle_playback)
+        setting_layout.addWidget(self.play_button)
+
+        # add a timer to play the frames
+        self.timer = QTimer()
+        self.timer.timeout.connect(self.play_frames)
+        self.current_frame_index = 0
+        self.is_playing = False
+        self.is_recording = False
+
+        # Add record checkbox
+        self.record_checkbox = QCheckBox("Record")
+        self.record_checkbox.stateChanged.connect(self.toggle_recording)
+        setting_layout.addWidget(self.record_checkbox)
+
+        # Add video path setting
+        video_path_layout = QHBoxLayout()
+        label_video_path = QLabel("Video Path:")
+        video_path_layout.addWidget(label_video_path)
+        self.video_path_edit = QLineEdit()
+        self.video_path_edit.setText(self.video_path)
+        self.video_path_edit.textChanged.connect(self.update_video_path)
+        video_path_layout.addWidget(self.video_path_edit)
+        setting_layout.addLayout(video_path_layout)
+
+        # Add a list of key frames
+        self.frame_list = QListWidget()
+        setting_layout.addWidget(self.frame_list)
+        self.frame_list.itemSelectionChanged.connect(self.on_select_frame)
+        self.installEventFilter(self)
+
+        # Add spin boxes for linear / angular velocity and stop time
+        self.lin_vel_spinbox = QDoubleSpinBox()
+        self.lin_vel_spinbox.setPrefix("Linear Velocity (m/s): ")
+        self.lin_vel_spinbox.setRange(0, 100)
+        self.lin_vel_spinbox.valueChanged.connect(self.set_frame_lin_vel)
+        setting_layout.addWidget(self.lin_vel_spinbox)
+
+        self.lin_ang_spinbox = QDoubleSpinBox()
+        self.lin_ang_spinbox.setPrefix("Angular Velocity (rad/s): ")
+        self.lin_ang_spinbox.setRange(0, 100)
+        self.lin_ang_spinbox.valueChanged.connect(self.set_frame_ang_vel)
+        setting_layout.addWidget(self.lin_ang_spinbox)
+
+        self.stop_time_spinbox = QDoubleSpinBox()
+        self.stop_time_spinbox.setPrefix("Stop Time: ")
+        self.stop_time_spinbox.setRange(0, 100)
+        self.stop_time_spinbox.valueChanged.connect(self.set_frame_stop_time)
+        setting_layout.addWidget(self.stop_time_spinbox)
+        
+        setting_layout.setAlignment(QtCore.Qt.AlignTop)
+        main_layout.addLayout(setting_layout)
+
+
+    def update_video_path(self, path):
+        self.video_path = path
+
+    def add_key_frame(self):
+        view_matrix = self.glwidget.view_matrix
+        # Get camera pose in world frame
+        Twc = np.linalg.inv(view_matrix)
+        # Add the key frame to the list
+        key_frame = KeyFrame(Twc)
+        current_index = self.frame_list.currentRow()
+        self.key_frames.insert(current_index + 1, key_frame)
+        # visualize this key frame using FrameItem
+        self.glwidget.add_item(key_frame.item)
+        # move the camera back to 0.5 meter, let the user see the frame
+        self.glwidget.update_dist(0.5)
+        # Add the key frame to the Qt ListWidget
+        item = QListWidgetItem(f"Frame {current_index + 2}")
+        self.frame_list.insertItem(current_index + 1, item)
+        self.frame_list.setCurrentRow(current_index + 1)
+        # Update frame labels
+        for i in range(len(self.key_frames)):
+            self.frame_list.item(i).setText(f"Frame {i + 1}")
+
+    def del_key_frame(self):
+        current_index = self.frame_list.currentRow()
+        if current_index >= 0:
+            self.glwidget.remove_item(self.key_frames[current_index].item)
+            self.key_frames.pop(current_index)
+            self.frame_list.itemSelectionChanged.disconnect(self.on_select_frame)
+            self.frame_list.takeItem(current_index)
+            self.frame_list.itemSelectionChanged.connect(self.on_select_frame)
+            self.on_select_frame()
+        # Update frame labels
+        for i in range(len(self.key_frames)):
+            
+            self.frame_list.item(i).setText(f"Frame {i + 1}")
+    
+    def on_select_frame(self):
+        current = self.frame_list.currentRow()
+        for i, frame in enumerate(self.key_frames):
+            if i == current:
+                # Highlight the selected frame
+                frame.item.set_color('#FF0000')
+                frame.item.set_line_width(5)
+                # show current frame's parameters in the spinboxes
+                self.lin_vel_spinbox.setValue(frame.linear_velocity)
+                self.lin_ang_spinbox.setValue(frame.angular_velocity)
+                self.stop_time_spinbox.setValue(frame.stop_time)
+            else:
+                frame.item.set_color('#0000FF')
+                frame.item.set_line_width(3)
+
+    def set_frame_lin_vel(self, value):
+        current_index = self.frame_list.currentRow()
+        if current_index >= 0:
+            self.key_frames[current_index].linear_velocity = value
+
+    def set_frame_ang_vel(self, value):
+        current_index = self.frame_list.currentRow()
+        if current_index >= 0:
+            self.key_frames[current_index].angular_velocity = value
+
+    def set_frame_stop_time(self, value):
+        current_index = self.frame_list.currentRow()
+        if current_index >= 0:
+            self.key_frames[current_index].stop_time = value
+
+    def create_frames(self):
+        """
+        Create the frames for playback by interpolating between key frames.
+        """
+        self.frames = []
+        dt = 1 / float(self.update_interval)
+        for i in range(len(self.key_frames) - 1):
+            current_frame = self.key_frames[i]
+            if current_frame.stop_time > 0:
+                num_steps = int(current_frame.stop_time / dt)
+                for j in range(num_steps):
+                    self.frames.append(current_frame.Twc)
+            next_frame = self.key_frames[i + 1]
+            Ts = q3d.interpolate_pose(current_frame.Twc, next_frame.Twc,
+                                      current_frame.linear_velocity,
+                                      current_frame.angular_velocity,
+                                      dt)
+            self.frames.extend(Ts)
+        
+        print(f"Total frames: {len(self.frames)}")
+        print(f"Total time: {len(self.frames) * dt:.2f} seconds")
+
+    def toggle_playback(self):
+        if self.is_playing:
+            self.stop_playback()
+        else:
+            self.start_playback()
+
+    def start_playback(self):
+        if self.key_frames:
+            self.create_frames()
+            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.record_checkbox.setEnabled(False)
+            if self.is_recording is True:
+                self.start_recording()
+
+    def stop_playback(self):
+        self.timer.stop()
+        self.is_playing = False
+        self.play_button.setStyleSheet("")
+        self.play_button.setText("Play")
+        self.record_checkbox.setEnabled(True)
+        if self.is_recording:
+            self.stop_recording()
+
+    def play_frames(self):
+        """
+        callback function for the timer to play the frames
+        """
+        # play the frames
+        if self.current_frame_index < len(self.frames):
+            self.glwidget.set_view_matrix(np.linalg.inv(self.frames[self.current_frame_index]))
+            self.current_frame_index += 1
+            if self.is_recording:
+                self.record_frame()
+        else:
+            self.stop_playback()
+
+    def toggle_recording(self, state):
+        if state == 2:
+            self.is_recording = True
+        else:
+            self.is_recording = False
+
+    def start_recording(self):
+        self.is_recording = True
+        self.frames_to_record = []
+        video_path = self.video_path_edit.text()
+        self.play_button.setStyleSheet("background-color: red")
+        self.play_button.setText("Recording")
+        self.writer = imageio.get_writer(video_path, fps=self.update_interval,
+                                         codec="libx264", bitrate="5M", quality=10)
+        # disable the all the frame_item while recording
+        for frame in self.key_frames:
+            frame.item.hide()
+
+    def stop_recording(self, save_movie=True):
+        self.is_recording = False
+        self.record_checkbox.setChecked(False)
+        # enable the all the frame_item after recording
+        for frame in self.key_frames:
+            frame.item.show()
+        if hasattr(self, 'writer') and save_movie:
+            self.writer.close()
+            self.show_save_message()
+
+    def show_save_message(self):
+        msg_box = QMessageBox()
+        msg_box.setIcon(QMessageBox.Information)
+        msg_box.setWindowTitle("Video Saved")
+        msg_box.setText(f"Video saved to {self.video_path_edit.text()}")
+        msg_box.setStandardButtons(QMessageBox.Ok)
+        msg_box.exec()
+
+    def record_frame(self):
+        frame = self.glwidget.capture_frame()
+        # make sure the frame size is multiple of 16
+        height, width, _ = frame.shape
+        if height % 16 != 0 or width % 16 != 0:
+            frame = frame[:-(height % 16), :-(width % 16), :]
+            frame = np.ascontiguousarray(frame)
+        self.frames_to_record.append(frame)
+        try:
+            self.writer.append_data(frame)
+        except Exception as e:
+            print("Don't change the window size during recording.")
+            self.stop_recording(False) # Stop recording without saving
+            self.stop_playback()
+
+    def eventFilter(self, obj, event):
+        if event.type() == QtCore.QEvent.KeyPress:
+            if event.key() == QtCore.Qt.Key_Delete:
+                self.del_key_frame()
+                return True
+        return super().eventFilter(obj, event)
+
+    def dragEnterEvent(self, event):
+        if event.mimeData().hasUrls():
+            event.accept()
+        else:
+            event.ignore()
+
+    def dropEvent(self, event):
+        """
+        Overwrite the drop event to open the cloud file.
+        """
+        self.progress_dialog = ProgressDialog(self)
+        self.progress_dialog.show()
+        files = event.mimeData().urls()
+        self.progress_thread = FileLoaderThread(self, files)
+        self['cloud'].load(files[0].toLocalFile(), append=False)
+        self.progress_thread.progress.connect(self.file_loading_progress)
+        self.progress_thread.finished.connect(self.file_loading_finished)
+        self.progress_thread.start()
+
+    def file_loading_progress(self, value):
+        self.progress_dialog.set_value(value)
+
+    def file_loading_finished(self):
+        self.progress_dialog.close()
+
+    def open_cloud_file(self, file, append=False):
+        cloud_item = self['cloud']
+        if cloud_item is None:
+            print("Can't find clouditem.")
+            return
+        cloud = cloud_item.load(file, append=append)
+        center = np.nanmean(cloud['xyz'].astype(np.float64), axis=0)
+        self.glwidget.set_cam_position(pos=center)
+
+def main():
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--path", help="the cloud file path")
+    args = parser.parse_args()
+    app = q3d.QApplication(['Cloud Movie Maker'])
+    viewer = CMMViewer(name='Cloud Movie Maker', update_interval=30)
+    cloud_item = q3d.CloudIOItem(size=1, alpha=0.1)
+    axis_item = q3d.AxisItem(size=0.5, width=5)
+    grid_item = q3d.GridItem(size=1000, spacing=20)
+
+    viewer.add_items(
+        {'cloud': cloud_item, 'grid': grid_item, 'axis': axis_item})
+
+    if args.path:
+        pcd_fn = args.path
+        viewer.open_cloud_file(pcd_fn)
+
+    viewer.show()
+    app.exec()
+
+
+if __name__ == '__main__':
+    main()