* 之前有维护一套自己的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! ')