smoothCameraPoses.py 文件源码

def main():

    filename = 'posegraph.posegraph'
    plot = False
    windowLength = 11
    windowType = 'hanning'

    if not os.path.isfile(filename):
        print 'could not find posegraph file:', filename
        sys.exit(1)

    data = np.loadtxt(filename)
    poseTimes = data[:,0]
    poses = np.array(data[:,1:])
    poses = np.array([to_xyzrpy(pose) for pose in poses])
    poses[:,3:] = np.unwrap(poses[:,3:])

    for i in range(poses.shape[1]):
        poses[:,i] = smooth(poses[:,i], window_len=windowLength, window=windowType)

    if plot:
        plot(poses, poseTimes)

    poses = np.array([to_xyzquat(pose) for pose in poses])
    poses = np.hstack([np.reshape(poseTimes, (-1,1)), poses])
    np.savetxt('posegraph_smoothed.posegraph', poses)