求出Pose(4*4)矩阵的 t 的平均值，并且减掉 t的均值

Motivation

Kitti360 中的cam2world.txt 中的 pose矩阵是一个4*4的矩阵，其中的t直接读取是一个非常大的数值，因此需要对t进行归一化

可以看到直接加载的 t = [846,3724,115] 很大，需要对所有的t 进行归一化 (求出t均值，再减掉均值)。

假设一共33张图象，pose 维度是（33，4，4），先for循环求出avg_t，维度是(3,1)。如何在一个pose(33,4,4)的ndarray 中减去一个（3，1）的向量？

思路

1. 先构造一个 ndarray 全零矩阵 t(33,4,4)，然后将该全零矩阵的前三行，第四列赋值为 avg_t,其余元素依然保持为0.

2. 这样的t 的维度是(33,4,4),poses 的维度也是（33，4，4），这样子可以直接进行减法运算

def Normailize_T(poses):
    num_frames = len(poses)
    t = np.zeros((1,3))
    for pose in poses:
        t += pose[:3,3]
    avg_t = t/num_frames
    ## 每个pose 的 前三行，第四列subtract avg_t
    t = np.zeros(poses.shape)
    t[:,:3,3] = avg_t
    poses = poses - t
    return poses

反思

这种思路也适用于 其他的numpy 数组，维度不相同，想做加减法的情况。