test_msckf.py 文件源码

def plot_position(self, pos_true, pos_est, cam_states):
        N = pos_est.shape[1]
        pos_true = pos_true[:, :N]
        pos_est = pos_est[:, :N]

        # Figure
        plt.figure()
        plt.suptitle("Position")

        # Ground truth
        plt.plot(pos_true[0, :], pos_true[1, :],
                 color="red", label="Grouth truth")
                 # color="red", marker="x", label="Grouth truth")

        # Estimated
        plt.plot(pos_est[0, :], pos_est[1, :],
                 color="blue", label="Estimated")
                 # color="blue", marker="o", label="Estimated")

        # Sliding window
        cam_pos = []
        for cam_state in cam_states:
            cam_pos.append(cam_state.p_G)
        cam_pos = np.array(cam_pos).reshape((len(cam_pos), 3)).T
        plt.plot(cam_pos[0, :], cam_pos[1, :],
                 color="green", label="Camera Poses")
                 # color="green", marker="o", label="Camera Poses")

        # Plot labels and legends
        plt.xlabel("East (m)")
        plt.ylabel("North (m)")
        plt.axis("equal")
        plt.legend(loc=0)