kalman_tests.py 文件源码

def process_lidar_csv_file(filename):
    with open(filename) as csvfile:
        reader = csv.DictReader(csvfile)

        csv_rows = [row for row in reader]
        print "%s lidar records" % len(csv_rows)

        n_limit_rows = 1000000

        lidar_obss = []
        bbox_obss = [[],[],[]]

        for i, row in enumerate(csv_rows):
            if i > n_limit_rows - 1:
                break

            time_ns = int(row['time'])
            x, y, z, yaw = float(row['x']), float(row['y']), float(row['z']), float(row['yaw'])
            l, w, h = float(row['l']), float(row['w']), float(row['h'])

            obs = LidarObservation(time_ns * 1e-9, x, y, z, yaw)
            lidar_obss.append(obs)

            bbox_obss[0].append(l)
            bbox_obss[1].append(w)
            bbox_obss[2].append(h)

        yaw = [np.rad2deg(o.yaw) for o in lidar_obss]
        print np.std(yaw)
        #plt.figure(figsize=(16,8))
        #plt.plot(yaw)
        #plt.grid(True)

        return lidar_obss, bbox_obss