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

HViktorTsoi

#!/bin/sh
#
# This script should be run via curl:
#   sh -c "$(curl -fsSL https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh)"
# or via wget:
#   sh -c "$(wget -qO- https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh)"
# or via fetch:
#   sh -c "$(fetch -o - https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh)"
#
# As an alternative, you can first download the install script and run it afterwards:

HViktorTsoi

@nb.njit(nb.float64[:, :](nb.float64[:, :], nb.float32))

HViktorTsoi

def BLH2XYZ(B, L, H):
    '''B: lat  L:lon  H: height'''
    Lat, Lon = B, L

    N, E, h = 0, 0, 0
    L0 = (int((L - 1.5) / 3.0) + 1) * 3.0  #

    a = 6378245.0  #
    F = 298.257223563  #

HViktorTsoi

import open3d as o3d
import time

class NonBlockVisualizer:
    def __init__(self, point_size=2, background_color=[0, 0, 0]):
        self.__visualizer = o3d.visualization.Visualizer()
        self.__visualizer.create_window()
        opt = self.__visualizer.get_render_option()
        opt.background_color = np.asarray(background_color)
        opt = self.__visualizer.get_render_option()