HViktorTsoi

def write_pcd_binary(points, filename):
    """
    将Nx4的点云数据保存为PCD二进制格式
    points: Nx4的numpy array,每行为(x,y,z,intensity)
    filename: 保存的文件名
    """
    n_points = len(points)

    # PCD文件头
    header = f"""# .PCD v0.7 - Point Cloud Data file format

HViktorTsoi

# location: /home/xxx/.local/share/applications
[Desktop Entry]
Name=VideoCut
Comment=Fast video cutting tool
Exec=/path/to/video_cut.py %U
Icon=/path/to/icon.png
Terminal=false
Type=Application

HViktorTsoi

# location: /home/xxx/.local/share/applications/
[Desktop Entry]
Name=PDF2PNG
Comment=converting pdf to png
Exec=/path/to/pdf_to_png/pdf_to_png.py %U
Icon=/path/to/pdf_to_png.png
Terminal=false
Type=Application

HViktorTsoi

# Error imformation: 

# No protocol specified
# qt.qpa.xcb: could not connect to display unix:0
# qt.qpa.plugin: Could not load the Qt platform plugin "xcb" in "" even though it was found.
# This application failed to start because no Qt platform plugin could be initialized. Reinstalling the application may fix this problem.
#  Available platform plugins are: eglfs, linuxfb, minimal, minimalegl, offscreen, vnc, xcb.

# Soultion:
xhost +si:localuser:root

HViktorTsoi

import matplotlib.pyplot as plt
import matplotlib.ticker as mtick

plt.gca().yaxis.set_major_formatter(mtick.FormatStrFormatter('%.2f'))

HViktorTsoi

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.legend_handler import HandlerLineCollection, HandlerNpoints, HandlerBase, HandlerLine2D, HandlerTuple

plot1 = plt.plot([3, 2, 1], [1, 2, 3])
plot2 = plt.plot([1, 2, 3], [3, 2, 1])

multi_color_line = tuple(
    [plt.plot([0, 0, 0], color=plt.get_cmap('rainbow')(v)[:3])[0] for v in np.linspace(0.0, 1.0, 5)])
l = plt.legend([multi_color_line, plot1[0], plot2[0]], ['Multiple color 0', 'Color1', 'Color2'],

HViktorTsoi

import subprocess
def get_process_cwd(process_name):
    try:
        pid = subprocess.check_output(['pgrep', '-f', process_name]).decode('utf-8').strip().split('\n')[0]
        cwd = subprocess.check_output(['pwdx', pid]).decode('utf-8').strip().split(" ")[-1]
        return cwd
    except subprocess.CalledProcessError:
        return None
      

HViktorTsoi

def odom_add_noise(poses_poor):
    ts_list = []
    pose_list = []
    for key, pose in sorted(poses_poor.items()):
        ts_list.append(key)
        pose_list.append(pose)

    velocity_integration = []
    for idx in range(1, len(pose_list)):
        velocity_integration.append(U.INV(pose_list[idx - 1]) @ pose_list[idx])

HViktorTsoi

def trajectory_slerp(evaluate_times, traj_raw_times, traj_raw, order=3):
    """
    linear interpolation trajectory
    :param evaluate_times: A list of float, the timestamps list at which to evaluate the interpolated poses.
    :param traj_raw_times: A list of float, the timestamps list of the input sparse poses
    :param traj_raw: A list of np[4x4] matrix, the SE3 matrices of the input sparse poses
    :param order: The degree of the spline fit
    :return:
    """
    assert len(traj_raw_times) == len(traj_raw)

HViktorTsoi

import copy
import time

import open3d as o3d
import os
from collections import defaultdict
import numpy as np
import numpy.linalg as LA
import gtsam
import tqdm