1. 动画生成
主要使用的是 matplotlib.animation ，具体示例如下：
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np
fig, ax = plt.subplots()
t = np.linspace(0, 3, 40)
g = -9.81
v0 = 12
z = g * t**2 / 2 + v0 * t
v02 = 5
z2 = g * t**2 / 2 + v02 * t
scat = ax.scatter(t[0], z[0], c="b", s=5, label=f'v0 = {v0} m/s')
line2 = ax.plot(t[0], z2[0], label=f'v0 = {v02} m/s')[0]
ax.set(xlim=[0, 3], ylim=[-4, 10], xlabel='Time [s]', ylabel='Z [m]')
ax.legend()
def update(frame):
# for each frame, update the data stored on each artist.
x = t[:frame]
y = z[:frame]
# update the scatter plot:
data = np.stack([x, y]).T
scat.set_offsets(data)
# update the line plot:
line2.set_xdata(t[:frame])
line2.set_ydata(z2[:frame])
return (scat, line2)
anim = animation.FuncAnimation(fig=fig, func=update, frames=40, interval=30)
anim.save("test.gif", writer='pillow') # save a gif
plt.show()
2. merge_asof 用法
merge_asof()是pandas库中的一个函数,可以根据两个dataframes中的时间戳列,基于最接近的时间戳将两个dataframes进行合并。
这个函数的作用类似于SQL中加入的操作,但是可以处理时间戳列,因此非常适用于金融数据等时间序列数据的合并。
此方法用于执行asof合并，这类似于left-join，除了我们匹配最近的键而不是相等的键。两个DataFrame都必须按键排序。
具体用法：
pandas.merge_asof
pandas.merge_asof(left, right, on=None, left_on=None, right_on=None, left_index=False, right_index=False, by=None, left_by=None, right_by=None, suffixes=('_x', '_y'), tolerance=None, allow_exact_matches=True, direction='backward'）
Perform a merge by key distance.
This is similar to a left-join except that we match on nearest key rather than equal keys. Both DataFrames must be sorted by the key.
For each row in the left DataFrame:
A “backward” search selects the last row in the right DataFrame whose ‘on’ key is less than or equal to the left’s key.【key:右侧dataframe对齐左侧数据最后看齐】
A “forward” search selects the first row in the right DataFrame whose ‘on’ key is greater than or equal to the left’s key.key:右侧dataframe对齐左侧数据最前看齐】
A “nearest” search selects the row in the right DataFrame whose ‘on’ key is closest in absolute distance to the left’s key.key:右侧dataframe对齐左侧数据朝最近看齐】
参数：
left, right: DataFrame
on:标签，要加入的字段名称。必须在两个DataFrame中都找到。
left_on:标签，要在左侧DataFrame中加入的字段名称。
right_on:标签，要在右侧DataFrame中加入的字段名称。
left_index:布尔值，使用左侧DataFrame的索引作为连接键。
right_index:布尔值，使用正确的DataFrame的索引作为连接键。
下面是上述方法的实现和一些示例
# importing package
import pandas
# creating data
left = pandas.DataFrame({'a':[1, 5, 10],
'left_val':['a', 'b', 'c']})
right = pandas.DataFrame({'a':[1, 2, 3, 6, 7],
'right_val':[1, 2, 3, 6, 7]})
# view data
print(left)
print(right)
# applying merge_asof on data
print(pandas.merge_asof(left, right, on='a',
direction='forward'))
print(pandas.merge_asof(left, right, on='a',
direction='nearest'))3. 具体应用
import imageio
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import pandas as pd
import os
from os import listdir
import numpy as np
import scipy.sparse as ss
import random
import datetime
import json
LA = np.linalg
rng = np.random
uss_unit = 100
angle_unit = 4096.0
def gen_fig():
fig = plt.figure()
ax = plt.axes(xlim=(10, 80), ylim=(-80, 20))
line, = ax.plot([], [], 'r-')
return fig, line
def update(line, data):
# 更新点的位置
line.set_data(data[0], data[1])
return line
def gen_data(input_data,xx, yy):
# 生成动态数据
for i in range(len(input_data)):
yield xx[:i], yy[:i]
def Q4ToMatrix2(x,y,z,w,xc,yc,xv,yv):
R = np.array([ [1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y],
[2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x],
[2*x*z-2*w*y, 2*y*z+2*w*x,1-2*x*x-2*y*y]])
Rv = np.linalg.inv(R)
pt = np.array([xc,yc,0]).reshape(-1,1) # uss corner point
pt_v = np.array([xv,yv,0]).reshape(-1,1) # vichle center point
pt_ = np.dot(Rv,pt)
pt_new = pt_ + pt_v
# t = Rv[2,0] * xc + Rv[2,1] * yc + Rv[2][2]
# pt_new_x = (Rv[0,0] * xc + Rv[0,1] * yc + Rv[0,2]) / t
# pt_new_y = (Rv[1,0] * xc + Rv[1,1] * yc + Rv[1,2]) / t
pt_new_x = pt_new[0,0]
pt_new_y = pt_new[1,0]
return [pt_new_x, pt_new_y]
def Q4ToMatrix(x, y, z, w, xc, yc, xv, yv):
# 根据四元数构建旋转矩阵
R = np.array([
[1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * w * z, 2 * x * z + 2 * w * y],
[2 * x * y + 2 * w * z, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * w * x],
[2 * x * z - 2 * w * y, 2 * y * z + 2 * w * x, 1 - 2 * x * x - 2 * y * y]
])
# xc, yc 是某个点在原坐标系中的坐标，xv, yv 是该坐标系相对于目标坐标系的平移量
pt = np.array([xc, yc, 0]) # 原始点坐标
pt_v = np.array([xv, yv, 0]) # 平移向量
# 应用旋转矩阵和平移向量
pt_transformed = R.dot(pt) + pt_v
return [pt_transformed[0], pt_transformed[1]]
def process_json(file_name):
# 读取并解析JSON文件
with open(file_name, 'r') as file:
data = json.load(file)
data_vcs = []
data_uss = []
data_ipm = []
for k,v in data.items():
if "loc_vcs" in v.keys():
data_vcs.append((k,v))
elif "ipm_pt" in v.keys():
dict_list = v["ipm_pt"]
if len(dict_list) > 0:
data_ipm.append((k,v))
else:
data_uss.append((k,v))
# 进一步解析IPM 车位信息, 生成dataframe
ts_ipm_list = []
idx_slot_list = []
idx_corner_list = []
pt_x_list = []
pt_y_list = []
pt_z_list = []
pt_score_list = []
pt_vis_list = []
pt_ort_x_list = []
pt_ort_y_list = []
for ipm in data_ipm:
ts = int(ipm[0])
for idx_slot,ipm_dcts in enumerate(ipm[1]["ipm_pt"]): # 10 or more or less
ipm_list = ipm_dcts["ipm_pt_slot_points"]
for idx_corner,ipm_pts in enumerate(ipm_list): # total 4 corner points
pt_x = ipm_pts["point"]["x"]
pt_y = ipm_pts["point"]["y"]
pt_z = ipm_pts["point"]["z"]
pt_score = ipm_pts["score"]
pt_vis = ipm_pts["visibility"]
pt_ort_x = ipm_pts["point_orientation"]["direct_x"]
pt_ort_y = ipm_pts["point_orientation"]["direct_y"]
ts_ipm_list.append(ts)
idx_slot_list.append(idx_slot)
idx_corner_list.append(idx_corner)
pt_x_list.append(pt_x)
pt_y_list.append(pt_y)
pt_z_list.append(pt_z)
pt_score_list.append(pt_score)
pt_vis_list.append(pt_vis)
pt_ort_x_list.append(pt_ort_x)
pt_ort_y_list.append(pt_ort_y)
df_dict_ipm = dict()
df_dict_ipm["timestamp"] = ts_ipm_list
df_dict_ipm["idx_slot"] = idx_slot_list
df_dict_ipm["idx_corner"] = idx_corner_list
df_dict_ipm["ipm_vcs_x"] = pt_x_list
df_dict_ipm["ipm_vcs_y"] = pt_y_list
df_dict_ipm["ipm_vcs_z"] = pt_z_list
df_dict_ipm["ipm_score"] = pt_score_list
df_dict_ipm["ipm_vis"] = pt_vis_list
df_dict_ipm["ipm_ort_x"] = pt_ort_x_list
df_dict_ipm["ipm_ort_y"] = pt_ort_y_list
df_ipm_ps = pd.DataFrame(df_dict_ipm)
df_ipm_ps = df_ipm_ps.sort_values(['timestamp',"idx_slot","idx_corner"])
df_ipm_ps.reset_index(drop=True, inplace=True)
# 进一步解析vcs信息, 生成dataframe
ts_list = []
vcs_wld_x_list = []
vcs_wld_y_list = []
vcs_wld_z_list = []
q4_x_list = []
q4_y_list = []
q4_z_list = []
q4_w_list = []
for vcs in data_vcs:
ts = int(int(vcs[0]))
vcs_wld_x = vcs[1]["loc_vcs"]["x"]
vcs_wld_y = vcs[1]["loc_vcs"]["y"]
vcs_wld_z = vcs[1]["loc_vcs"]["z"]
loc_q4_x = vcs[1]["loc_q4"]["x"]
loc_q4_y = vcs[1]["loc_q4"]["y"]
loc_q4_z = vcs[1]["loc_q4"]["z"]
loc_q4_w = vcs[1]["loc_q4"]["w"]
ts_list.append(ts)
vcs_wld_x_list.append(vcs_wld_x)
vcs_wld_y_list.append(vcs_wld_y)
vcs_wld_z_list.append(vcs_wld_z)
q4_x_list.append(loc_q4_x)
q4_y_list.append(loc_q4_y)
q4_z_list.append(loc_q4_z)
q4_w_list.append(loc_q4_w)
df_dict_vcs = dict()
df_dict_vcs["timestamp"] = ts_list
df_dict_vcs["vwld_x"] = vcs_wld_x_list
df_dict_vcs["vwld_y"] = vcs_wld_y_list
df_dict_vcs["vwld_z"] = vcs_wld_z_list
df_dict_vcs["q4_x"] = q4_x_list
df_dict_vcs["q4_y"] = q4_y_list
df_dict_vcs["q4_z"] = q4_z_list
df_dict_vcs["q4_w"] = q4_w_list
df_ndm_vcs = pd.DataFrame(df_dict_vcs)
df_ndm_vcs = df_ndm_vcs.sort_values('timestamp')
df_ndm_vcs.reset_index(drop=True, inplace=True)
# 进一步解析 uss obj & uss ps信息, 生成dataframe
ts_list_uss = []
uss_obj_x_id_list = []
uss_obj_x_cfd_list = []
uss_obj_x_type_list = []
uss_obj_x_height_list = []
uss_obj_x_pos_1_x_list = []
uss_obj_x_pos_1_y_list = []
uss_obj_x_pos_2_x_list = []
uss_obj_x_pos_2_y_list = []
ts_list_ps = []
uss_ps_id_list = []
uss_ps_length_list = []
uss_ps_depth_list = []
uss_ps_angle_list = []
uss_ps_status_list = []
uss_ps_type_list = []
uss_ps_1st_corner_x_list = []
uss_ps_1st_corner_y_list = []
uss_ps_2nd_corner_x_list = []
uss_ps_2nd_corner_y_list = []
for uss in data_uss:
ts = int(uss[0])
for obj_list in uss[1]["uss_obj"]:
ts_list_uss.append(ts)
uss_obj_x_id = obj_list["uss_obj_x_id_ui8"]
uss_obj_x_cfd = obj_list["uss_obj_x_cfd_ui8"] #obj confidence
uss_obj_x_type = obj_list["uss_obj_x_type_en"] #obj type
uss_obj_x_height = obj_list["uss_obj_x_height_en"]/uss_unit
uss_obj_x_pos_1_x = obj_list["uss_obj_x_pos_1"]["x_si16"]/uss_unit
uss_obj_x_pos_1_y = obj_list["uss_obj_x_pos_1"]["y_si16"]/uss_unit
uss_obj_x_pos_2_x = obj_list["uss_obj_x_pos_2"]["x_si16"]/uss_unit
uss_obj_x_pos_2_y = obj_list["uss_obj_x_pos_2"]["y_si16"]/uss_unit
uss_obj_x_id_list.append(uss_obj_x_id)
uss_obj_x_cfd_list.append(uss_obj_x_cfd)
uss_obj_x_type_list.append(uss_obj_x_type)
uss_obj_x_height_list.append(uss_obj_x_height)
uss_obj_x_pos_1_x_list.append(uss_obj_x_pos_1_x)
uss_obj_x_pos_1_y_list.append(uss_obj_x_pos_1_y)
uss_obj_x_pos_2_x_list.append(uss_obj_x_pos_2_x)
uss_obj_x_pos_2_y_list.append(uss_obj_x_pos_2_y)
for ps_list in uss[1]["uss_ps"]:
ts_list_ps.append(ts)
uss_ps_id = ps_list["uss_ps_id"]
uss_ps_length = ps_list["uss_ps_length_ui16"]/uss_unit
uss_ps_depth = ps_list["uss_ps_depth_ui16"]/uss_unit
uss_ps_angle = ps_list["uss_ps_angle_ui16"] # /angle_unit + np.pi/2
uss_ps_status = ps_list["uss_ps_status_en"] # NONE=0,SEARCH =1, CORRECT=2
uss_ps_type = ps_list["uss_ps_type_en"] # NONE=0,PARALLEL=1,CROSS=2,DIAGONAL=3
uss_ps_1st_corner_x = ps_list["uss_ps_1st_corner"]["x_si16"]/uss_unit
uss_ps_1st_corner_y = ps_list["uss_ps_1st_corner"]["y_si16"]/uss_unit
uss_ps_2nd_corner_x = ps_list["uss_ps_2nd_corner"]["x_si16"]/uss_unit
uss_ps_2nd_corner_y = ps_list["uss_ps_2nd_corner"]["y_si16"]/uss_unit
uss_ps_id_list.append(uss_ps_id)
uss_ps_length_list.append(uss_ps_length)
uss_ps_depth_list.append(uss_ps_depth)
uss_ps_angle_list.append(uss_ps_angle)
uss_ps_status_list.append(uss_ps_status)
uss_ps_type_list.append(uss_ps_type)
uss_ps_1st_corner_x_list.append(uss_ps_1st_corner_x)
uss_ps_1st_corner_y_list.append(uss_ps_1st_corner_y)
uss_ps_2nd_corner_x_list.append(uss_ps_2nd_corner_x)
uss_ps_2nd_corner_y_list.append(uss_ps_2nd_corner_y)
df_dict_uss_obj = dict()
df_dict_uss_ps = dict()
df_dict_uss_obj["timestamp"] = ts_list_uss
df_dict_uss_obj["uss_obj_x_id"] = uss_obj_x_id_list
df_dict_uss_obj["uss_obj_x_confidence"] = uss_obj_x_cfd_list
df_dict_uss_obj["uss_obj_x_type"] = uss_obj_x_type_list
df_dict_uss_obj["uss_obj_x_height"] = uss_obj_x_height_list
df_dict_uss_obj["uss_obj_x_pos_1_x"] = uss_obj_x_pos_1_x_list
df_dict_uss_obj["uss_obj_x_pos_1_y"] = uss_obj_x_pos_1_y_list
df_dict_uss_obj["uss_obj_x_pos_2_x"] = uss_obj_x_pos_2_x_list
df_dict_uss_obj["uss_obj_x_pos_2_y"] = uss_obj_x_pos_2_y_list
df_uss_obj = pd.DataFrame(df_dict_uss_obj)
df_uss_obj = df_uss_obj.sort_values(["timestamp","uss_obj_x_id"])
df_uss_obj.reset_index(drop=True, inplace=True)
df_dict_uss_ps["timestamp"] = ts_list_ps
df_dict_uss_ps["uss_ps_id"] = uss_ps_id_list
df_dict_uss_ps["uss_ps_length"] = uss_ps_length_list
df_dict_uss_ps["uss_ps_depth"] = uss_ps_depth_list
df_dict_uss_ps["uss_ps_angle"] = uss_ps_angle_list
df_dict_uss_ps["uss_ps_status"] = uss_ps_status_list
df_dict_uss_ps["uss_ps_type"] = uss_ps_type_list
df_dict_uss_ps["uss_ps_1st_corner_x"] = uss_ps_1st_corner_x_list
df_dict_uss_ps["uss_ps_1st_corner_y"] = uss_ps_1st_corner_y_list
df_dict_uss_ps["uss_ps_2nd_corner_x"] = uss_ps_2nd_corner_x_list
df_dict_uss_ps["uss_ps_2nd_corner_y"] = uss_ps_2nd_corner_y_list
df_uss_ps = pd.DataFrame(df_dict_uss_ps)
df_uss_ps = df_uss_ps.sort_values(['timestamp',"uss_ps_id"])
df_uss_ps.reset_index(drop=True, inplace=True)
return df_ndm_vcs, df_uss_obj, df_uss_ps, df_ipm_ps
def data_merge(df_vcs, df_obj, df_uss, df_ipm, idx_list, dt = 6000):
# 分析第一个case,车位id 164,166
df_uss_list = []
for idx in idx_list:
df_uss_slice = df_uss[(df_uss["uss_ps_id"]==idx)] # uss id filter
df_uss_list.append(df_uss_slice)
df_uss_filter = pd.concat(df_uss_list, ignore_index=True)
# 使用merge实现就近join
# 假设df1和df2的keys是升序的,这里使用了'left'，'right'来指定merge的方向
# 使用'backward'来找到就近的key进行合并
df_ps_merged = pd.merge_asof(df_uss_filter.sort_values('timestamp'), df_vcs.sort_values('timestamp'),
on='timestamp', direction='backward')
result11 = df_ps_merged.apply(lambda row: Q4ToMatrix(row['q4_x'], row['q4_y'],row['q4_z'],row['q4_w'],
row['uss_ps_1st_corner_x'], row['uss_ps_1st_corner_y'],
row['vwld_x'], row['vwld_y']), axis=1) # get corner points wcs x & y
result12 = df_ps_merged.apply(lambda row: Q4ToMatrix(row['q4_x'], row['q4_y'],row['q4_z'],row['q4_w'],
row['uss_ps_2nd_corner_x'], row['uss_ps_2nd_corner_y'],
row['vwld_x'], row['vwld_y']), axis=1) # get corner points wcs x & y
x_wld_1 = [res[0] for res in result11] # corner 1 x
y_wld_1 = [res[1] for res in result11] # corner 1 y
x_wld_2 = [res[0] for res in result12] # corner 2 x
y_wld_2 = [res[1] for res in result12] # corner 2 y
df_ps_merged["uss_ps_1st_corner_x_wld"] = x_wld_1
df_ps_merged["uss_ps_1st_corner_y_wld"] = y_wld_1
df_ps_merged["uss_ps_2nd_corner_x_wld"] = x_wld_2
df_ps_merged["uss_ps_2nd_corner_y_wld"] = y_wld_2
df_ps_merged = df_ps_merged.sort_values(['timestamp',"uss_ps_id"])
dt_min = df_ps_merged["timestamp"].min() - dt # dt:ms
# dt_max = df_ps_merged["timestamp"].max() + dt*1.2
# df_obj_slice = df_obj[(df_obj["timestamp"]>=dt_min) & (df_obj["timestamp"]<=dt_max)]
df_obj_slice = df_obj[(df_obj["timestamp"]>=dt_min)]
df_obj_merged = pd.merge_asof(df_obj_slice.sort_values('timestamp'), df_vcs.sort_values('timestamp'),
on='timestamp', direction='backward')
df_obj_merged = df_obj_merged.sort_values(['timestamp',"uss_obj_x_id"])
result21 = df_obj_merged.apply(lambda row: Q4ToMatrix(row['q4_x'], row['q4_y'],row['q4_z'],row['q4_w'],
row['uss_obj_x_pos_1_x'], row['uss_obj_x_pos_1_y'],
row['vwld_x'], row['vwld_y']), axis=1) # get object points wcs x & y
result22 = df_obj_merged.apply(lambda row: Q4ToMatrix(row['q4_x'], row['q4_y'],row['q4_z'],row['q4_w'],
row['uss_obj_x_pos_2_x'], row['uss_obj_x_pos_2_y'],
row['vwld_x'], row['vwld_y']), axis=1) # get object points wcs x & y
x_obj_1 = [res[0] for res in result21]
y_obj_1 = [res[1] for res in result21]
x_obj_2 = [res[0] for res in result22]
y_obj_2 = [res[1] for res in result22]
df_obj_merged["uss_obj_x_pos_1_x_wld"] = x_obj_1
df_obj_merged["uss_obj_x_pos_1_y_wld"] = y_obj_1
df_obj_merged["uss_obj_x_pos_2_x_wld"] = x_obj_2
df_obj_merged["uss_obj_x_pos_2_y_wld"] = y_obj_2
df_obj_merged = df_obj_merged[(df_obj_merged["uss_obj_x_confidence"] > 0)] # confidence means effective obj
df_obj_merged = df_obj_merged.sort_values(["timestamp","uss_obj_x_id"])
df_obj_merged.reset_index(drop=True, inplace=True)
# merge ipm slot
df_ipm_merged = pd.merge_asof(df_ipm.sort_values('timestamp'), df_vcs.sort_values('timestamp'),
on='timestamp', direction='backward')
res_ipm = df_ipm_merged.apply(lambda row: Q4ToMatrix(row['q4_x'], row['q4_y'],row['q4_z'],row['q4_w'],
row['ipm_vcs_x'], row['ipm_vcs_y'],
row['vwld_x'], row['vwld_y']), axis=1) # get corner points wcs x & y
x_ipm = [res[0] for res in res_ipm]
y_ipm = [res[1] for res in res_ipm]
df_ipm_merged["ipm_wld_x"] = x_ipm
df_ipm_merged["ipm_wld_y"] = y_ipm
df_ipm_merged = df_ipm_merged.sort_values(['timestamp',"idx_slot","idx_corner"])
df_ipm_merged.reset_index(drop=True, inplace=True)
return df_ps_merged, df_obj_merged, df_ipm_merged
def get_data(df_ps, df_obj, df_ipm, dfv, idx_list, dt = 6*1000):
# dt: ms
dt_min = df_ps["timestamp"].min() - dt # uss corner首次出现时刻往前推dt
dt_max = df_ps["timestamp"].max() + dt*1.2 # uss corner首次出现时刻往后推dt
df_vcs_out = dfv[(dfv["timestamp"]>=dt_min) & (dfv["timestamp"]<=dt_max)]
pt_list = []
pt0_t = df_vcs_out["timestamp"].tolist() #
pt0_x = df_vcs_out["vwld_x"].tolist() #
pt0_y = df_vcs_out["vwld_y"].tolist() #
pt_vcs = (pt0_t, pt0_x, pt0_y)
pt_list.append(pt_vcs) # vcs vehicle trajectory point
dt_min_list = []
dt_max_list = []
pt_corner_list = []
for idx in idx_list:
df_ps_idx = df_ps[df_ps["uss_ps_id"] == idx]
dt_min_list.append(dt_min)
dt_max_list.append(dt_max)
pt_c1_t = df_ps_idx["timestamp"].tolist()
pt_c1_x = df_ps_idx["uss_ps_1st_corner_x_wld"].tolist()
pt_c1_y = df_ps_idx["uss_ps_1st_corner_y_wld"].tolist()
pt_c2_x = df_ps_idx["uss_ps_2nd_corner_x_wld"].tolist()
pt_c2_y = df_ps_idx["uss_ps_2nd_corner_y_wld"].tolist()
pt_corner = (pt_c1_t, pt_c1_x, pt_c1_y, pt_c2_x, pt_c2_y )
pt_corner_list.append(pt_corner) # each corner, 2 points
pt_list.append(pt_corner_list)
# for idx in range(1,13):
# df_obj_idx = df_obj[df_obj["uss_obj_x_id"]==idx]
# if df_obj_idx.shape[0] > 0:
# # obj_x_pos_1 与 obj_x_pos_2 坐标相同对应点状obs, 坐标不相同对应杆状obs
# pt1_x = df_obj_idx["uss_obj_x_pos_1_x_wld"].tolist()
# pt1_y = df_obj_idx["uss_obj_x_pos_1_y_wld"].tolist()
# pt_t = df_obj_idx["timestamp"].tolist()
# pt2_x = df_obj_idx["uss_obj_x_pos_2_x_wld"].tolist()
# pt2_y = df_obj_idx["uss_obj_x_pos_2_y_wld"].tolist()
# pts = (pt_t, pt1_x, pt1_y, pt2_x, pt2_y)
# pt_list.append(pts)
# obj_x_pos_1 与 obj_x_pos_2 坐标相同对应点状obs, 坐标不相同对应杆状obs
pt_t = df_obj["timestamp"].tolist()
pt1_x = df_obj["uss_obj_x_pos_1_x_wld"].tolist()
pt1_y = df_obj["uss_obj_x_pos_1_y_wld"].tolist()
pt2_x = df_obj["uss_obj_x_pos_2_x_wld"].tolist()
pt2_y = df_obj["uss_obj_x_pos_2_y_wld"].tolist()
pt_obj_1 = (pt_t, pt1_x, pt1_y)
pt_obj_2 = (pt_t, pt2_x, pt2_y)
pt_list.append(pt_obj_1)
pt_list.append(pt_obj_2)
# ipm corner
df0 = df_ipm[df_ipm["idx_corner"]==0]
df1 = df_ipm[df_ipm["idx_corner"]==1]
df2 = df_ipm[df_ipm["idx_corner"]==2]
df3 = df_ipm[df_ipm["idx_corner"]==3]
df0 = df0.sort_values(['timestamp',"idx_slot"])
df1 = df1.sort_values(['timestamp',"idx_slot"])
df2 = df2.sort_values(['timestamp',"idx_slot"])
df3 = df3.sort_values(['timestamp',"idx_slot"])
pt_ipm_t0 = df0["timestamp"].tolist()
pt0_ipm_x = df0["ipm_wld_x"].tolist()
pt0_ipm_y = df0["ipm_wld_y"].tolist()
pt_ipm_t1 = df1["timestamp"].tolist()
pt1_ipm_x = df1["ipm_wld_x"].tolist()
pt1_ipm_y = df1["ipm_wld_y"].tolist()
pt_ipm_t2 = df2["timestamp"].tolist()
pt2_ipm_x = df2["ipm_wld_x"].tolist()
pt2_ipm_y = df2["ipm_wld_y"].tolist()
pt_ipm_t3 = df3["timestamp"].tolist()
pt3_ipm_x = df3["ipm_wld_x"].tolist()
pt3_ipm_y = df3["ipm_wld_y"].tolist()
pt_ipm_0 = (pt_ipm_t0, pt0_ipm_x, pt0_ipm_y)
pt_ipm_1 = (pt_ipm_t1, pt1_ipm_x, pt1_ipm_y)
pt_ipm_2 = (pt_ipm_t2, pt2_ipm_x, pt2_ipm_y)
pt_ipm_3 = (pt_ipm_t3, pt3_ipm_x, pt3_ipm_y)
pt_list.append(pt_ipm_0)
pt_list.append(pt_ipm_1)
pt_list.append(pt_ipm_2)
pt_list.append(pt_ipm_3)
return pt_list
def find_first_index_greater_than(st, elem):
nums = len(st)
for index, value in enumerate(st):
if value > elem:
return index
if index == nums -1:
return nums
return 0 # 如果没有找到，返回-1或者None, 视具体情况而定
def update_line(frame, args, plots):
print("current frame is: ", frame)
pt_vcs = args[0]
pt_ps_list = args[1]
pt_obj_pos_1 = args[2]
pt_obj_pos_2 = args[3]
pt_ipm_0 = args[4]
pt_ipm_1 = args[5]
pt_ipm_2 = args[6]
pt_ipm_3 = args[7]
line_vcs = plots[0]
scat_obj_pos_1 = plots[1]
scat_obj_pos_2 = plots[2]
scat_ipm_0 = plots[3]
scat_ipm_1 = plots[4]
scat_ipm_2 = plots[5]
scat_ipm_3 = plots[6]
line_corner_list = []
if frame == 0:
line_vcs.set_data([], [])
data = np.stack([[], []]).T
scat_obj_pos_1.set_offsets(data)
scat_obj_pos_2.set_offsets(data)
scat_ipm_0.set_offsets(data)
scat_ipm_1.set_offsets(data)
scat_ipm_2.set_offsets(data)
scat_ipm_3.set_offsets(data)
for idx in range(7,len(plots)-1,2):
line_corner_1 = plots[idx]
line_corner_2 = plots[idx + 1]
line_corner_1.set_data([], [])
line_corner_2.set_data([], [])
line_corner_list.append(line_corner_1)
line_corner_list.append(line_corner_2)
else:
pt0_t = pt_vcs[0]
pt0_x = pt_vcs[1]
pt0_y = pt_vcs[2]
t0 = max([pt for pt in pt0_t[:frame]])
print("current vcs time is: ", t0)
x0 = [pt for pt in pt0_x[:frame]]
y0 = [pt for pt in pt0_y[:frame]]
""" update vehicle trajectory"""
line_vcs.set_data(x0, y0)
# text_pt.set_position(x0[:frame],y0[:frame])
# axt.set_title(f'Current Time: {t0}',fontsize=16)
pt_t_pos = pt_obj_pos_1[0]
pt_x_pos_1 = pt_obj_pos_1[1]
pt_y_pos_1 = pt_obj_pos_1[2]
pt_x_pos_2 = pt_obj_pos_2[1]
pt_y_pos_2 = pt_obj_pos_2[2]
""" update each obj,2 position"""
fm_pos = find_first_index_greater_than(pt_t_pos, t0)
x_pos_1 = [pt for pt in pt_x_pos_1[:fm_pos]]
y_pos_1 = [pt for pt in pt_y_pos_1[:fm_pos]]
pos_1 = np.stack([x_pos_1, y_pos_1]).T
scat_obj_pos_1.set_offsets(pos_1)
x_pos_2 = [pt for pt in pt_x_pos_2[:fm_pos]]
y_pos_2 = [pt for pt in pt_y_pos_2[:fm_pos]]
pos_2 = np.stack([x_pos_2, y_pos_2]).T
scat_obj_pos_2.set_offsets(pos_2)
""" update ipm 4 corner"""
fm_ipm_0 = find_first_index_greater_than(pt_ipm_0[0], t0)
# if frame > 5 and fm_ipm_0 == 0:
# fm_ipm_0 = -1
x_ipm_0 = [pt for pt in pt_ipm_0[1][:fm_ipm_0]]
y_ipm_0 = [pt for pt in pt_ipm_0[2][:fm_ipm_0]]
ipm_0 = np.stack([x_ipm_0, y_ipm_0]).T
scat_ipm_0.set_offsets(ipm_0)
fm_ipm_1 = find_first_index_greater_than(pt_ipm_1[0], t0)
# if frame > 5 and fm_ipm_1 == 0:
# fm_ipm_1 = -1
x_ipm_1 = [pt for pt in pt_ipm_1[1][:fm_ipm_1]]
y_ipm_1 = [pt for pt in pt_ipm_1[2][:fm_ipm_1]]
ipm_1 = np.stack([x_ipm_1, y_ipm_1]).T
scat_ipm_1.set_offsets(ipm_1)
fm_ipm_2 = find_first_index_greater_than(pt_ipm_2[0], t0)
# if frame > 5 and fm_ipm_2 == 0:
# fm_ipm_2 = -1
x_ipm_2 = [pt for pt in pt_ipm_2[1][:fm_ipm_2]]
y_ipm_2 = [pt for pt in pt_ipm_2[2][:fm_ipm_2]]
ipm_2 = np.stack([x_ipm_2, y_ipm_2]).T
scat_ipm_2.set_offsets(ipm_2)
fm_ipm_3 = find_first_index_greater_than(pt_ipm_3[0], t0)
# if frame > 5 and fm_ipm_3 == 0:
# fm_ipm_3 = -1
x_ipm_3 = [pt for pt in pt_ipm_3[1][:fm_ipm_3]]
y_ipm_3 = [pt for pt in pt_ipm_3[2][:fm_ipm_3]]
ipm_3 = np.stack([x_ipm_3, y_ipm_3]).T
scat_ipm_3.set_offsets(ipm_3)
""" update each corner,2 position"""
idxx = range(7,len(plots)-1,2)
# """ update each corner,2 position"""
for num, idx in enumerate(idxx):
line_corner_1 = plots[idx]
line_corner_2 = plots[idx + 1]
pt_t = pt_ps_list[num][0]
pt1_x = pt_ps_list[num][1]
pt1_y = pt_ps_list[num][2]
pt2_x = pt_ps_list[num][3]
pt2_y = pt_ps_list[num][4]
fm12 = find_first_index_greater_than(pt_t, t0)
# print("fm12 is: ", fm12)
# if fm12 > 0:
# fm_old[num] = fm12
# if frame > 0 and fm12 == 0:
# fm12 = fm_old[num]
# fm1 = fm1_old
x1 = [pt for pt in pt1_x[:fm12]]
y1 = [pt for pt in pt1_y[:fm12]]
line_corner_1.set_data(x1, y1)
x2 = [pt for pt in pt2_x[:fm12]]
y2 = [pt for pt in pt2_y[:fm12]]
line_corner_2.set_data(x2, y2)
line_corner_list.append(line_corner_1)
line_corner_list.append(line_corner_2)
# plt.xlabel('x(wcs-m)',fontsize=16)
# plt.ylabel("y(wcs-m)",fontsize=16)
return [line_vcs, scat_obj_pos_1, scat_obj_pos_2, scat_ipm_0, scat_ipm_1, scat_ipm_2, scat_ipm_3] + line_corner_list
if __name__ == '__main__':
fname = "D:\\work\\data\\data105130.json"
df_vcs, df_uss_obj, df_uss_ps, df_ipm_ps = process_json(fname)
idx_uss = [180]
dt = 8 * 1000 #ms
df_uss_mg, df_obj_mg, df_ipm_mg = data_merge(df_vcs, df_uss_obj, df_uss_ps, df_ipm_ps, idx_uss,dt)
data_args = get_data(df_uss_mg, df_obj_mg, df_ipm_mg, df_vcs, idx_uss, dt)
len_frames = len(data_args[0][0])
x_min = min(data_args[0][1]) - 10
x_max = max(data_args[0][1]) + 10
y_min = min(data_args[0][2]) - 10
y_max = max(data_args[0][2]) + 10
fig, ax = plt.subplots(figsize=(21,10))
ax.set(xlim=[x_min, x_max], ylim=[y_min, y_max])
# ax.set(xlim=[-80, 10], ylim=[-30, 0])
ax.set_xlabel(r"wcs_x [m]",fontsize=16)
ax.set_ylabel(r"wcs_y [m]",fontsize=16)
title_ani = ax.set_title("UssObj & USSParkingSlots & IPMParkingSlots",fontsize=16)
ax.grid(True)
xticks_labels = range(int(x_min)-1, int(x_max) + 1, 1) # x轴刻度间隔设置为5
yticks_labels = range(int(y_min)-1, int(y_max) + 1, 1) # y轴刻度间隔设置为50
# 设置x轴和y轴的刻度
ax.set_xticks(xticks_labels)
ax.set_yticks(yticks_labels)
line_vcs, = ax.plot([], [], 'ko', label='ego trajectory')
scat_obj_pos_1 = ax.scatter([], [], c='r', s=15, label='obj pos 1')
scat_obj_pos_2 = ax.scatter([], [], c='r', s=15, label='obj pos 2')
scat_ipm_0 = ax.scatter([], [], c='g', marker="o", s=15, label='ipm corner 0')
scat_ipm_1 = ax.scatter([], [], c='g', marker="d", s=15, label='ipm corner 1')
scat_ipm_2 = ax.scatter([], [], c='g', marker="s", s=15, label='ipm corner 2')
scat_ipm_3 = ax.scatter([], [], c='g', marker="*", s=15, label='ipm corner 3')
#text_pt = plt.text(3.5,0.8,"",fontsize=16)
plot_list = []
plot_list.append(line_vcs)
plot_list.append(scat_obj_pos_1)
plot_list.append(scat_obj_pos_2)
plot_list.append(scat_ipm_0)
plot_list.append(scat_ipm_1)
plot_list.append(scat_ipm_2)
plot_list.append(scat_ipm_3)
for idx in idx_uss:
line_corner_1, = ax.plot([], [], 'b-', label="id=" + str(idx) + " conner 1")
line_corner_2, = ax.plot([], [], 'c-', label="id=" + str(idx) + " conner 2")
plot_list.append(line_corner_1)
plot_list.append(line_corner_2)
ax.legend()
anim = animation.FuncAnimation(fig, update_line, frames=len_frames,
fargs=(data_args, plot_list),interval=100,repeat=False)
anim.save("113151_6.gif", writer='pillow')
