kitti.py 文件源码

def plot_ground_truth(self):
        """Plot ground truth"""
        # Home point
        lat_ref = self.oxts[0]['lat']
        lon_ref = self.oxts[0]['lon']
        alt_ref = self.oxts[0]['alt']

        # Calculate position relative to home point
        ground_truth_x = [0.0]
        ground_truth_y = [0.0]
        ground_truth_z = [0.0]

        for i in range(1, len(self.oxts)):
            lat = self.oxts[i]['lat']
            lon = self.oxts[i]['lon']
            alt = self.oxts[i]['alt']

            dist_N, dist_E = latlon_diff(lat_ref, lon_ref, lat, lon)
            height = alt - alt_ref

            ground_truth_x.append(dist_E)
            ground_truth_y.append(dist_N)
            ground_truth_z.append(height)

        # Plot
        fig = plt.figure()
        plt.suptitle("Ground Truth")
        ax1 = fig.add_subplot(211)
        ax2 = fig.add_subplot(212)

        ax1.plot(ground_truth_x, ground_truth_y)
        ax1.axis('equal')
        ax1.set_xlabel("East (m)")
        ax1.set_ylabel("North (m)")

        ax2.plot(self.timestamps, ground_truth_z)
        ax2.set_xlabel("Date Time")
        ax2.set_ylabel("Height (m)")

        fig.tight_layout()