2021-08-16

LaneGCN

Data

使用到的数据：

feats：坐标都转移到以orig为原点的坐标系下，所有actor的前20个时刻的时序特征（actor_num，3，20） 3=两个坐标+一个标志位（是否pad）

ctrs：坐标都转移到以orig为原点的坐标系下，所有actor的最后一个时刻的位置特征

rot：是一个旋转矩阵，t-1时刻转到t0时刻的，根据theta角计算得到， xy*rot+orig

orig：原始的t0时刻位置，T个时刻里最后一个时刻（t=0）的位置。

gt_preds：每个actor的真实未来30个时刻的坐标

has_preds：每个actor未来30个时刻是否有真实的坐标

trajs: 坐标未转移前的，轨迹的xy坐标

steps：时间步骤，从时间戳变成0——t的

theta: t-1时刻到t0时刻移动过程的旋转角度

graph： num_nodes：当前scence中抽象出来的lane节点个数

​ ctrs: 经过坐标系处理的 lane中心线坐标

​ feature：两个中心线坐标的差

​ turn：向左还是向右转向

​ control：是否交通管控

​ intersect：路口

​ pre：前驱

​ suc：后继

​ left：左侧节点

​ right：右侧节点

​ u：nodeid

​ v：nodeid

Argoverse数据的处理过程在data.py文件中，主要是三个函数：

class ArgoDataset(Dataset):
     def read_argo_data(self, idx):
        city = copy.deepcopy(self.avl[idx].city)

        """TIMESTAMP,TRACK_ID,OBJECT_TYPE,X,Y,CITY_NAME"""
        df = copy.deepcopy(self.avl[idx].seq_df)
        
        agt_ts = np.sort(np.unique(df['TIMESTAMP'].values))
        mapping = dict()
        #按照时间戳排序，并映射，每个对象统计了50个时间戳
        for i, ts in enumerate(agt_ts):
            mapping[ts] = i

        trajs = np.concatenate((
            df.X.to_numpy().reshape(-1, 1),
            df.Y.to_numpy().reshape(-1, 1)), 1)
        
        #按时间戳统计XY
        steps = [mapping[x] for x in df['TIMESTAMP'].values]
        steps = np.asarray(steps, np.int64)

        objs = df.groupby(['TRACK_ID', 'OBJECT_TYPE']).groups   #轨迹ID，对象类型，Index
        keys = list(objs.keys())  #轨迹ID及其类型
        obj_type = [x[1] for x in keys]  #所有的类型

        #只统计‘AGENT’障碍物的
        agt_idx = obj_type.index('AGENT')
        idcs = objs[keys[agt_idx]]   #AGENT每个时间戳的Index.
       
        agt_traj = trajs[idcs]
        agt_step = steps[idcs]

        del keys[agt_idx]
        ctx_trajs, ctx_steps = [], []
        for key in keys:  #统计其他障碍物的信息，不同的OTHERS，AV等
            idcs = objs[key]
            ctx_trajs.append(trajs[idcs])
            ctx_steps.append(steps[idcs])

        data = dict()
        data['city'] = city
        data['trajs'] = [agt_traj] + ctx_trajs  
        #坐标未转移前的，轨迹的xy坐标，第一个是AGENT的，后面是其他的
        data['steps'] = [agt_step] + ctx_steps  #0-T的时间步，第一个是AGENT的，后面是其他的
        return data 

第二个函数，处理每个ACTOR的特征：

def get_obj_feats(self, data):
    orig = data['trajs'][0][19].copy().astype(np.float32) #第20个时刻的位置

    if self.train and self.config['rot_aug']:
        theta = np.random.rand() * np.pi * 2.0
    else:
        pre = data['trajs'][0][18] - orig  #第19个时刻的位置
        theta = np.pi - np.arctan2(pre[1], pre[0])  #theta角

    rot = np.asarray([   #旋转矩阵
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta), np.cos(theta)]], np.float32)

    feats, ctrs, gt_preds, has_preds = [], [], [], []
    for traj, step in zip(data['trajs'], data['steps']):
        if 19 not in step:
            continue

        gt_pred = np.zeros((30, 2), np.float32)
        has_pred = np.zeros(30, np.bool)
        future_mask = np.logical_and(step >= 20, step < 50)
        #前20个时刻用来训练和学习，后30个时刻用来预测
        post_step = step[future_mask] - 20
        post_traj = traj[future_mask]
        gt_pred[post_step] = post_traj  #真实的位置
        has_pred[post_step] = 1
        
        obs_mask = step < 20
        step = step[obs_mask]
        traj = traj[obs_mask]
        idcs = step.argsort()
        step = step[idcs]
        traj = traj[idcs]
        
        for i in range(len(step)):
            if step[i] == 19 - (len(step) - 1) + i:
                break
        step = step[i:]
        traj = traj[i:]
		
        #特征处理成三维的，前两位是位置特征进行处理后的坐标，第三位表示是否为pad
        feat = np.zeros((20, 3), np.float32)
        feat[step, :2] = np.matmul(rot, (traj - orig.reshape(-1, 2)).T).T  
        feat[step, 2] = 1.0

        x_min, x_max, y_min, y_max = self.config['pred_range']
        if feat[-1, 0] < x_min or feat[-1, 0] > x_max or feat[-1, 1] < y_min or feat[-1, 1] > y_max:
            continue

        ctrs.append(feat[-1, :2].copy())
        feat[1:, :2] -= feat[:-1, :2]  #相邻时刻的位置差
        feat[step[0], :2] = 0  #0step(相当于t-T时刻设置为0）
        feats.append(feat)
        gt_preds.append(gt_pred)
        has_preds.append(has_pred)

    feats = np.asarray(feats, np.float32)
    ctrs = np.asarray(ctrs, np.float32)
    gt_preds = np.asarray(gt_preds, np.float32)
    has_preds = np.asarray(has_preds, np.bool)

    data['feats'] = feats #旋转，位移特征
    data['ctrs'] = ctrs #19时刻的位置
    data['orig'] = orig
    data['theta'] = theta
    data['rot'] = rot
    data['gt_preds'] = gt_preds
    data['has_preds'] = has_preds
    return data

处理图网络数据的函数：

def get_lane_graph(self, data):
    """Get a rectangle area defined by pred_range."""
    x_min, x_max, y_min, y_max = self.config['pred_range']
    radius = max(abs(x_min), abs(x_max)) + max(abs(y_min), abs(y_max))
    lane_ids = self.am.get_lane_ids_in_xy_bbox(data['orig'][0], data['orig'][1], data['city'], radius)
    #lane_ids是一部分道路的ID，有10个lane_node位置
    lane_ids = copy.deepcopy(lane_ids)
    
    lanes = dict()
    for lane_id in lane_ids:  
        lane = self.am.city_lane_centerlines_dict[data['city']][lane_id]
        lane = copy.deepcopy(lane)
        #有十对位置来描述centerline,一个lane有9个lane_node.
        centerline = np.matmul(data['rot'], (lane.centerline - data['orig'].reshape(-1, 2)).T).T
        #进行旋转变换
        x, y = centerline[:, 0], centerline[:, 1]
        if x.max() < x_min or x.min() > x_max or y.max() < y_min or y.min() > y_max:
            continue
            #筛选出超出范围的
        else:
            """Getting polygons requires original centerline"""
            polygon = self.am.get_lane_segment_polygon(lane_id, data['city'])
            polygon = copy.deepcopy(polygon)
            lane.centerline = centerline
            lane.polygon = np.matmul(data['rot'], (polygon[:, :2] - data['orig'].reshape(-1, 2)).T).T
            lanes[lane_id] = lane
        
    lane_ids = list(lanes.keys())
    ctrs, feats, turn, control, intersect = [], [], [], [], []
    for lane_id in lane_ids:
        lane = lanes[lane_id] #一个lane_seg
        ctrln = lane.centerline #10个位置
        #ctrln: has num_seg+1 xy,
        num_segs = len(ctrln) - 1

        t = ctrln[:-1]
        t1 = ctrln[1:]
        ctrs.append(np.asarray((ctrln[:-1] + ctrln[1:]) / 2.0, np.float32))  #旋转变换之后的中心位置
        feats.append(np.asarray(ctrln[1:] - ctrln[:-1], np.float32))  #旋转变换之后的位移差
        
        x = np.zeros((num_segs, 2), np.float32)
        #两个标志位表示方向
        if lane.turn_direction == 'LEFT': #10
            x[:, 0] = 1
        elif lane.turn_direction == 'RIGHT': #01
            x[:, 1] = 1
        else:
            pass
        turn.append(x)

        control.append(lane.has_traffic_control * np.ones(num_segs, np.float32))
        intersect.append(lane.is_intersection * np.ones(num_segs, np.float32))
        
    node_idcs = []
    count = 0
    for i, ctr in enumerate(ctrs):
        node_idcs.append(range(count, count + len(ctr)))
        #记录所有lane_seg里all_lane_node的id.
        count += len(ctr)
    num_nodes = count #all_node数量。
    
    pre, suc = dict(), dict()
    for key in ['u', 'v']:
        pre[key], suc[key] = [], []
    for i, lane_id in enumerate(lane_ids):
        lane = lanes[lane_id]  #lane_seg
        idcs = node_idcs[i] #lane_seg里的lane_node_id，eg:[0-8]
        
        pre['u'] += idcs[1:]  #'u':1-8
        pre['v'] += idcs[:-1] #'v':0-7
        if lane.predecessors is not None:
            for nbr_id in lane.predecessors: #前驱lane_seg
                if nbr_id in lane_ids:  #前驱lane_seg_id
                    j = lane_ids.index(nbr_id)
                    pre['u'].append(idcs[0])  #补充前驱lane_seg的第一个lane_node
                    pre['v'].append(node_idcs[j][-1])   #补充前驱lane_seg的最后一个lane_node
                 
        suc['u'] += idcs[:-1]
        suc['v'] += idcs[1:]
        if lane.successors is not None:
            for nbr_id in lane.successors:
                if nbr_id in lane_ids:
                    j = lane_ids.index(nbr_id)
                    suc['u'].append(idcs[-1])
                    suc['v'].append(node_idcs[j][0])

    lane_idcs = []
    for i, idcs in enumerate(node_idcs):
        lane_idcs.append(i * np.ones(len(idcs), np.int64))
    lane_idcs = np.concatenate(lane_idcs, 0)
    #类似于[0,0,0,0,0,0,0,0,0,1X9,2X9,…………]

    pre_pairs, suc_pairs, left_pairs, right_pairs = [], [], [], []
    for i, lane_id in enumerate(lane_ids):
        lane = lanes[lane_id]

        nbr_ids = lane.predecessors
        if nbr_ids is not None:
            for nbr_id in nbr_ids:
                if nbr_id in lane_ids:
                    j = lane_ids.index(nbr_id)
                    pre_pairs.append([i, j])  #两个lane_seg的索引id对

        nbr_ids = lane.successors
        if nbr_ids is not None:
            for nbr_id in nbr_ids:
                if nbr_id in lane_ids:
                    j = lane_ids.index(nbr_id)
                    suc_pairs.append([i, j])

        nbr_id = lane.l_neighbor_id
        if nbr_id is not None:
            if nbr_id in lane_ids:
                j = lane_ids.index(nbr_id)
                left_pairs.append([i, j])

        nbr_id = lane.r_neighbor_id
        if nbr_id is not None:
            if nbr_id in lane_ids:
                j = lane_ids.index(nbr_id)
                right_pairs.append([i, j])
    pre_pairs = np.asarray(pre_pairs, np.int64)
    suc_pairs = np.asarray(suc_pairs, np.int64)
    left_pairs = np.asarray(left_pairs, np.int64)
    right_pairs = np.asarray(right_pairs, np.int64)
                
    graph = dict()
    graph['ctrs'] = np.concatenate(ctrs, 0)  #二维
    graph['num_nodes'] = num_nodes
    graph['feats'] = np.concatenate(feats, 0)  #二维
    graph['turn'] = np.concatenate(turn, 0)  #二维
    graph['control'] = np.concatenate(control, 0)
    graph['intersect'] = np.concatenate(intersect, 0)
    graph['pre'] = [pre]
    graph['suc'] = [suc]
    graph['lane_idcs'] = lane_idcs
    graph['pre_pairs'] = pre_pairs
    graph['suc_pairs'] = suc_pairs   #lanesegment_index0,  lanesegment_index1
    graph['left_pairs'] = left_pairs
    graph['right_pairs'] = right_pairs
    
    for k1 in ['pre', 'suc']:
        for k2 in ['u', 'v']:
            graph[k1][0][k2] = np.asarray(graph[k1][0][k2], np.int64)
    
    for key in ['pre', 'suc']:
        if 'scales' in self.config and self.config['scales']:
            #TODO: delete here
            graph[key] += dilated_nbrs2(graph[key][0], graph['num_nodes'], self.config['scales'])
        else:
            graph[key] += dilated_nbrs(graph[key][0], graph['num_nodes'], self.config['num_scales'])
    return graph