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 * tv02 = 5
z2 = g * t**2 / 2 + v02 * tscat = 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]).Tscat.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.randomuss_unit = 100
angle_unit = 4096.0def gen_fig():fig = plt.figure()ax = plt.axes(xlim=(10, 80), ylim=(-80, 20))line, = ax.plot([], [], 'r-')return fig, linedef update(line, data):# 更新点的位置line.set_data(data[0], data[1])return linedef 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 pointpt_v = np.array([xv,yv,0]).reshape(-1,1) # vichle center pointpt_ = 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]) / tpt_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_vreturn [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 lessipm_list = ipm_dcts["ipm_pt_slot_points"]for idx_corner,ipm_pts in enumerate(ipm_list): # total 4 corner pointspt_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_listdf_dict_ipm["idx_slot"] = idx_slot_listdf_dict_ipm["idx_corner"] = idx_corner_listdf_dict_ipm["ipm_vcs_x"] = pt_x_listdf_dict_ipm["ipm_vcs_y"] = pt_y_listdf_dict_ipm["ipm_vcs_z"] = pt_z_listdf_dict_ipm["ipm_score"] = pt_score_listdf_dict_ipm["ipm_vis"] = pt_vis_listdf_dict_ipm["ipm_ort_x"] = pt_ort_x_listdf_dict_ipm["ipm_ort_y"] = pt_ort_y_listdf_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_listdf_dict_vcs["vwld_x"] = vcs_wld_x_listdf_dict_vcs["vwld_y"] = vcs_wld_y_listdf_dict_vcs["vwld_z"] = vcs_wld_z_listdf_dict_vcs["q4_x"] = q4_x_listdf_dict_vcs["q4_y"] = q4_y_listdf_dict_vcs["q4_z"] = q4_z_listdf_dict_vcs["q4_w"] = q4_w_listdf_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 confidenceuss_obj_x_type = obj_list["uss_obj_x_type_en"] #obj typeuss_obj_x_height = obj_list["uss_obj_x_height_en"]/uss_unituss_obj_x_pos_1_x = obj_list["uss_obj_x_pos_1"]["x_si16"]/uss_unituss_obj_x_pos_1_y = obj_list["uss_obj_x_pos_1"]["y_si16"]/uss_unituss_obj_x_pos_2_x = obj_list["uss_obj_x_pos_2"]["x_si16"]/uss_unituss_obj_x_pos_2_y = obj_list["uss_obj_x_pos_2"]["y_si16"]/uss_unituss_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_unituss_ps_depth = ps_list["uss_ps_depth_ui16"]/uss_unituss_ps_angle = ps_list["uss_ps_angle_ui16"] # /angle_unit + np.pi/2uss_ps_status = ps_list["uss_ps_status_en"] # NONE=0,SEARCH =1, CORRECT=2uss_ps_type = ps_list["uss_ps_type_en"] # NONE=0,PARALLEL=1,CROSS=2,DIAGONAL=3uss_ps_1st_corner_x = ps_list["uss_ps_1st_corner"]["x_si16"]/uss_unituss_ps_1st_corner_y = ps_list["uss_ps_1st_corner"]["y_si16"]/uss_unituss_ps_2nd_corner_x = ps_list["uss_ps_2nd_corner"]["x_si16"]/uss_unituss_ps_2nd_corner_y = ps_list["uss_ps_2nd_corner"]["y_si16"]/uss_unituss_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_ussdf_dict_uss_obj["uss_obj_x_id"] = uss_obj_x_id_listdf_dict_uss_obj["uss_obj_x_confidence"] = uss_obj_x_cfd_listdf_dict_uss_obj["uss_obj_x_type"] = uss_obj_x_type_listdf_dict_uss_obj["uss_obj_x_height"] = uss_obj_x_height_listdf_dict_uss_obj["uss_obj_x_pos_1_x"] = uss_obj_x_pos_1_x_listdf_dict_uss_obj["uss_obj_x_pos_1_y"] = uss_obj_x_pos_1_y_listdf_dict_uss_obj["uss_obj_x_pos_2_x"] = uss_obj_x_pos_2_x_listdf_dict_uss_obj["uss_obj_x_pos_2_y"] = uss_obj_x_pos_2_y_listdf_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_psdf_dict_uss_ps["uss_ps_id"] = uss_ps_id_listdf_dict_uss_ps["uss_ps_length"] = uss_ps_length_listdf_dict_uss_ps["uss_ps_depth"] = uss_ps_depth_listdf_dict_uss_ps["uss_ps_angle"] = uss_ps_angle_listdf_dict_uss_ps["uss_ps_status"] = uss_ps_status_listdf_dict_uss_ps["uss_ps_type"] = uss_ps_type_listdf_dict_uss_ps["uss_ps_1st_corner_x"] = uss_ps_1st_corner_x_listdf_dict_uss_ps["uss_ps_1st_corner_y"] = uss_ps_1st_corner_y_listdf_dict_uss_ps["uss_ps_2nd_corner_x"] = uss_ps_2nd_corner_x_listdf_dict_uss_ps["uss_ps_2nd_corner_y"] = uss_ps_2nd_corner_y_listdf_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_psdef data_merge(df_vcs, df_obj, df_uss, df_ipm, idx_list, dt = 6000):# 分析第一个case,车位id 164,166df_uss_list = []for idx in idx_list: df_uss_slice = df_uss[(df_uss["uss_ps_id"]==idx)] # uss id filterdf_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 & yresult12 = 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 & yx_wld_1 = [res[0] for res in result11] # corner 1 xy_wld_1 = [res[1] for res in result11] # corner 1 yx_wld_2 = [res[0] for res in result12] # corner 2 xy_wld_2 = [res[1] for res in result12] # corner 2 ydf_ps_merged["uss_ps_1st_corner_x_wld"] = x_wld_1df_ps_merged["uss_ps_1st_corner_y_wld"] = y_wld_1df_ps_merged["uss_ps_2nd_corner_x_wld"] = x_wld_2df_ps_merged["uss_ps_2nd_corner_y_wld"] = y_wld_2df_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 & yresult22 = 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 & yx_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_1df_obj_merged["uss_obj_x_pos_1_y_wld"] = y_obj_1df_obj_merged["uss_obj_x_pos_2_x_wld"] = x_obj_2df_obj_merged["uss_obj_x_pos_2_y_wld"] = y_obj_2df_obj_merged = df_obj_merged[(df_obj_merged["uss_obj_x_confidence"] > 0)] # confidence means effective objdf_obj_merged = df_obj_merged.sort_values(["timestamp","uss_obj_x_id"])df_obj_merged.reset_index(drop=True, inplace=True)# merge ipm slotdf_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 & yx_ipm = [res[0] for res in res_ipm]y_ipm = [res[1] for res in res_ipm]df_ipm_merged["ipm_wld_x"] = x_ipmdf_ipm_merged["ipm_wld_y"] = y_ipmdf_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_mergeddef get_data(df_ps, df_obj, df_ipm, dfv, idx_list, dt = 6*1000):# dt: msdt_min = df_ps["timestamp"].min() - dt # uss corner首次出现时刻往前推dtdt_max = df_ps["timestamp"].max() + dt*1.2 # uss corner首次出现时刻往后推dtdf_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 pointdt_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 pointspt_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, 坐标不相同对应杆状obspt_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 cornerdf0 = 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_listdef find_first_index_greater_than(st, elem):nums = len(st)for index, value in enumerate(st):if value > elem:return indexif index == nums -1:return numsreturn 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([[], []]).Tscat_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]).Tscat_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]).Tscat_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 = -1x_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]).Tscat_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 = -1x_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]).Tscat_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 = -1x_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]).Tscat_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 = -1x_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]).Tscat_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_oldx1 = [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_listif __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 #msdf_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]) - 10x_max = max(data_args[0][1]) + 10y_min = min(data_args[0][2]) - 10y_max = max(data_args[0][2]) + 10fig, 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轴刻度间隔设置为5yticks_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')
