dataset.py 文件源码

def plot(self, track, track_cam_states, estimates):
        plt.figure()

        # Feature
        feature = T_global_camera * track.ground_truth
        plt.plot(feature[0], feature[1],
                 marker="o", color="red", label="feature")

        # Camera states
        for cam_state in track_cam_states:
            pos = T_global_camera * cam_state.p_G
            plt.plot(pos[0], pos[1],
                     marker="o", color="blue", label="camera")

        # Estimates
        for i in range(len(estimates)):
            cam_state = track_cam_states[i]
            cam_pos = T_global_camera * cam_state.p_G
            estimate = (T_global_camera * estimates[i]) + cam_pos
            plt.plot(estimate[0], estimate[1],
                     marker="o", color="green")

        plt.legend(loc=0)
        plt.show()