python_文件读写_提取kitti数据集中带时间戳的IMU测量数据

叁歲伎倆 2023-01-13 04:13 234阅读 0赞

# 功能说明: #

根据时间戳文件timestamps.txt, 每一个时间戳下对应的一个GPS/IMU测量数据, 提取出IMU的陀螺仪三个轴的测量值(wx,wy,wz), 和加速度计的三个测量值(ax,ay,z), 生成指定格式的kitti\_imu\_data.csv文件, 为VIO提供有时间戳的IMU测量数据

## 输入: ##

*  时间戳文件1个(45700条记录) timestamps.txt
 *  IMU测量数据45700个(每个文件1条记录) 0000000000.txt-0000045699.txt

## 输出: ##

*  kitti\_imu\_data.csv文件

### 格式: ###

# example: #timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]
    1403636579758555392,-0.099134701513277898,0.14730578886832138,0.02722713633111154,8.1476917083333333,-0.37592158333333331,-2.4026292499999999
    1403636579763555584,-0.099134701513277898,0.14032447186034408,0.029321531433504733,8.033280791666666,-0.40861041666666664,-2.4026292499999999
    ...

# 代码实现: #

# imu data source:
    #               oxts_raw/data/0000000000.txt-0000045699.txt
    #               dataformat.txt
    #               timestamps.txt
    
    #   # example: #timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]
    #   1403636579758555392,-0.099134701513277898,0.14730578886832138,0.02722713633111154,8.1476917083333333,-0.37592158333333331,-2.4026292499999999
    #   1403636579763555584,-0.099134701513277898,0.14032447186034408,0.029321531433504733,8.033280791666666,-0.40861041666666664,-2.4026292499999999
    #   ...
    
    # 步骤:
    #   0. 定义文件,写入文件列说明 #timestamp [ns],w_RS_S_x [rad s^-1],w ...
    #   1. 从 timestamp.txt读取一行, 转换成时间戳整数形式
    #   2. 读取 000000000_i.txt 文件 选择需要的 6个参数, 组合成字符串, 写入文件一行
    #   3. 以此类推, 循环次数[0,45700)
    #   4. 保存内容, 关闭文件
    
    import os
    import time
    from datetime import datetime
    
    MAXNUMBER = 45700
    
    def combineKittiImudata(targerFile, timestampsFile, sourcePath):
        imu_target_handle = open(targerFile, 'w')
        timestamps_handle = open(timestampsFile, 'r')
        headline = "#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\n"
        imu_target_handle.writelines(headline)
    
        for i in range(0, MAXNUMBER):
            # timestamp = "2011-10-03 12:55:36.407975484" #484
            timestamp = timestamps_handle.readline().strip(
                '\n')[0:-3]  # no nanosecond available
            # print(timestamp)
            datetime_obj = datetime.strptime(timestamp, "%Y-%m-%d %H:%M:%S.%f")
            timeStamp19 = 1000000 * \
                int(time.mktime(datetime_obj.timetuple()) *
                    1000.0 + datetime_obj.microsecond / 1000.0)
            # print (timeStamp19)  # such as 1317617736407000000
            
            index = '%010d' % i
            sourceFileName = sourcePath + index + ".txt"
            sourceFile = open(sourceFileName, 'r')
            sourceData =sourceFile.readline().strip('\n')
            sourceFile.close()
            
            sourceArray = sourceData.split(" ")
            ax = sourceArray[11]
            ay = sourceArray[12]
            az = sourceArray[13]
            wx = sourceArray[17]
            wy = sourceArray[18]
            wz = sourceArray[19]
            
            item = str(timeStamp19)+","+wx+","+wy+","+wz+","+ax+","+ay+","+az+"\n"
            print(item)
            
            imu_target_handle.writelines(item)
        imu_target_handle.close()
        timestamps_handle.close()
        pass
    
    
    if __name__ == "__main__":
        targerFile = "./kitti_imu_data.csv"
        timestampsFile = "./oxts_raw/timestamps.txt"
        sourcePath = "./oxts_raw/data/"
    
        combineKittiImudata(targerFile, timestampsFile, sourcePath)