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

q3dviewer/tools/film_maker.py

@@ -0,0 +1,395 @@
+#!/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, QDockWidget, QWidget, QComboBox
+from PySide6.QtCore import QTimer
+from q3dviewer.tools.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, lin_vel=10, ang_vel=np.pi/3, stop_time=0):
+        self.Twc = Twc
+        self.lin_vel = lin_vel
+        self.ang_vel = ang_vel # rad/s
+        self.stop_time = stop_time
+        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()
+        elif ev.key() == QtCore.Qt.Key_C:
+            self.viewer.dock.show()
+        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 (Key Space)")
+        add_button.clicked.connect(self.add_key_frame)
+        setting_layout.addWidget(add_button)
+        del_button = QPushButton("Delete Key Frame (Key Delete)")
+        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 codec setting
+        codec_layout = QHBoxLayout()
+        label_codec = QLabel("Codec:")
+        codec_layout.addWidget(label_codec)
+        self.codec_combo = QComboBox()
+        self.codec_combo.addItems(["mjpeg", "mpeg4", "libx264", "libx265"])
+        codec_layout.addWidget(self.codec_combo)
+        setting_layout.addLayout(codec_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, 1000)
+        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 (deg/s): ")
+        self.lin_ang_spinbox.setRange(0, 360)
+        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)
+
+        # Create a dock widget for the settings
+        dock_widget = QDockWidget("Settings", self)
+        dock_widget.setWidget(QWidget())
+        dock_widget.widget().setLayout(setting_layout)
+        self.addDockWidget(QtCore.Qt.RightDockWidgetArea, dock_widget)
+
+        # Add the dock widget to the main layout
+        main_layout.addWidget(dock_widget)
+        self.dock = dock_widget
+
+
+    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 end of the list
+        if self.key_frames:
+            prev = self.key_frames[-1]
+            key_frame = KeyFrame(Twc, 
+                                 lin_vel=prev.lin_vel, 
+                                 ang_vel=prev.ang_vel)
+        else:
+            key_frame = KeyFrame(Twc)
+        self.key_frames.append(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 {len(self.key_frames)}")
+        self.frame_list.addItem(item)
+        self.frame_list.setCurrentRow(len(self.key_frames) - 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.lin_vel)
+                self.lin_ang_spinbox.setValue(np.rad2deg(frame.ang_vel))
+                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].lin_vel = value
+
+    def set_frame_ang_vel(self, value):
+        current_index = self.frame_list.currentRow()
+        if current_index >= 0:
+            self.key_frames[current_index].ang_vel = np.deg2rad(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([i, current_frame.Twc])
+            next_frame = self.key_frames[i + 1]
+            Ts = q3d.interpolate_pose(current_frame.Twc, next_frame.Twc,
+                                      current_frame.lin_vel,
+                                      current_frame.ang_vel,
+                                      dt)
+            for T in Ts:
+                self.frames.append([i, T])
+        
+        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)
+        self.frame_list.setCurrentRow(len(self.key_frames) - 1)
+        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):
+            key_id, Tcw = self.frames[self.current_frame_index]
+            self.glwidget.set_view_matrix(np.linalg.inv(Tcw))
+            self.frame_list.setCurrentRow(key_id)
+            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()
+        codec = self.codec_combo.currentText()
+        self.play_button.setStyleSheet("background-color: red")
+        self.play_button.setText("Recording")
+        self.writer = imageio.get_writer(video_path, 
+                                         fps=self.update_interval,
+                                         codec=codec)  # quality=10
+        # disable the all the frame_item while recording
+        for frame in self.key_frames:
+            frame.item.hide()
+        self.dock.hide()  # Hide the dock while recording
+
+    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()
+        self.dock.show()  # Show the dock when recording stops
+
+    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(['Film Maker'])
+    viewer = CMMViewer(name='Film Maker', update_interval=30)
+    cloud_item = q3d.CloudIOItem(size=0.1, point_type='SPHERE', alpha=0.5, depth_test=True)
+    grid_item = q3d.GridItem(size=1000, spacing=20)
+
+    viewer.add_items(
+        {'cloud': cloud_item, 'grid': grid_item})
+
+    if args.path:
+        pcd_fn = args.path
+        viewer.open_cloud_file(pcd_fn)
+
+    viewer.show()
+    app.exec()
+
+
+if __name__ == '__main__':
+    main()

q3dviewer/tools/lidar_calib.py

@@ -43,7 +43,7 @@ class CustomDoubleSpinBox(QDoubleSpinBox):
         return float(text)
 
 
-class ViewerWithPanel(q3d.Viewer):
+class LiDARCalibViewer(q3d.Viewer):
     def __init__(self, **kwargs):
         self.t01 = np.array([0, 0, 0])
         self.R01 = np.eye(3)
@@ -51,15 +51,15 @@ class ViewerWithPanel(q3d.Viewer):
         self.radius = 0.2
         super().__init__(**kwargs)
 
-    def init_ui(self):
-        center_widget = QWidget()
-        self.setCentralWidget(center_widget)
-        main_layout = QHBoxLayout()
-        center_widget.setLayout(main_layout)
-
+    def default_gl_setting(self, glwidget):
+        # Set camera position and background color
+        glwidget.set_bg_color('#ffffff')
+        glwidget.set_cam_position(distance=5)
+    
+    def add_control_panel(self, main_layout):
         # Create a vertical layout for the settings
         setting_layout = QVBoxLayout()
-
+        setting_layout.setAlignment(QtCore.Qt.AlignTop)
         # Add XYZ spin boxes
         label_xyz = QLabel("Set XYZ:")
         setting_layout.addWidget(label_xyz)
@@ -135,19 +135,8 @@ class ViewerWithPanel(q3d.Viewer):
         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.glwidget = q3d.GLWidget()
         main_layout.addLayout(setting_layout)
-        main_layout.addWidget(self.glwidget, 1)
-
-        timer = QtCore.QTimer(self)
-        timer.setInterval(20)  # period, in milliseconds
-        timer.timeout.connect(self.update)
-        self.glwidget.set_cam_position(distance=5)
-        self.glwidget.set_bg_color('#ffffff')
-        timer.start()
+        
 
     def update_radius(self):
         self.radius = self.box_radius.value()
@@ -271,8 +260,8 @@ def main():
     args = parser.parse_args()
 
     app = q3d.QApplication(["LiDAR Calib"])
-    viewer = ViewerWithPanel(name='LiDAR Calib')
-    grid_item = q3d.GridItem(size=10, spacing=1, color=(0, 0, 0, 70))
+    viewer = LiDARCalibViewer(name='LiDAR Calib')
+    grid_item = q3d.GridItem(size=10, spacing=1, color='#00000040')
     scan0_item = q3d.CloudItem(
         size=2, alpha=1, color_mode='FLAT', color='#ff0000')
     scan1_item = q3d.CloudItem(

q3dviewer/tools/lidar_cam_calib.py

@@ -36,7 +36,7 @@ class CustomDoubleSpinBox(QDoubleSpinBox):
         return float(text)
 
 
-class ViewerWithPanel(q3d.Viewer):
+class LidarCamViewer(q3d.Viewer):
     def __init__(self, **kwargs):
         # b: camera body frame
         # c: camera image frame
@@ -54,15 +54,15 @@ class ViewerWithPanel(q3d.Viewer):
         self.en_rgb = False
         super().__init__(**kwargs)
 
-    def init_ui(self):
-        center_widget = QWidget()
-        self.setCentralWidget(center_widget)
-        main_layout = QHBoxLayout()
-        center_widget.setLayout(main_layout)
+    def default_gl_setting(self, glwidget):
+        # Set camera position and background color
+        glwidget.set_bg_color('#ffffff')
+        glwidget.set_cam_position(distance=5)
 
+    def add_control_panel(self, main_layout):
         # Create a vertical layout for the settings
         setting_layout = QVBoxLayout()
-
+        setting_layout.setAlignment(QtCore.Qt.AlignTop)
         # Add a checkbox for RGB
         self.checkbox_rgb = QCheckBox("Enable RGB Cloud")
         self.checkbox_rgb.setChecked(False)
@@ -148,19 +148,8 @@ class ViewerWithPanel(q3d.Viewer):
         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.glwidget = q3d.GLWidget()
         main_layout.addLayout(setting_layout)
-        main_layout.addWidget(self.glwidget, 1)
 
-        timer = QtCore.QTimer(self)
-        timer.setInterval(20)  # period, in milliseconds
-        timer.timeout.connect(self.update)
-        self.glwidget.set_cam_position(distance=5)
-        self.glwidget.set_bg_color('#ffffff')
-        timer.start()
 
     def update_point_size(self):
         self.psize = self.box_psize.value()
@@ -292,8 +281,8 @@ def main():
     args = parser.parse_args()
 
     app = q3d.QApplication(['LiDAR Cam Calib'])
-    viewer = ViewerWithPanel(name='LiDAR Cam Calib')
-    grid_item = q3d.GridItem(size=10, spacing=1, color=(0, 0, 0, 70))
+    viewer = LidarCamViewer(name='LiDAR Cam Calib')
+    grid_item = q3d.GridItem(size=10, spacing=1, color='#00000040')
     scan_item = q3d.CloudItem(size=2, alpha=1, color_mode='I')
     img_item = q3d.ImageItem(pos=np.array([0, 0]), size=np.array([800, 600]))
     viewer.add_items({'scan': scan_item, 'grid': grid_item, 'img': img_item})

q3dviewer/utils/maths.py

@@ -3,9 +3,145 @@ Copyright 2024 Panasonic Advanced Technology Development Co.,Ltd. (Liu Yang)
 Distributed under MIT license. See LICENSE for more information.
 """
 
+"""
+Math proofs and implementations: 
+https://github.com/scomup/MathematicalRobotics.git
+"""
+
+
 import numpy as np
 
 
+_epsilon_ = 1e-5
+
+
+def skew(vector):
+    return np.array([[0, -vector[2], vector[1]],
+                     [vector[2], 0, -vector[0]],
+                     [-vector[1], vector[0], 0]])
+
+
+def expSO3(omega):
+    """
+    Exponential map of SO3
+    The proof is shown in 3d_rotation_group.md (10)
+    """
+    theta2 = omega.dot(omega)
+    theta = np.sqrt(theta2)
+    nearZero = theta2 <= _epsilon_
+    W = skew(omega)
+    if (nearZero):
+        return np.eye(3) + W
+    else:
+        K = W/theta
+        KK = K.dot(K)
+        sin_theta = np.sin(theta)
+        one_minus_cos = 1 - np.cos(theta)
+        R = np.eye(3) + sin_theta * K + one_minus_cos * KK  # rotation.md (10)
+        return R
+
+
+def logSO3(R):
+    """
+    Logarithm map of SO3
+    The proof is shown in rotation.md (14)
+    """
+    R11, R12, R13 = R[0, :]
+    R21, R22, R23 = R[1, :]
+    R31, R32, R33 = R[2, :]
+    tr = np.trace(R)
+    omega = np.zeros(3)
+    v = np.array([R32 - R23, R13 - R31, R21 - R12])
+    # when trace == -1, i.e., the theta is approx to +-pi, +-3pi, +-5pi, etc.
+    # we do something special
+    if (tr + 1.0 < 1e-3):
+        if (R33 > R22 and R33 > R11):
+            # R33 is largest
+            # sin(theta) approx to sgn_w*pi-theta, cos(theta) approx to -1
+            W = R21 - R12  # 2*r3*sin(theta) = 2*r3*(sgn_w*pi-theta)
+            Q1 = R31 + R13          # 4 * r1*r3
+            Q2 = R23 + R32          # 4 * r2*r3
+            Q3 = 2.0 + 2.0 * R33    # 4 * r3*r3
+            r = np.sqrt(Q3)         # 2 * r3
+            one_over_r = 1 / r      # 1 / (2*r3)
+            norm = np.sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W)  # 4*r3
+            sgn_w = np.sign(W)      # get the sgn of theta
+            mag = np.pi - (2 * sgn_w * W) / norm   # theta*sgn_w
+            scale = 0.5 * mag * one_over_r  # theta * sgn_w / (4*r3)
+            # omega = theta * [4*r1*r3, 4*r2*r3, 4*r3*r3]/ (4*r3)
+            omega = sgn_w * scale * np.array([Q1, Q2, Q3])
+        elif (R22 > R11):
+            # R22 is the largest
+            W = R13 - R31  # 2*r2*sin(theta) = 2*r2*(sgn_w*pi-theta)
+            Q1 = R12 + R21        # 4 * r2*r1
+            Q2 = 2.0 + 2.0 * R22  # 4 * r2*r2
+            Q3 = R23 + R32        # 4 * r2*r3
+            r = np.sqrt(Q2)
+            one_over_r = 1 / r
+            norm = np.sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W)
+            sgn_w = np.sign(W)
+            mag = np.pi - (2 * sgn_w * W) / norm
+            scale = 0.5 * one_over_r * mag
+            omega = sgn_w * scale * np.array([Q1, Q2, Q3])
+        else:
+            # R11 is the largest
+            W = R32 - R23  # 2*r1*sin(theta) = 2*r1*(sgn_w*pi-theta)
+            Q1 = 2.0 + 2.0 * R11 # 4 * r1*r1
+            Q2 = R12 + R21       # 4 * r1*r2
+            Q3 = R31 + R13       # 4 * r1*r3
+            r = np.sqrt(Q1)
+            one_over_r = 1 / r
+            norm = np.sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W)
+            sgn_w = np.sign(W)
+            mag = np.pi - (2 * sgn_w * W) / norm
+            scale = 0.5 * one_over_r * mag
+            omega = sgn_w * scale * np.array([Q1, Q2, Q3])
+    else:
+        magnitude = 0
+        tr_3 = tr - 3.0
+        if (tr_3 < -1e-6):
+            # this is the normal case -1 < trace < 3
+            theta = np.arccos((tr - 1.0) / 2.0)
+            magnitude = theta / (2.0 * np.sin(theta))
+        else:
+            # when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
+            # use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
+            # see https://github.com/borglab/gtsam/issues/746 for details
+            magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0
+        omega = magnitude * np.array([R32 - R23, R13 - R31, R21 - R12])
+    return omega
+
+
+def interpolate_pose(T1, T2, v_max, omega_max, dt=0.02):
+    R1, t1 = makeRt(T1)
+    R2, t2 = makeRt(T2)
+    
+    # Get transfrom 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):
+        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
+
+
 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:
@@ -157,11 +293,25 @@ def makeRt(T):
     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)
+    if not hex_color.startswith("#"):
+        print("Invalid hex color string.")
+        return (1.0, 1.0, 1.0, 1.0)
+    if len(hex_color) == 7:
+        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)
+    elif len(hex_color) == 9:
+        color_flat = int(hex_color[1:], 16)
+        red = (color_flat >> 24) & 0xFF
+        green = (color_flat >> 16) & 0xFF
+        blue = (color_flat >> 8) & 0xFF
+        alpha = color_flat & 0xFF
+        return (red / 255.0, green / 255.0, blue / 255.0, alpha / 255.0)
+    else:
+        print("Invalid hex color string.")
+        return (1.0, 1.0, 1.0, 1.0)
 
 # euler = np.array([1, 0.1, 0.1])
 # euler_angles = matrix_to_euler(euler_to_matrix(euler))