Common Function - Python3

* 之前有维护一套自己的CommonFunc.py（基础的、常用的小函数）

* 离职的时候发现拷不出来，很痛心>...<

* 重新写吧

* 碰到啥写啥

# for python3

def read_file2list_2D(file_path, data_type=int, split_str=','):
    # 从文件读入二维数据，返回list
    import os, io
    import numpy as np
    if not os.path.exists(file_path):
        print('Error: No such file', file_path)
        return
    with open(file_path, 'r') as f:
        out_lst = np.loadtxt(io.StringIO(f.read().replace(split_str, ' ')))
    if len(out_lst):
        out_lst = [list(map(data_type, i)) for i in out_lst]
    else:
        print('Wrong: %s is empty' % file_path)
        return
    return out_lst

def mkdir_deep(file_path):
    # 深度创建文件夹，效果同 mkdir -p
    import os, subprocess
    if not file_path:
        print('Error: %s is not a path' % file_path)
        return
    if os.path.exists(file_path):
        return
    try:
        os.makedirs(file_path)
    except:
        cmd = f'sudo mkdir -p {file_path}'
        print('cmd: ', cmd)
        subprocess.call(cmd,shell=True)

def comp_round(num, preci=0):
    # 四舍五入
    # preci==0，返回int；preci>0，返回float
    from decimal import Decimal, ROUND_HALF_UP
    if preci < 0:
        print("Error, wrong element! preci must bigger than 0, preci=", preci)
        return None
    if not isinstance(num, str):
        num = str(num)
    num = Decimal(num).quantize((Decimal('0.' + '0'*preci)), rounding=ROUND_HALF_UP)
    if preci == 0:
        num = round(int(num), preci)
    else:
        num = round(float(num), preci)
    return num

def rotate_3D(x_orig, y_orig, z_orig, angle, Vector):
    # 绕轴旋转曲线
    # Vector:旋转轴的方向向量（可以未归一化）
    import numpy as np
    if len(Vector) != 3:
        print('Error: Vector must have 3 elements. Vector =', Vector)
        return 
    c = np.cos(angle)
    s = np.sin(angle)
    Vector /= np.linalg.norm(Vector)
    [x, y, z] = Vector
    x_new = (x*x*(1 - c) + c)*x_orig + (x*y*(1 - c) - z*s)*y_orig + (x*z*(1 - c) + y*s)*z_orig
    y_new = (x*y*(1 - c) + z*s)*x_orig + (y*y*(1 - c) + c)*y_orig + (y*z*(1 - c) - x*s)*z_orig
    z_new = (x*z*(1 - c) - y*s)*x_orig + (y*z*(1 - c) + x*s)*y_orig + (z*z*(1 - c) + c)*z_orig
    return x_new, y_new, z_new

def trans_matrix_pose(matr):
    # 根据位姿矩阵计算位姿
    # from 4*4 matrix to x,y,z,pitch,yaw,roll
    import math
    import numpy as np
    # 检查matr格式
    if matr.shape != (4, 4):
        print('input shape error ', matr.shape)
        return []
    if matr[-1,0] != 0 or matr[-1,1] != 0 or matr[-1,2] != 0 or matr[-1,-1] != 1:
        print('input num error, last line ', matr[-1,:])
        return []
    # trans
    x, y, z = matr[0,-1], matr[1,-1], matr[2,-1]
    # rotates
    yaw = np.arctan2(-matr[2,0], math.sqrt(matr[0][0]**2 + matr[1][0]**2))
    pitch = np.arctan2(matr[2,1]/np.cos(yaw), matr[2,2]/np.cos(yaw))
    roll = np.arctan2(matr[1,0]/np.cos(yaw), matr[0,0]/np.cos(yaw))
    return [x,y,z,pitch,yaw,roll]

def trans_pose_matrix(pose):
    # 根据位姿计算位姿矩阵
    # from x,y,z,pitch,yaw,roll to 4*4 matrix
    import numpy as np
    
    [x,y,z,pitch,yaw,roll] = pose
    matr = np.eye(4)
    # trans
    matr[0,-1], matr[1,-1], matr[2,-1] = x, y, z
    # rotates
    cx, sx = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    cz, sz = np.cos(roll), np.sin(roll)
    matr[0,0] = cy*cz
    matr[0,1] = sx*sy*cz - cx*sz
    matr[0,2] = cx*sy*cz + sx*sz
    matr[1,0] = cy*sz
    matr[1,1] = sx*sy*sz + cx*cz
    matr[1,2] = cx*sy*sz - sx*cz
    matr[2,0] = -sy
    matr[2,1] = sx*cy
    matr[2,2] = cx*cy
    return matr

def plot_traj_3D(x, y, z, points=[], save_path=None):
    # 绘制3D轨迹
    # points 需要标出的点
    import matplotlib.pyplot as plt

    fig = plt.figure()
    ax = fig.add_subplot(projection='3d', title='Trajectory')
    ax.plot(x, y, z, color='r', linestyle='-')
    if len(points): # 绘制真实轨迹点 
        for p in points:
            ax.scatter(p[0], p[1], p[2], color='r', marker='o')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    if save_path:
        plt.savefig(save_path)
    # plt.show()
    plt.close()

def pic_2_video(pic_path, video_path):
    # 通过连续的图片生成视频
    # 注意图片名字格式
    import os, subprocess
    # check dest
    dir_path = os.path.dirname(video_path)
    if not os.path.exists(dir_path):
        cmd = f'sudo mkdir -p {dir_path}'
        print('cmd: ', cmd)
        subprocess.call(cmd,shell=True)
    # check src
    all_pics = os.listdir(pic_path)
    if len(all_pics) == 0:
        print('ERROR! no images')
        return
    # check pic name
    need_rename = 0
    tmp_pic_name = all_pics[0].split('.')[0]
    if not tmp_pic_name.isalnum():
        need_rename = 1
    elif len(tmp_pic_name) < 4:
        need_rename = 2
    if need_rename != 0:
        print(tmp_pic_name, 'is not standard format, need rename')
        suffix = '.' + all_pics[0].split('.')[-1]
        if need_rename == 1:
            all_pics = sorted(all_pics, key=lambda x: int(x.split('_')[0]))
        elif need_rename == 2:
            all_pics = sorted(all_pics, key=lambda x: int(x.split('.')[0]))
        for i, one_pic in enumerate(all_pics):
            orig_name = os.path.join(pic_path, one_pic)
            new_name = os.path.join(pic_path, '%04d'%i + suffix)
            cmd = f'sudo mv {orig_name} {new_name}'
            # print(cmd)
            subprocess.call(cmd,shell=True)
    else:
        print('pic_2_video pass check')

    ss = 'sudo ffmpeg -i ' + pic_path + '/%4d.jpg -strict -2 -vf scale=-1:480 ' + video_path
    try:
        subprocess.call(ss,shell=True)
    except:
        print('pic_2_video fail! ')