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++字元
Android儲存讀取txt檔案
2017-11-23
Android
Mybatis讀取和儲存json型別的資料
2024-09-12
MyBatisJSON型別
讀取和儲存Excel表
2019-02-21
Excel
Redis資料儲存和讀寫
2024-06-02
Redis
Android內部儲存和外部儲存，以及讀取Android讀取RAM,ROM內部儲存和外部儲存卡容量
2017-12-02
Android
java讀取倒序儲存的int型資料
2020-10-14
Java
Spark(16) -- 資料讀取與儲存的主要方式
2020-10-05
Spark
spark學習筆記--資料讀取與儲存
2018-07-09
Spark筆記
java mysql儲存和讀取檔案
2011-12-11
JavaMySql
在 SAP BTP Kyma Runtime 上使用 Redis 讀取和儲存資料
2021-06-04
Redis
redis叢集資料儲存和獲取原理
2018-12-28
Redis
資料儲存(1)：從資料儲存看人類文明-資料儲存器發展歷程
2020-07-27
（slam工具）1檔案讀取和儲存
2024-06-17
SLAM
C#中從Clipboard儲存獲取資料的方法
2009-04-20
C#
Web快取知多少(快取機制和資料儲存)
2019-03-04
Web快取
實用小程式（1）之讀取xml並儲存為txt文字
2020-11-22
XML
java+pgsql實現儲存圖片到資料庫，以及讀取資料庫儲存的圖片
2020-11-13
JavaSQL資料庫
python讀取檔案——python讀取和儲存mat檔案
2017-11-20
Python
java儲存讀取文字oracle
2011-12-11
JavaOracle
使用Hibernate和Struts向資料庫中儲存、讀取並顯示圖片
2010-09-01
資料庫
NOPI讀取Word模板並儲存
2018-08-04
Python -讀取,儲存檔案
2017-05-23
Python
Pandas之EXCEL資料讀取/儲存/檔案分割/檔案合併
2019-01-30
Excel
一文詳解 JuiceFS 讀效能：預讀、預取、快取、FUSE 和物件儲存
2024-07-26
UI快取物件
Javascript寫入txt和讀取txt檔案示例
2016-11-12
JavaScript
大資料的儲存和管理
2013-04-17
大資料
資料儲存--檔案儲存
2024-05-26
Java讀取暫存器資料的方法
2024-09-16
Java
Logstash : 從 SQL Server 讀取資料
2019-07-17
SQLServer
大資料儲存平臺之異構儲存實踐深度解讀
2018-06-06
大資料
Excel 讀取圖片並獲取儲存路徑
2021-01-12
Excel
資料儲存
2016-12-28
讀書筆記5-資料儲存篇
2019-03-01
筆記
從一條資料說起——InnoDB儲存資料結構
2020-05-07
資料結構
從資料儲存發展史看IPFS/Filecoin
2021-01-05
從舊版本SQL Server中重新儲存資料
2008-07-03
SQLServer

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

相關文章