（7-5-1）行为预测算法：Social LSTM轨迹预测算法

7.5  Social LSTM轨迹预测算法

Social LSTM（Long Short-Term Memory）是一种基于LSTM的神经网络模型，专门设计用于处理多智能体系统（multi-agent systems）中的轨迹预测问题。Social LSTM 扩展了传统的LSTM模型，以便更好地处理由多个移动智能体组成的系统的时空数据。在本节的项目中，利用Social-LSTM算法进行行人轨迹预测，目的是研究轨迹数据处理对行人轨迹预测准确性的影响。并且在项目中重写了Social-LSTM算法，使其能够处理标准化和非标准化的完整轨迹预测。

实例2-8：路上行人轨迹预测系统（codes/2/Social-LSTM-main1）

7.5.1  LSTM和Social LSTM关系

LSTM和Social LSTM关系总结如下所示。

1. 基于LSTM：Social LSTM是建立在LSTM模型基础之上的。LSTM是一种循环神经网络（Recurrent Neural Network，RNN）的变体，专门设计用于处理和学习时序数据。它在序列数据中具有较长的记忆，适用于捕捉时间依赖性。
2. 处理多智能体系统：Social LSTM 的主要目标是解决多智能体系统中的轨迹预测问题。在这种问题中，每个智能体代表一个移动的实体，例如人、车辆等。模型需要考虑多个智能体之间的相互影响，以更准确地预测它们的未来轨迹。
3. 考虑社交关系：Social LSTM 引入了社交池（social pool）的概念，用于捕捉智能体之间的相互关系。社交池允许模型在预测一个智能体的轨迹时，考虑其他智能体对其运动的影响。
4. 时空建模：Social LSTM 不仅考虑了时间上的依赖关系，还引入了对空间上相邻智能体的关注。这使得模型能够更好地理解多智能体系统中的运动模式。

总体而言，Social LSTM 是在LSTM基础上进行扩展，专门用于解决多智能体轨迹预测问题，并通过引入社交池等机制，更好地考虑了多个智能体之间的相互关系。

7.5.2  功能模块

本项目的关键组成模块如下所示。

（1）轨迹预测算法Social-LSTM：该算法是行人轨迹预测的核心算法，它利用长短时记忆（LSTM）网络，并考虑行人之间的社交互动来进行预测。

（2）数据处理

1. 标准化轨迹：项目考虑了标准化和非标准化的轨迹数据。标准化是轨迹预测中常用的一种技术，用于使数据具有一致性和规模。
2. 信号处理：由于轨迹数据涉及时间信息，项目使用信号处理技术进行数据处理。

（3）目录结构

与标准化轨迹预测相关的代码存储在“Normalized”目录中。

与非标准化轨迹预测相关的代码存储在“Non_Normalized”目录中。

（4）结果分析

利用ADE（平均位移误差）和FDE（最终位移误差），在不同数据集上的性能进行评估。结果表明，在标准化轨迹预测方面，Social-LSTM相比非标准化轨迹预测有更好的性能。

7.5.3  工具函数

编写文件utils_expert.py，实现了一组实用工具函数，用于处理轨迹数据的预处理和计算。其中包括进行Sinkhorn操作，旋转点集，将序列数据转换为节点表示，计算节点相对位置，计算序列之间的邻接关系，以及判断轨迹是否为线性或非线性。这些函数为轨迹数据处理提供了多样的工具，包括相对坐标计算、邻接矩阵生成等功能，为轨迹预测模型的研究提供了基础支持。文件utils_expert.py的具体实现流程如下所示。

（1）编写函数sinkhorn(log_alpha, n_iters=5)，功能是执行Sinkhorn迭代，计算输入log_alpha的softmax操作，提高计算效率。

MAX_NODE = 57  # 最大节点数

def sinkhorn(log_alpha, n_iters=5):
    # Sinkhorn迭代，用于计算softmax
    n = log_alpha.shape[1]
    log_alpha = log_alpha.view(-1, n, n)
    for i in range(n_iters):
        log_alpha = log_alpha - (torch.logsumexp(log_alpha, dim=2, keepdim=True)).view(
            -1, n, 1
        )
        log_alpha = log_alpha - (torch.logsumexp(log_alpha, dim=1, keepdim=True)).view(
            -1, 1, n
        )
    return torch.exp(log_alpha)

（2）编写函数rotate_pc(coords, alpha)，功能是将输入坐标按照给定的角度alpha进行逆时针旋转。

def rotate_pc(coords, alpha):
    # 旋转坐标
    alpha = alpha * np.pi / 180
    M = np.array([[np.cos(alpha), -np.sin(alpha)], [np.sin(alpha), np.cos(alpha)]])
    return M @ coords

（3）编写函数torch_seq_to_nodes(seq_)，功能是将PyTorch序列（表示节点坐标）转换为节点张量，用于后续图形处理。

def torch_seq_to_nodes(seq_):
    # 将PyTorch序列转换为节点
    seq_ = seq_.squeeze()
    batch = seq_.shape[0]
    seq_len = seq_.shape[1]
    num_ped = seq_.shape[2]

    V = torch.zeros((batch, seq_len, num_ped, 2), requires_grad=True).cuda()
    for s in range(seq_len):
        step_ = seq_[:, s, :, :]
        for h in range(num_ped):
            V[:, s, h, :] = step_[:, h]

    return V.squeeze()

（4）编写函数torch_nodes_rel_to_nodes_abs(nodes, init_node)，功能是将相对节点坐标转换为绝对节点坐标，考虑了初始节点的影响。

def torch_nodes_rel_to_nodes_abs(nodes, init_node):
    # 将相对节点转换为绝对节点
    nodes_ = torch.zeros_like(nodes, requires_grad=True).cuda()
    for s in range(nodes.shape[1]):
        for ped in range(nodes.shape[2]):
            nodes_[:, s, ped, :] = (
                torch.sum(nodes[:, : s + 1, ped, :], axis=1) + init_node[:, ped, :]
            )
    return nodes_.squeeze()

（5）编写函数anorm(p1, p2)，功能是计算两点之间的欧几里德距离的倒数，用于计算节点之间的权重。

def anorm(p1, p2):
    # 计算两点之间的欧几里德范数的倒数
    NORM = math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)
    if NORM == 0:
        return 0
    return 1 / (NORM)

（6）编写函数torch_anorm(p1, p2)，功能是实现PyTorch版本的欧几里德距离的倒数计算函数。

def torch_anorm(p1, p2):
    # 计算两点之间的欧几里德范数的倒数（PyTorch版本）
    NORM = torch.sqrt((p1[:, 0] - p2[:, 0]) ** 2 + (p1[:, 1] - p2[:, 1]) ** 2)
    rst = torch.where(NORM != 0.0, (1 / NORM).data, NORM.data)
    return rst

（7）编写函数seq_to_graph(seq_, seq_rel, norm_lap_matr=True, alloc=False)，功能是将输入序列转换为图形表示，包括节点和邻接矩阵。可以选择是否对邻接矩阵进行归一化和是否分配内存来存储邻接矩阵。

def seq_to_graph(seq_, seq_rel, norm_lap_matr=True, alloc=False):
    """
    将序列转换为图形
    Pytorch版本；
    对于此函数，输入为PyTorch张量：
        （seq_rel）的形状为[num_ped, 2, seq_len]
    """
    norm_lap_matr = False
    seq_ = seq_.squeeze()
    V = seq_rel.permute(2, 0, 1)
    seq_rel = seq_.clone()

    if len(seq_.shape) < 3:
        seq_ = seq_.unsqueeze(-1)
        seq_rel = seq_rel.unsqueeze(-1)

    seq_len = seq_.shape[2]
    max_nodes = seq_.shape[0]

    seq_rel = (
        seq_rel.permute(2, 0, 1).unsqueeze(-2).repeat(1, 1, max_nodes, 1)
    )

    trans_seq_rel = seq_rel.permute(0, 2, 1, 3)
    seq_rel_r = seq_rel - trans_seq_rel
    seq_rel_r = torch.sqrt(seq_rel_r[..., 0] ** 2 + seq_rel_r[..., 1] ** 2)

    seq_rel_r = torch.where(seq_rel_r != 0.0, (1 / seq_rel_r).data, seq_rel_r.data)

    if seq_rel.is_cuda:
        seq_rel_r = torch.where(
            seq_rel_r > 1.0,
            torch.ones(1).cuda(),
            seq_rel_r,
        )
    else:
        seq_rel_r = torch.where(
            seq_rel_r > 1.0,
            torch.ones(1),
            seq_rel_r,
        )

    seq_rel_r[:, :] = seq_rel_r[:, :] + torch.eye(max_nodes).cuda()
    A = seq_rel_r

    if norm_lap_matr:
        A_sumed = torch.sum(A, axis=1).unsqueeze(-1)
        diag_ones_tensor = torch.eye(max_nodes).unsqueeze(0).repeat(seq_len, 1, 1)
        D = diag_ones_tensor * A_sumed
        DH = torch.sqrt(D)
        DH = torch.where(DH != 0, 1.0 / DH, DH)
        L = D - A
        A = torch.bmm(DH, torch.bmm(L, DH))

    if alloc:
        A_alloc = adj_rel_shift(seq_rel_r.cpu().numpy())
        A_alloc = torch.from_numpy(A_alloc)

        if norm_lap_matr:
            A_sumed = torch.sum(A_alloc, axis=1).unsqueeze(-1)
            diag_ones_tensor = torch.eye(max_nodes).unsqueeze(0).repeat(seq_len, 1, 1)
            D = diag_ones_tensor * A_sumed
            DH = 1.0 / torch.sqrt(D)
            DH[torch.isinf(DH)] = 0.0
            L = D - A_alloc
            A_alloc = torch.bmm(DH, torch.bmm(L, DH))

        return V, A, A_alloc
    return V, A, A

（8）编写函数adj_rel_shift(A)，功能是调整相对邻接矩阵的边，使其相对于最近的邻居发生偏移。它通过找到每个节点的最近邻居，并将其他邻居的边替换为最近邻居的边来实现。

def adj_rel_shift(A):
    """
    将邻接矩阵A进行偏移，使每个节点的邻接边相对于最近的邻居发生变化。

    参数：
    - A：邻接矩阵，形状为[seq_len, num_ped, num_ped]

    注意：ped可能会被填充（padded），在预处理中处理。

    更新：使用numpy而非pytorch实现。
    """
    seq_len, num_ped = A.shape[:2]

    indices = np.argmin(A, axis=-1)
    min_values = np.min(A, axis=-1)
    indices = np.expand_dims(indices, -1)
    min_values = np.expand_dims(min_values, -1)
    out = np.take_along_axis(A, indices, 1)

    idenity = np.eye(num_ped)
    ivt_idenity = np.where(idenity != 0, 0, 1)
    out = out * ivt_idenity
    np.put_along_axis(out, indices, min_values, axis=-1)

    return out

（9）编写函数poly_fit(traj, traj_len, threshold)，功能是判断轨迹是否为非线性。通过拟合轨迹的二次多项式并计算残差，如果残差超过阈值，则将轨迹标记为非线性。

def poly_fit(traj, traj_len, threshold):
    """
    输入：
    - traj：形状为(2, traj_len)的NumPy数组
    - traj_len：轨迹长度
    - threshold：被认为是非线性轨迹的最小残差

    输出：
    - int：1表示非线性，0表示线性
    """
    t = np.linspace(0, traj_len - 1, traj_len)
    res_x = np.polyfit(t, traj[0, -traj_len:], 2, full=True)[1]
    res_y = np.polyfit(t, traj[1, -traj_len:], 2, full=True)[1]
    if res_x + res_y >= threshold:
        return 1.0
    else:
        return 0.0

（10）编写函数read_file(_path, delim="\t")，功能是从给定路径读取文件，将文件中的数据转换为NumPy数组并返回。

def read_file(_path, delim="\t"):
    """
    从文件中读取数据，并返回包含文件数据的NumPy数组。

    参数：
    - _path：文件路径
    - delim：文件中的分隔符，默认为制表符

    返回：
    - NumPy数组
    """
    data = []
    if delim == "tab":
        delim = "\t"
    elif delim == "space":
        delim = " "
    with open(_path, "r") as f:
        for line in f:
            line = line.strip().split(delim)
            line = [float(i) for i in line]
            data.append(line)
    return np.asarray(data)

（11）编写类TrajectoryDataset，实现一个数据集加载器，用于处理轨迹数据集。它将轨迹数据文件读取为NumPy数组，并进行数据增强（旋转、缩放和放大）。加载后提供了一些有关轨迹序列的信息，包括位置、速度、起始点、终点等。

class TrajectoryDataset(Dataset):
    """
    轨迹数据集的数据加载器。
    """

    def __init__(
        self,
        data_dir,
        obs_len=8,
        pred_len=8,
        skip=1,
        threshold=0.002,
        min_ped=1,
        delim="\t",
        norm_lap_matr=True,
        alloc=False,
        angles=[0],
        grad_eff=0.4,
    ):
        """
        Args:
        - data_dir: 包含数据集文件的目录路径
        - obs_len: 输入轨迹的时间步数
        - pred_len: 输出轨迹的时间步数
        - skip: 创建数据集时跳过的帧数
        - threshold: 用于线性预测时被认为是非线性轨迹的最小残差
        - min_ped: 序列中应包含的最小行人数
        - delim: 数据集文件中的分隔符
        """
        super(TrajectoryDataset, self).__init__()

        global MAX_NODE

        self.max_peds_in_frame = 0
        self.data_dir = data_dir
        self.obs_len = obs_len
        self.pred_len = pred_len
        self.skip = skip
        judge_test_train_index = data_dir[:-1].rfind('/')
        judge_test_train = data_dir[judge_test_train_index+1:]
        if judge_test_train == 'test/':
            self.seq_len = self.obs_len + self.pred_len
        else:
            self.seq_len = self.obs_len + self.pred_len + 1
        self.delim = delim
        self.norm_lap_matr = norm_lap_matr
        self.alloc = alloc

        all_files = os.listdir(self.data_dir)
        all_files = [
            os.path.join(self.data_dir, _path)
            for _path in all_files
            if _path.endswith("txt")
        ]
        num_peds_in_seq = []
        seq_list = []
        seq_list_ori = []
        seq_list_rel = []
        seq_list_v = []
        seq_list_start = []
        seq_list_end = []
        seq_ped_index = []

        loss_mask_list = []
        non_linear_ped = []

        angles = [0]

        data_scale = 1.0

        for path in all_files:
            print(f'The processing file is {path}')
            data_ori = read_file(path, delim)
            for angle in angles:
                data = np.copy(data_ori) * data_scale
                data[:, -2:] = rotate_pc(data[:, -2:].transpose(), angle).transpose()

                for amp in amplify:
                    if "test" not in self.data_dir:
                        data[:, -2:] *= np.array((amp, amp))

                    frames = np.unique(data[:, 0]).tolist()
                    frame_data = []

                    for frame in frames:
                        frame_data.append(data[frame == data[:, 0], :])
                    num_sequences = int(
                        math.ceil((len(frames) - self.seq_len + 1) / skip)
                    )

                    for idx in range(0, num_sequences * self.skip + 1, skip):
                        curr_seq_data = np.concatenate(
                            frame_data[idx : idx + self.seq_len], axis=0
                        )
                        peds_in_curr_seq = np.unique(curr_seq_data[:, 1])
                        self.max_peds_in_frame = max(
                            self.max_peds_in_frame, len(peds_in_curr_seq)
                        )

                        curr_seq_rel = np.zeros(
                            (len(peds_in_curr_seq), 2, self.seq_len)
                        )
                        curr_seq = np.zeros((len(peds_in_curr_seq), 2, self.seq_len))
                        curr_ori_seq = np.zeros(
                            (len(peds_in_curr_seq), 3, self.seq_len)
                        )
                        curr_seq_v = np.zeros(
                            (len(peds_in_curr_seq), 2, self.seq_len)
                        )
                        curr_loss_mask = np.zeros(
                            (len(peds_in_curr_seq), self.seq_len)
                        )
                        curr_seq_start = np.zeros((len(peds_in_curr_seq), 2))
                        curr_seq_end = np.zeros((len(peds_in_curr_seq), 2))
                        num_peds_considered = 0
                        _non_linear_ped = []
                        curr_peds_index = []
                        for _, ped_id in enumerate(peds_in_curr_seq):
                            curr_ped_seq = curr_seq_data[
                                curr_seq_data[:, 1] == ped_id, :
                            ]
                            curr_ped_seq = np.around(curr_ped_seq, decimals=4)
                            pad_front = frames.index(curr_ped_seq[0, 0]) - idx
                            pad_end = frames.index(curr_ped_seq[-1, 0]) - idx + 1
                            if pad_end - pad_front != self.seq_len:
                                continue
                            curr_ped_seq_ori = np.transpose(
                                np.copy(curr_ped_seq[:, 1:])
                            )
                            curr_ped_seq = np.transpose(curr_ped_seq[:, 2:])

                            seq_start = np.array(
                                (curr_ped_seq[0, 0], curr_ped_seq[1, 0])
                            )
                            seq_end = np.array(
                                (curr_ped_seq[0, -1], curr_ped_seq[1, -1])
                            )

                            curr_ped_seq[0, :] = curr_ped_seq[0, :] - curr_ped_seq[0, 0]
                            curr_ped_seq[1, :] = curr_ped_seq[1, :] - curr_ped_seq[1, 0]

                            rel_curr_ped_seq = np.zeros(curr_ped_seq.shape)
                            rel_curr_ped_seq[:, 1:] = (
                                curr_ped_seq[:, 1:] - curr_ped_seq[:, :-1]
                            )

                            v_curr_ped_seq = np.gradient(
                                np.array(curr_ped_seq),
                                grad_eff,
                                axis=1
                            )

                            _idx = num_peds_considered
                            curr_seq_start[_idx, :] = seq_start
                            curr_seq_end[_idx, :] = seq_end
                            curr_ori_seq[_idx, :, pad_front:pad_end] = curr_ped_seq_ori
                            curr_seq[_idx, :, pad_front:pad_end] = curr_ped_seq
                            curr_seq_rel[_idx, :, pad_front:pad_end] = rel_curr_ped_seq
                            curr_seq_v[_idx, :, pad_front:pad_end] = v_curr_ped_seq

                            _non_linear_ped.append(
                                poly_fit(curr_ped_seq, pred_len, threshold)
                            )
                            curr_loss_mask[_idx, pad_front:pad_end] = 1
                            curr_peds_index.append(_idx)
                            num_peds_considered += 1

                        min_ped = 0
                        max_ped = 1000
                        flip = False
                        if (
                            num_peds_considered > min_ped
                            and num_peds_considered <= max_ped
                        ):
                            non_linear_ped.append(_non_linear_ped)
                            num_peds_in_seq.append(num_peds_considered)
                            loss_mask_list.append(curr_loss_mask[:num_peds_considered])
                            seq_list.append(curr_seq[:num_peds_considered])
                            seq_list_ori.append(curr_ori_seq[:num_peds_considered])
                            seq_list_rel.append(curr_seq_rel[:num_peds_considered])
                            seq_list_v.append(curr_seq_v[:num_peds_considered])
                            seq_list_start.append(curr_seq_start[:num_peds_considered])
                            seq_list_end.append(curr_seq_end[:num_peds_considered])

                            a = [curr_peds_index for _ in range(self.seq_len)]
                            seq_ped_index.append(a)

                            if flip and "train" in self.data_dir:
                                non_linear_ped.append(_non_linear_ped)
                                num_peds_in_seq.append(num_peds_considered)
                                loss_mask_list.append(
                                    curr_loss_mask[:num_peds_considered]
                                )
                                seq_list.append(
                                    np.flip(curr_seq[:num_peds_considered], 2)
                                )
                                seq_list_ori.append(np.flip(curr_ori_seq[:num_peds_considered], 2))
                                seq_list_rel.append(
                                    np.flip(curr_seq_rel[:num_peds_considered], 2)
                                )
                                seq_list_v.append(
                                    np.flip(curr_seq_v[:num_peds_considered], 2)
                                )

        self.num_seq = len(seq_list)
        self.sequence = seq_list
        self.ori_sequence = seq_list_ori
        self.traj_velocity = seq_list_v
        self.traj_start = seq_list_start
        self.traj_end = seq_list_end
        self.loss_mask = loss_mask_list
        self.non_linear_ped = non_linear_ped
        self.seq_ped_index = seq_ped_index

（12）编写函数__len__(self)，功能是返回数据集中的轨迹序列数量。这个函数是Python内置的特殊方法，用于获取数据集的长度。

    def __len__(self):

        num_seq = self.num_seq

        return num_seq

（13）编写函数__getitem__(self, index)，功能是获取数据集中特定索引处的元素。在这个上下文中，返回数据集中给定索引的轨迹序列以及相关信息，如速度、起始点、终点等。这个函数也是Python内置的特殊方法，允许使用索引访问数据集的元素。

    def __getitem__(self, index):
        out = [
            self.sequence[index],
            self.traj_velocity[index],
            self.traj_start[index],
            self.traj_end[index],
            self.loss_mask[index],
            self.non_linear_ped[index],
        ]
        return out

未完待续