c++ (2-0) 從txt讀取和儲存資料

MKT-porter發表於2024-08-17

CMakeLists.txt

# 設定 CMake 的最小版本要求
cmake_minimum_required(VERSION 3.10)

# 設定專案名稱和版本
project(PoseSaver VERSION 1.0)

# 設定 C++ 標準為 C++11
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)


# 查詢 Eigen 庫
find_package(Eigen3 3.3 REQUIRED NO_MODULE)

# 新增可執行檔案
add_executable(PoseSaver main.cpp)

# 連結 Eigen3 庫
target_link_libraries(PoseSaver Eigen3::Eigen)

# 可選：設定編譯器選項（根據需要）
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")

# 可選：設定輸出目錄
# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)

　　main.cpp

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <iomanip> // For std::setprecision
#include <Eigen/Dense>
#include <map>         // 包含 map 標頭檔案

using namespace std;
using namespace Eigen;

/*
函式功能:
讀取 時間戳和Eigen::Vector3d位置t(沒有旋轉R)資訊到filename的txt中
輸入：
檔名 filename
列表 map<double, Vector3d> &dataMap
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Vector3d>& poses

輸出：
1.000000000 14.000000000 15.000000000 16.000000000 
2.000000000 24.000000000 25.000000000 26.000000000 
3.000000000 34.000000000 35.000000000 36.000000000 
*/
void Read_poses_t3x3FromFile(const string& filename,map<double, Vector3d> &dataMap) {
    
    ifstream file(filename);
    if (!file.is_open()) {
        cout << "Error opening file: " << filename << endl;
        
    }

    std::string line;
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string token;
        std::vector<std::string> row;
        
        /*
        1.000000000 14.000000000 15.000000000 16.000000000 
        2.000000000 24.000000000 25.000000000 26.000000000 
        3.000000000 34.000000000 35.000000000 36.000000000 
        */

        while (std::getline(ss, token, ' ')) { // 空格分割字串
            row.push_back(token);
        }
     
        double timestamp=stod(row[0]);
        double x = stod(row[1]);
        double y = stod(row[2]);
        double z = stod(row[3]);
      
        Vector3d position(x, y, z);
        dataMap[timestamp] = position;
     
    }

    // string line;
    // while (getline(file, line)) {
    //     stringstream ss(line);
    //     double timestamp;
    //     double x, y, z;

    //     if (!(ss >> timestamp >> x >> y >> z)) {
    //         cerr << "Error reading line: " << line << endl;
    //         continue;
    //     }

    //     Vector3d position(x, y, z);
    //     dataMap[timestamp] = position;
    // }

    file.close();
   
}

/*
函式功能:
讀取 時間戳和Eigen::Vector3d位置t + 旋轉 資訊到filename的txt中
輸入：
檔名 filename
列表 map<double, Matrix4d> &dataMap
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Matrix4d>& poses

輸出：
1.000000000000000 11.123456789 0.000000001 0.000000000 14.000000000 0.000000000 12.987654321 0.000000000 15.000000000 0.000000000 0.000000000 13.234567890 16.000000000 0.000000000 0.000000000 0.000000000 1.000000000
2.000000000000000 21.111111111 0.000000000 0.000000000 24.000000000 0.000000000 22.222222222 0.000000000 25.000000000 0.000000000 0.000000000 23.333333333 26.000000000 0.000000000 0.000000000 0.000000000 1.000000000
3.000000000000000 31.444444444 0.000000000 0.000000000 34.000000000 0.000000000 32.555555555 0.000000000 35.000000000 0.000000000 0.000000000 33.666666666 36.000000000 0.000000000 0.000000000 0.000000000 1.000000000
*/
void Read_poses_T4x4FromFile(const string& filename,map<double, Eigen::Matrix4d> &dataMap) {
    
    ifstream file(filename);
    if (!file.is_open()) {
        cerr << "Error opening file: " << filename << endl;
        
    }

    std::string line;
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string token;
        std::vector<std::string> row;
        
        while (std::getline(ss, token, ' ')) { // 空格分割字串
            row.push_back(token);
        }

        
        if(row.size()!=17){
          cout<< "  無效資料  "<< row[0] << "   "<< row.size() <<endl;
          continue;
        }
        double timestamp=stod(row[0]);
        Eigen::Matrix4d matrix;
        matrix(0, 0) = stod(row[1]);
        matrix(0, 1) = stod(row[2]);
        matrix(0, 2) = stod(row[3]);
        matrix(0, 3) = stod(row[4]);
        matrix(1, 0) = stod(row[5]);
        matrix(1, 1) = stod(row[6]);
        matrix(1, 2) = stod(row[7]);
        matrix(1, 3) = stod(row[8]);
        matrix(2, 0) = stod(row[9]);
        matrix(2, 1) = stod(row[10]);
        matrix(2, 2) = stod(row[11]);
        matrix(2, 3) = stod(row[12]);
        matrix(3, 0) = stod(row[13]);
        matrix(3, 1) = stod(row[14]);
        matrix(3, 2) = stod(row[15]);
        matrix(3, 3) = stod(row[16]);
        dataMap[timestamp] = matrix;
    }

    file.close();
   

    // std::string line;
    // while (std::getline(file, line)) {
    //     std::istringstream ss(line);
    //     double timestamp;
    //     Eigen::Matrix4d matrix;
        
    //     // Read timestamp
    //     ss >> timestamp;
        
    //     // Read matrix
    //     for (int i = 0; i < 4; ++i) {
    //         for (int j = 0; j < 4; ++j) {
    //             ss >> matrix(i, j);
    //         }
    //     }
        
    //     // Insert into map
    //     dataMap[timestamp] = matrix;
    // }
}



/*
函式功能:
讀取 時間戳和Eigen::Vector3d位置t + 旋轉四元數 資訊到filename的txt中
輸入：
檔名 filename
列表 map<double, Matrix4d> &dataMap
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Matrix4d>& poses

輸出：時間戳 t q
1.000000000000000 11.123456789 0.000000001 0.000000000 14.000000000 0.000000000 12.987654321 0.000000000 15.000000000 0.000000000 0.000000000 13.234567890 16.000000000 0.000000000 0.000000000 0.000000000 1.000000000
2.000000000000000 21.111111111 0.000000000 0.000000000 24.000000000 0.000000000 22.222222222 0.000000000 25.000000000 0.000000000 0.000000000 23.333333333 26.000000000 0.000000000 0.000000000 0.000000000 1.000000000
3.000000000000000 31.444444444 0.000000000 0.000000000 34.000000000 0.000000000 32.555555555 0.000000000 35.000000000 0.000000000 0.000000000 33.666666666 36.000000000 0.000000000 0.000000000 0.000000000 1.000000000
*/
void Read_poses_Qt4x4FromFile(const string& filename,map<double, Eigen::Matrix4d> &dataMap) {
    
    ifstream file(filename);
    if (!file.is_open()) {
        cerr << "Error opening file: " << filename << endl;
        
    }

    std::string line;
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string token;
        std::vector<std::string> row;
        
        while (std::getline(ss, token, ' ')) { // 空格分割字串
            row.push_back(token);
        }

        
        if(row.size()!= 8){
          cout<< "  無效資料  "<< row[0] << "   "<< row.size() <<endl;
          continue;
        }

        double timestamp=stod(row[0]);
        
        const Eigen::Vector3d trans_wc(stod(row[1]),stod(row[2]),stod(row[3]));
        const Eigen::Quaterniond quat_wc(stod(row[4]),stod(row[5]),stod(row[6]),stod(row[7])) ;
        // 從四元數構建旋轉矩陣
        Eigen::Matrix3d rot_wc = quat_wc.toRotationMatrix();

        // 建立一個4x4變換矩陣
        Eigen::Matrix4d transformationMatrix = Eigen::Matrix4d::Identity();
        // 填充旋轉部分
        transformationMatrix.block<3, 3>(0, 0) = rot_wc;
        // 填充平移部分
        transformationMatrix.block<3, 1>(0, 3) = trans_wc;

        dataMap[timestamp] = transformationMatrix;
    }

    file.close();
   
}


/*
函式功能:
儲存時間戳和Eigen::Vector3d位置t(沒有旋轉R)資訊到filename的txt中
輸入：
檔名 filename
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Vector3d>& poses
輸出：
1.000000000 14.000000000 15.000000000 16.000000000 
2.000000000 24.000000000 25.000000000 26.000000000 
3.000000000 34.000000000 35.000000000 36.000000000 
*/
void savePoses_t3x1ToFile(const std::string& filename,
                     const std::vector<double>& timestamps,
                     const std::vector<Eigen::Vector3d>& poses) {

    if (timestamps.size() != poses.size()) {
        std::cerr << "Error: Number of timestamps does not match number of poses." << std::endl;
        return;
    }

    std::ofstream file(filename, std::ios::out);
 

    if (!file.is_open()) {
        std::cerr << "開啟檔案失敗: " << filename << std::endl;
        return;
    }

    // Set the precision for the output stream
    file << std::fixed << std::setprecision(9);

    // Write each timestamp and its corresponding matrix
    for (size_t i = 0; i < timestamps.size(); ++i) {
        file << timestamps[i] << " ";
        
        const Eigen::Vector3d& matrix = poses[i];
        file << matrix(0) << " "
             << matrix(1) << " "
             << matrix(2) << " "
             << std::endl;
    }

    file.close();
}


/*
函式功能:
儲存時間戳和Eigen::Matrix4d位置t(沒有旋轉R)資訊到filename的txt中
輸入：
檔名 filename
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Matrix4d>& poses
輸出：
1.000000000 14.000000000 15.000000000 16.000000000 
2.000000000 24.000000000 25.000000000 26.000000000 
3.000000000 34.000000000 35.000000000 36.000000000 
*/

// Function to save timestamps and matrices to file with precision
void savePoses_t3x1ToFile(const std::string& filename,
                     const std::vector<double>& timestamps,
                     const std::vector<Eigen::Matrix4d>& poses) {

    if (timestamps.size() != poses.size()) {
        std::cerr << "Error: Number of timestamps does not match number of poses." << std::endl;
        return;
    }

    std::ofstream file(filename, std::ios::out);
 

    if (!file.is_open()) {
        std::cerr << "開啟檔案失敗: " << filename << std::endl;
        return;
    }

    // Set the precision for the output stream
    file << std::fixed << std::setprecision(9);

    // Write each timestamp and its corresponding matrix
    for (size_t i = 0; i < timestamps.size(); ++i) {
        file << timestamps[i] << " ";
        
        const Eigen::Matrix4d& matrix = poses[i];
        file << matrix(0, 3) << " "
             << matrix(1, 3) << " "
             << matrix(2, 3) << " "
             << std::endl;
    }

    file.close();
}


/*
函式功能:
儲存時間戳和Eigen::Matrix4d旋轉R和位置t資訊到filename的txt中
輸入：
檔名 filename
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Matrix4d>& poses
輸出：
1.000000000000000 11.123456789 0.000000001 0.000000000 14.000000000 0.000000000 12.987654321 0.000000000 15.000000000 0.000000000 0.000000000 13.234567890 16.000000000 0.000000000 0.000000000 0.000000000 1.000000000
2.000000000000000 21.111111111 0.000000000 0.000000000 24.000000000 0.000000000 22.222222222 0.000000000 25.000000000 0.000000000 0.000000000 23.333333333 26.000000000 0.000000000 0.000000000 0.000000000 1.000000000
3.000000000000000 31.444444444 0.000000000 0.000000000 34.000000000 0.000000000 32.555555555 0.000000000 35.000000000 0.000000000 0.000000000 33.666666666 36.000000000 0.000000000 0.000000000 0.000000000 1.000000000

*/
void savePoses_T4x4ToFile(const std::string& filename,
                     const std::vector<double>& timestamps,
                     const std::vector<Eigen::Matrix4d>& poses) {

    if (timestamps.size() != poses.size()) {
        std::cerr << "Error: Number of timestamps does not match number of poses." << std::endl;
        return;
    }

    std::ofstream file(filename, std::ios::out);
 

    if (!file.is_open()) {
        std::cerr << "開啟檔案失敗: " << filename << std::endl;
        return;
    }

    // Set the precision for the output stream
    file << std::fixed << std::setprecision(9);

    // Write each timestamp and its corresponding matrix
    for (size_t i = 0; i < timestamps.size(); ++i) {

        const Eigen::Matrix4d& matrix = poses[i];

        file << std::setprecision(15)
        << timestamps[i]<< " "

        << std::setprecision(9)
        << matrix(0, 0) << " " << matrix(0, 1) << " " << matrix(0, 2) << " " << matrix(0, 3) << " "
        << matrix(1, 0) << " " << matrix(1, 1) << " " << matrix(1, 2) << " " << matrix(1, 3) << " "
        << matrix(2, 0) << " " << matrix(2, 1) << " " << matrix(2, 2) << " " << matrix(2, 3) << " "
        << matrix(3, 0) << " " << matrix(3, 1) << " " << matrix(3, 2) << " " << matrix(3, 3) << std::endl;
    }

    file.close();
}


/*
函式功能:
儲存時間戳和Eigen::Matrix4d 位置t 和 旋轉四元數q資訊到filename的txt中
輸入：
檔名 filename
時間戳或者ID列表 timestamps
位姿  std::vector<Eigen::Matrix4d>& poses
輸出：
1.000000000000000 14.000000000 15.000000000 16.000000000 0.000000000 0.000000000 -0.000000000 3.096194398
2.000000000000000 24.000000000 25.000000000 26.000000000 0.000000000 0.000000000 0.000000000 4.112987560
3.000000000000000 34.000000000 35.000000000 36.000000000 0.000000000 0.000000000 0.000000000 4.966554809

*/
void savePoses_Qt4x4ToFile(const std::string& filename,
                     const std::vector<double>& timestamps,
                     const std::vector<Eigen::Matrix4d>& poses) {

    if (timestamps.size() != poses.size()) {
        std::cerr << "Error: Number of timestamps does not match number of poses." << std::endl;
        return;
    }

    std::ofstream file(filename, std::ios::out);
 

    if (!file.is_open()) {
        std::cerr << "開啟檔案失敗: " << filename << std::endl;
        return;
    }

    // Set the precision for the output stream
    file << std::fixed << std::setprecision(9);


    // Write each timestamp and its corresponding matrix
    for (size_t i = 0; i < timestamps.size(); ++i) {
      
        const Eigen::Matrix4d& cam_pose_wc = poses[i];// 相機到世界位姿== 相機在世界座標系下的世界位姿

        const Eigen::Matrix3d& rot_wc = cam_pose_wc.block<3, 3>(0, 0);
        const Eigen::Vector3d& trans_wc = cam_pose_wc.block<3, 1>(0, 3);
        const  Eigen::Quaterniond quat_wc =  Eigen::Quaterniond(rot_wc);

        file << std::setprecision(15)
            << timestamps[i]<< " "
            << std::setprecision(9)
            << trans_wc(0) << " " << trans_wc(1) << " " << trans_wc(2) << " "
            << quat_wc.x() << " " << quat_wc.y() << " " << quat_wc.z() << " " << quat_wc.w() << std::endl;
    }
    
    

    file.close();
}


int main() {
    // Example data
    std::vector<double> timestamps = {1.0, 2.0, 3.0};
    
    // Initialize example poses with some values
    Eigen::Matrix4d pose1, pose2, pose3;
    pose1 << 11.123456789, 0.000000001, 0.000000000, 14.000000000,
             0.000000000, 12.987654321, 0.000000000, 15.000000000,
             0.000000000, 0.000000000, 13.234567890, 16.000000000,
             0.000000000, 0.000000000, 0.000000000, 1.000000000;

    pose2 << 21.111111111, 0.000000000, 0.000000000, 24.000000000,
             0.000000000, 22.222222222, 0.000000000, 25.000000000,
             0.000000000, 0.000000000, 23.333333333, 26.000000000,
             0.000000000, 0.000000000, 0.000000000, 1.000000000;

    pose3 << 31.444444444, 0.000000000, 0.000000000, 34.000000000,
             0.000000000, 32.555555555, 0.000000000, 35.000000000,
             0.000000000, 0.000000000, 33.666666666, 36.000000000,
             0.000000000, 0.000000000, 0.000000000, 1.000000000;

    // Store matrices in a vectorsavePoses_T4x4ToFile
    std::vector<Eigen::Matrix4d> poses = {pose1, pose2, pose3};

    // Save to file
    savePoses_Qt4x4ToFile("poses_Qt4x4.txt", timestamps, poses);
    savePoses_T4x4ToFile("poses_T4x4.txt", timestamps, poses);
    savePoses_t3x1ToFile("poses_t3x3.txt", timestamps, poses);



    map<double, Eigen::Vector3d> poses_t3x3 ;
    Read_poses_t3x3FromFile("poses_t3x3.txt",poses_t3x3);

    // 輸出讀取的資料
    for (const auto& entry : poses_t3x3) {
        cout << "Timestamp: " << entry.first 
             << ", Position: (" 
             << entry.second.x() << ", " 
             << entry.second.y() << ", " 
             << entry.second.z() << ")" << endl;
    }

    map<double, Eigen::Matrix4d> poses_T4x4 ;
    Read_poses_T4x4FromFile("poses_T4x4.txt",poses_T4x4);
    // Example output to verify correctness
    for (const auto& pair : poses_T4x4) {
        std::cout << "Timestamp: " << pair.first << std::endl;
        std::cout << "Matrix:\n" << pair.second << std::endl;
    }

    map<double, Eigen::Matrix4d> poses_T4x4_qt ;
    Read_poses_Qt4x4FromFile("poses_Qt4x4.txt",poses_T4x4_qt);
    // Example output to verify correctness
    for (const auto& pair : poses_T4x4_qt) {
        std::cout << "Timestamp: " << pair.first << std::endl;
        std::cout << "Matrix:\n" << pair.second << std::endl;
    }

    return 0;
}

C++(2) 從yml或者txt讀取和儲存資料
2024-07-15
C++
c++ 從txt讀取資料按照特殊字元拆分 gnss
2024-07-31
C++字元
Mybatis讀取和儲存json型別的資料
2024-09-12
MyBatisJSON型別
讀取和儲存Excel表
2019-02-21
Excel
Redis資料儲存和讀寫
2024-06-02
Redis
在 SAP BTP Kyma Runtime 上使用 Redis 讀取和儲存資料
2021-06-04
Redis
spark學習筆記--資料讀取與儲存
2018-07-09
Spark筆記
java讀取倒序儲存的int型資料
2020-10-14
Java
Spark(16) -- 資料讀取與儲存的主要方式
2020-10-05
Spark
基本工具（1） openvslam 讀取gnss.txt，然後匹配讀取影像路徑，儲存軌跡到txt
2024-12-07
SLAM
redis叢集資料儲存和獲取原理
2018-12-28
Redis
（slam工具）1檔案讀取和儲存
2024-06-17
SLAM
資料儲存(1)：從資料儲存看人類文明-資料儲存器發展歷程
2020-07-27
[20210318]bbed讀取資料塊.txt
2021-03-18
Web快取知多少(快取機制和資料儲存)
2019-03-04
Web快取
java+pgsql實現儲存圖片到資料庫，以及讀取資料庫儲存的圖片
2020-11-13
JavaSQL資料庫
實用小程式（1）之讀取xml並儲存為txt文字
2020-11-22
XML
[20191219]oracle timestamp資料型別的儲存.txt
2019-12-19
Oracle資料型別
NOPI讀取Word模板並儲存
2018-08-04
[20210831]bbed讀取資料塊6.txt
2021-09-01
[20210323]bbed讀取資料塊5.txt
2021-03-23
[20210318]bbed讀取資料塊2.txt
2021-03-19
[20210319]bbed讀取資料塊3.txt
2021-03-19
Pandas之EXCEL資料讀取/儲存/檔案分割/檔案合併
2019-01-30
Excel
一文詳解 JuiceFS 讀效能：預讀、預取、快取、FUSE 和物件儲存
2024-07-26
UI快取物件
Logstash : 從 SQL Server 讀取資料
2019-07-17
SQLServer
資料儲存--檔案儲存
2024-05-26
[20241009]oracle timestamp with time zone資料型別的儲存.txt
2024-10-09
Oracle資料型別
大資料儲存平臺之異構儲存實踐深度解讀
2018-06-06
大資料
Excel 讀取圖片並獲取儲存路徑
2021-01-12
Excel
Java讀取暫存器資料的方法
2024-09-16
Java
讀書筆記5-資料儲存篇
2019-03-01
筆記
golang 讀取切分儲存byte流檔案
2019-02-16
Golang
[20210930]bbed讀取資料塊7 fffext.sh.txt
2021-10-08
[20210906]bbed讀取資料塊(bbed-wrap.sh).txt
2021-09-07
用IPFS和以太坊儲存資料
2018-11-23
以太坊和IPFS如何儲存資料
2018-11-23
MySQL原理簡介—5.儲存模型和資料讀寫機制
2024-11-24
MySql模型

c++ (2-0) 從txt讀取和儲存資料

相關文章