首页 > 编程语言 >c++ (2-0) 从txt读取和保存数据

c++ (2-0) 从txt读取和保存数据

时间：2024-08-17 14:53:57浏览次数：13  
标签：std 0.000000000 txt const 读取 c++ filename timestamps poses

 

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;
}

　　

标签：std,0.000000000,txt,const,读取,c++,filename,timestamps,poses
From： https://www.cnblogs.com/gooutlook/p/18364400

相关文章

  • OpenCV图像处理——轮廓的面积与弧长计算（C++/Python）
    概述轮廓面积与轮廓周长是图像分析中的两项核心统计特征，它们为理解和量化图像中的形状提供了基础。轮廓面积：这代表了轮廓所界定区域的像素数量，是衡量区域大小的直接指标。面积的计算结果以像素平方为单位，为我们提供了一个量化的尺度来比较不同物体的相对大小。轮廓周长......
  • C++多线程详解 | 线程创建 | 互斥锁 | 条件变量 | 线程池
    目录前言1.线程创建2.互斥锁3.lock_guard与std::unique_lock4.condition_variable 5.线程池前言在说线程之前，先说说进程和线程的关系，以及什么是多线程（为了方便理解就用大白话来说）进程：进程就是运行中的程序，比如说一个微信的程序，你双击它，它运行起来了就是一个进程，在还......
  • C++ 模版详解 | 函数模板 | 类模版
    前言 什么是模板？模板是一个泛型编程的概念，即不考虑类型的一种编程方式，能够实现代码重用，提高效率模板可分为函数模板、类模板 模板的声明和定义模板的声明有两种，一种就是typename，另外一种就是使用class ，一般使用一种声明格式就可以了，不建议混合使用。template<typenam......
  • ZYNQ SoC如何读取在Windows下配置的环境变量
    在解释ZYNQSoC如何读取在Windows下配置的环境变量之前，需要澄清一点：通常，ZYNQSoC或任何嵌入式系统并不直接在Windows操作系统下运行或配置环境变量。环境变量的配置通常是在嵌入式系统的开发阶段，在开发主机（可能是运行Windows的PC）上进行的，然后通过交叉编译、生成镜像文件等方......
  • C++多维数组与指针
    定义inta[3][4]={{1,3,5,7},{9,11,13,15},{17,18,21,23}};a代表二维数组首元素的地址，现在的首元素不是一个整型变量，而是由4个整型元素所组成的一维数组，因此a代表的是首行的起始地址，a+1代表第二行首地址。a代表的是首行的起始地址，即a[0]行的首地址，&a[0]a+1代表第二行首......
  • C++类和对象(中)
    前言：我们学习了类和对象的上部分，对类和对象有了一些认识，接下来了解类和对象的中间部分，构造函数，析构函数，拷贝构造，赋值构造这部分也比较重要，我们需要牢牢掌握，一起加油吧！1.类的默认成员函数默认成员函数就是我们不用写系统自动生成的函数，我们不写的情况下编译器会默认生成6......
  • C++编程：内存栅栏（Memory Barrier）详解及在多线程编程中的应用
    文章目录0.引言1.什么是内存栅栏？2.为什么需要内存栅栏？本质原因是什么？2.1编译器优化2.2CPU乱序执行3.ARM64和x86架构下的内存栅栏差异3.1x86架构3.2ARM64架构4.代码示例4.1代码解析4.2memory_order_release和memory_order_acquire解释4.3为什么是“releas......
  • 杭电基础100题（2000~2099）C++ 本萌新的刷题日记
    开始之前本人是刚学完C++基础语法的萌新，从B站了解到了杭电的100道水题基础题，于是打算开始刷题并在这里写下解题思路和一些想法，以便日后回顾，顺便分享给大家。我的计划是一天15题。这是我第一次在CSDN上发文章，还不是很熟悉怎么编辑。基本上每一题都会把代码和感想放这里。200......
  • [20240815]oracle21c环境变量ORACLE_PATH与SQLPATH(windows).txt
    [20240815]oracle21c环境变量ORACLE_PATH与SQLPATH(windows).txt--//我记忆以前测试过这个问题,当时是家里的笔记本,安装oracle12.2cforwindows.OS:windows7,发现无法访问SQLPATH或者--//ORACLE_PATH环境变量定义的路径下login.sql文件.我当时解决办法就是登录手工执行init.sq......
  • [20240816]oracle21c环境变量ORACLE_PATH与SQLPATH(linux).txt
    [20240816]oracle21c环境变量ORACLE_PATH与SQLPATH(linux).txt--//我记忆以前测试过这个问题,当时是家里的笔记本,安装oracle12.2cforwindows.OS:windows7,发现无法访问SQLPATH或者--//ORACLE_PATH环境变量定义的路径下login.sql文件.我当时解决办法就是登录手工执行init.sql......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99