python画二维图图

import codecs
import re
import numpy as np
import matplotlib.pyplot as plt
class Compare:
    def __init__(self, str_demo):
        self.str_demo = str_demo
    def transfer(self, vec):
        vec_match_list = [i.split(",") for i in vec.findall(self.str_demo)]
        for i in range(len(vec_match_list)):
            for j in range(len(vec_match_list[0])):
                vec_match_list[i][j] = float(vec_match_list[i][j])
        return vec_match_list
def run(method):
    with codecs.open('pose1.txt', encoding="iso-8859-15") as file:
        str_demo = file.read()
    odom_vec = re.compile(r"info odom:(.*?),pose:")
    pose_vec = re.compile(r",pose:(.*?),c_delta:")
    compare_demo = Compare(str_demo)
    odom_vec_match_list = compare_demo.transfer(odom_vec)
    pose_vec_match_list = compare_demo.transfer(pose_vec)
    odom_first = odom_vec_match_list[0]
    pose_first = pose_vec_match_list[0]
    TF_odom_map = []
    if method == 1:
        # 方法一
        d_th = pose_first[2] - odom_first[2]
        d_x = -1 * odom_first[0] * np.cos(d_th) + odom_first[1] * np.sin(d_th) + pose_first[0]
        d_y = -1 * odom_first[1] * np.cos(d_th) - odom_first[0] * np.sin(d_th) + pose_first[1]
        TF_odom_map = np.array([[np.cos(d_th), -np.sin(d_th), d_x],
                                [np.sin(d_th), np.cos(d_th), d_y],
                                [0, 0, 1]])
    elif method == 2:
        # 方法2
        TF_base_map = np.array([[np.cos(pose_first[2]), -np.sin(pose_first[2]), pose_first[0]],
                                [np.sin(pose_first[2]), np.cos(pose_first[2]), pose_first[1]],
                                [0, 0, 1]])
        TF_base_odom = np.array([[np.cos(odom_first[2]), -np.sin(odom_first[2]), odom_first[0]],
                                 [np.sin(odom_first[2]), np.cos(odom_first[2]), odom_first[1]],
                                 [0, 0, 1]])
        TF_odom_map = np.dot(TF_base_map, np.linalg.inv(TF_base_odom))
    odom_x = []
    odom_y = []
    pose_x = []
    pose_y = []
    pose_x_by_odom = []
    pose_y_by_odom = []
    plt.ion() #开启interactive mode 成功的关键函数
    
    for i in range(len(odom_vec_match_list)):
        odom_ele = odom_vec_match_list[i]
        pose_ele = pose_vec_match_list[i]
        odom_i = np.array([[odom_ele[0]],
                           [odom_ele[1]],
                           [odom_ele[2]]])
        pose_i = np.array([[pose_ele[0]],
                           [pose_ele[1]],
                           [pose_ele[2]]])
        odom_i[-1][0] = 1  # 转成齐次坐标
        pose_i_by_odom = np.dot(TF_odom_map, odom_i)
        odom_x.append(odom_i[0][0])
        odom_y.append(odom_i[1][0])
        pose_x.append(pose_i[0][0])
        pose_y.append(pose_i[1][0])
        pose_x_by_odom.append(pose_i_by_odom[0][0])
        pose_y_by_odom.append(pose_i_by_odom[1][0])
        plt.xlabel('x-value')
        plt.ylabel('y-label')
        plt.axis('equal')
        plt.plot(pose_x, pose_y,'-b')
        plt.plot(pose_x_by_odom, pose_y_by_odom,'-g')
        plt.draw()#注意此函数需要调用
        plt.pause(0.01)
        if(i == 0):
            plt.pause(0.01)
    plt.pause(100)
if __name__ == '__main__':
    run(2)