读取CSV文件数据、编辑完再存入CSV

坐标转换工作中经常需要处理各类数据，而输入的信息中，有很多形式，ROSbag、txt和CSV等等。不同于ROSbag，CAN信号等数据流。CSV需要先提取一下。这里记录一下，后面好查。

这类数据处理，python好用。先上：

# 用于坐标数据转化
import numpy as np
import pandas as pd
import EulerAndQuaternionTransform
import rpy2quat

# 读取本地csv文件，并确认其是否有表头
df = pd.read_csv("map_trajectory_kitti_type.csv", header=None)
# 读取的df为结构体DataFrame (x, y)
# print(df)

# 将结构体转化为数组形式
df_array = np.array(df)
# print(df_array)
# 也可以根据需要转化为list格式
# df_list = df_array.tolist()
# array_len = len(df_array)
# for i in range(len(df_array)):
#     print(df_array[i, 4])

#将数组中需要处理的部分数据先删除
df_array1 = np.delete(df_array, [4, 5, 6], axis=1)
# print(df_array1)

#初始化一个空的数组，准备储存处理后的数据
df_array2 = np.empty(shape=(0, 4))
# df_array2 = np.array([0, 1, 2, 3])

# 遍历df_array中所有需要处理的数据
for i in range(len(df_array)):
    # 将所需处理的数据转化为功能函数需要的参数格式
    input_data = np.array([df_array[i, 4], df_array[i, 5], df_array[i, 6]])

    # 手写计算函数
    # four_data = EulerAndQuaternionTransform.EulerAndQuaternionTransform(input_data)

    # 调用script中的转化函数
    four_data = rpy2quat.rpy2quat(input_data)

    # 将处理后的数据转化为数组格式
    df_fourfacts = np.array(four_data)
    # print(df_fourfacts)

    # 将每行处理好的数据储存到全局变量中去
    df_array2 = np.row_stack((df_array2, df_fourfacts))

# print(df_array2)

# 将未处理部分的数据和已处理部分的数据做合并
df_array3 = np.column_stack((df_array1, df_array2))

# 将处理后的数据储存到txt文件中去
np.savetxt("trajectory_tum.txt", df_array3, fmt='%f', delimiter=' ')

这里把两个功能函数也记一下。

import numpy as np
from scipy.spatial.transform import Rotation as R

def rpy2quat(input_data):
    MyQuat = R.from_euler('zyx', [input_data[2], input_data[1], input_data[0]]).as_quat()
    # print(MyQuat)
    w = MyQuat[0]
    x = MyQuat[1]
    y = MyQuat[2]
    z = MyQuat[3]

return [w, x, y, z]

另一个

import math



def EulerAndQuaternionTransform(intput_data):
    """
        四元素与欧拉角互换
    """
    data_len = len(intput_data)
    angle_is_not_rad = False

    if data_len == 3:
        r = 0
        p = 0
        y = 0
        if angle_is_not_rad:  # 180 ->pi
            r = math.radians(intput_data[0])
            p = math.radians(intput_data[1])
            y = math.radians(intput_data[2])
        else:
            r = intput_data[0]
            p = intput_data[1]
            y = intput_data[2]

        sinp = math.sin(p / 2)
        siny = math.sin(y / 2)
        sinr = math.sin(r / 2)

        cosp = math.cos(p / 2)
        cosy = math.cos(y / 2)
        cosr = math.cos(r / 2)

        w = cosr * cosp * cosy + sinr * sinp * siny
        x = sinr * cosp * cosy - cosr * sinp * siny
        y = cosr * sinp * cosy + sinr * cosp * siny
        z = cosr * cosp * siny - sinr * sinp * cosy
        return [x, y, z, w]

    elif data_len == 4:

        w = intput_data[0]
        x = intput_data[1]
        y = intput_data[2]
        z = intput_data[3]

        r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
        p = math.asin(2 * (w * y - z * x))
        y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))

        if angle_is_not_rad:  # pi -> 180
            r = math.degrees(r)
            p = math.degrees(p)
            y = math.degrees(y)
        return [r, p, y]

如果用C++的话，相对复杂一些，需要用数组一个个赋值再记录（确实很水，这里只是记录一下哈）

#include <iostream>
#include <fstream>
#include <math.h> 
#include <iomanip> 
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "vector"

using namespace std;

vector<Eigen::Quaterniond> read_gt_file(string filename)
{   
    ifstream inFile(filename, ios::in);   
    if(inFile.fail()){
        cout <<"Read files fail....."<<endl;
        Eigen::Quaterniond::Identity();
    }
    
    vector<Eigen::Quaterniond> gt_poses;
    string lineStr;
    while (getline(inFile, lineStr)) 
    {
        Eigen::Quaterniond gt_pose = Eigen::Quaterniond::Identity();

        vector<double> lineArray;
        stringstream ss(lineStr);
        string str;
        char delim = ',';
        
        while (getline(ss, str, delim))
            lineArray.push_back(std::stod(str));

        Eigen::AngleAxisd rollAngle(lineArray[4], Eigen::Vector3d::UnitX());
        Eigen::AngleAxisd pitchAngle(lineArray[5], Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd yawAngle(lineArray[6], Eigen::Vector3d::UnitZ());

        gt_pose = yawAngle * pitchAngle * rollAngle;
        gt_poses.push_back(gt_pose);
    }    
    inFile.close();
    return gt_poses;
}

vector<vector<double>> read_gt_file_full(string filename){
    ifstream inFile(filename, ios::in);
    if (inFile.fail()){
        cout <<"Write files fail....."<<endl;
    }

    vector<vector<double>> gt_double_full;
    string lineStr;
    while (getline(inFile, lineStr)){
        vector<double> gt_double_line;
        stringstream ss(lineStr);
        string str;
        char delim = ' ';
        while (getline(ss, str, delim))
            gt_double_line.push_back(std::stod(str));
        
        gt_double_full.push_back(gt_double_line);
    }
    inFile.close();
    return gt_double_full;
}

int main()
{
    string filename = "/home/kelven/testdata/0811_bag/map_trajectory_kitti_type.csv";
    vector<Eigen::Quaterniond> poses = read_gt_file(filename);
    vector<vector<double>> front_data = read_gt_file_full(filename);
    
    string outfilename = "/home/kelven/testdata/0811_bag/map_trajectory_out_tum_type.txt";
    ofstream outfile(outfilename, ios::out);
    if (outfile.fail()){
        cout <<"Write files fail....."<<endl;
    }
    
    int n = poses.size();
    double a[n][7];
    for (int i = 0; i < n; i++){
        a[i][0] = front_data[i][0];
        a[i][1] = front_data[i][1];
        a[i][2] = front_data[i][2];
        a[i][3] = front_data[i][3];
        a[i][4] = poses[i].x();
        a[i][5] = poses[i].y();
        a[i][6] = poses[i].z();
        a[i][7] = poses[i].w();
        outfile << setprecision(17) << a[i][0] << " " << a[i][1] << " " << a[i][2] << " " 
        << a[i][3] << " " << a[i][4] << " " << a[i][5] << " " << a[i][6] << " " << a[i][7]<< endl;   
    }
    outfile.close();

    return 0;
}

以上代码mark一下，后面应该还会用到。