c++ read and save txt

 read and save 

#include "util/image_util.h"

#ifdef USE_PANGOLIN_VIEWER
#include "pangolin_viewer/viewer.h"
#elif USE_SOCKET_PUBLISHER
#include "socket_publisher/publisher.h"
#endif

#include "openvslam/system.h"
#include "openvslam/config.h"

#include <iostream>
#include <chrono>
#include <numeric>

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <spdlog/spdlog.h>
#include <popl.hpp>
// 打印精度控制
#include <iostream>
#include <iomanip>


using namespace std;

#ifdef USE_STACK_TRACE_LOGGER
#include <glog/logging.h>
#endif

#ifdef USE_GOOGLE_PERFTOOLS
#include <gperftools/profiler.h>
#endif



void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<string> &vTimestamps, vector<vector<double>> &GPSDataList)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/FHY_config/FHY_gps.txt";
 
 
    // load image list
    FILE* file;
    file = std::fopen(strPathTimeFile.c_str() , "r"); //.c_str()
    if(file == NULL){
        printf("cannot find file: %s  \n", strPathTimeFile.c_str());
         
        return ;         
    }
 
    char timeshara_c[30];
    char lat_c[30], lon_c[30], alt_c[30];
 
 
 
 
    vector<string> imageNameTimeList;
 
    //vector<vector<string>> GPSDataList;
 
 
    // 按照string 读取出来 容易保存超高精度
    while (fscanf(file, "%s %s %s %s", &timeshara_c, &lat_c, &lon_c, &alt_c) != EOF)
    ////%lf之间应该有逗号，因为没有逗号只能读第一个数。用&是因为要把数存到对应数组元素的地址中去。\n是换行读取
    //while (fscanf(file, "%lf %lf %lf %lf", &timesharap_l, &lat_l, &lon_l, &alt_l) != EOF)
    {
 
 
        string timesharap_s;
        string lat_s, lon_s, alt_s;
 
 
        timesharap_s=(string(timeshara_c)); //stod
        lat_s=(string(lat_c));
        lon_s=(string(lon_c));
        alt_s=(string(alt_c));
 
         
        double timesharap_d;
        double lat_d, lon_d, alt_d;
 
 
 
        timesharap_d=std::stod(timesharap_s);
        lat_d=std::stod(lat_s);
        lon_d=std::stod(lon_s);
        alt_d=std::stod(alt_s);
 
 
 
        imageNameTimeList.push_back(timesharap_s);
 
 
 
        vTimestamps.push_back(timesharap_s);

        //cout<<"timesharap_s  "<<timesharap_s<< "   timesharap_d  "<<timesharap_d<<endl;
 
        vector<double> gpsdata_i;
        gpsdata_i.push_back(timesharap_d);
        gpsdata_i.push_back(lat_d);
        gpsdata_i.push_back(lon_d);
        gpsdata_i.push_back(alt_d);
        GPSDataList.push_back(gpsdata_i);
 
        std::cout<< fixed << setprecision(18)   <<"timesharap_string  "<<timesharap_s<< "   timesharap_double18  "<<timesharap_d << ' '<<  setprecision(18)  <<lat_s<< " " << lon_s <<" "<<alt_s<<std::endl;
        
    }
    std::fclose(file);
 
 
    string strPrefixLeft = strPathToSequence + "/left/";
 
    const int nTimes = vTimestamps.size();
    vstrImageFilenames.resize(nTimes);
 
    for(int i=0; i<nTimes; i++)
    {
        //stringstream ss;
        //ss << setfill('0') << setw(10) << i;
 
        vstrImageFilenames[i] = strPrefixLeft + imageNameTimeList[i] + ".png";

        //cout<< vstrImageFilenames[i] <<endl;
    }
}

    vector<string> vstrImageFilenames;  // 图像路径
    vector<double> vTimestamps;    // 时间戳  也就是图像名字
    vector<vector<double>> GPSDataList;// 保存GPS数据列表

//==============================跟踪进程=========================================
void mono_tracking(const std::shared_ptr<openvslam::config>& cfg, 
                   const std::string& vocab_file_path, 
                   const std::string& image_dir_path, 
                   const std::string& mask_img_path, 
                   const unsigned int frame_skip, 
                   const bool no_sleep, const bool auto_term, 
                   const bool eval_log, const std::string& map_db_path, 
                   std::string &data_path,
                   vector<string> &vstrImageFilenames, 
                   vector<string> &vTimestamps,        
                   vector<vector<double>> &GPSDataList 
                   ) {
    // load the mask image
    const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE);
    
    
    //const image_sequence sequence(image_dir_path, cfg->camera_->fps_);
    //const auto frames = sequence.get_frames();

    int nImages = vstrImageFilenames.size();


    // build a SLAM system
    openvslam::system SLAM(cfg, vocab_file_path);
    // startup the SLAM process
    SLAM.startup();

#ifdef USE_PANGOLIN_VIEWER
    pangolin_viewer::viewer viewer(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher());
#elif USE_SOCKET_PUBLISHER
    socket_publisher::publisher publisher(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher());
#endif

    std::vector<double> track_times;
    track_times.reserve(nImages);

    // run the SLAM in another thread
    std::thread thread([&]() {

        string path= data_path+"/openvslam_outdata/slam_xyzgps.txt";
        std::ofstream ofs(path, std::ios::out);
        if (!ofs.is_open()) {
            spdlog::critical("cannot create a file at {}", path);
            throw std::runtime_error("cannot create a file at " + path);
        }


        for (unsigned int i = 0; i < nImages; ++i) {
            //const auto& frame = frames.at(i);

            double timesharpi_fps=(1.0 / cfg->camera_->fps_) * i; // 根据帧率控制读取离线图像的速度
            double timesharpi1_fps=(1.0 / cfg->camera_->fps_) * (i+1);


            const auto img = cv::imread(vstrImageFilenames[i], cv::IMREAD_UNCHANGED);

            const auto tp_1 = std::chrono::steady_clock::now();

            if (!img.empty() && (i % frame_skip == 0)) {
                // input the current frame and estimate the camera pose
                //=======================识别当前帧================
                // 这里的位姿并没有经历过局部和全局优化
                //double img_time_sharp=std::stod(vTimestamps[i]);//  time will lost  no img name
                double img_time_sharp=i;
                 
                Eigen::Matrix4d cam_pose_cw  = SLAM.feed_monocular_frame(img, img_time_sharp , mask);

                Eigen::Matrix4d cam_pose_wc = cam_pose_cw.inverse();

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

                cout << fixed <<  std::setprecision(4)
                    //<< vTimestamps[i] << " "
                    << 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;
                    << GPSDataList[i][1] << " " << GPSDataList[i][2] << " " << GPSDataList[i][3] << std::endl;


                ofs <<  fixed << std::setprecision(4)
                            //<< vTimestamps[i]  << " "
                            << i  << " "
                            << std::setprecision(9)
                            << trans_wc(0) << " " << trans_wc(1) << " " << trans_wc(2) << " "
                            << GPSDataList[i][1] << " " << GPSDataList[i][2] << " " << GPSDataList[i][3] << std::endl;
                            


                 
            }

            const auto tp_2 = std::chrono::steady_clock::now();

            const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
            if (i % frame_skip == 0) {
                track_times.push_back(track_time);
            }

            // wait until the timestamp of the next frame
            if (!no_sleep && i < nImages - 1) {
                //// 根据帧率控制读取离线图像的速度
                const auto wait_time = timesharpi1_fps - (timesharpi_fps+ track_time);
                if (0.0 < wait_time) {
                    //std::this_thread::sleep_for(std::chrono::microseconds(static_cast<unsigned int>(wait_time * 1e6)));
                }
            }

            // check if the termination of SLAM system is requested or not
            if (SLAM.terminate_is_requested()) {
                break;
            }
        }

        // wait until the loop BA is finished
        while (SLAM.loop_BA_is_running()) {
            std::this_thread::sleep_for(std::chrono::microseconds(5000));
        }

        // automatically close the viewer
        #ifdef USE_PANGOLIN_VIEWER
                if (auto_term) {
                    viewer.request_terminate();
                }
        #elif USE_SOCKET_PUBLISHER
                if (auto_term) {
                    publisher.request_terminate();
                }
        #endif


        ofs.close();



    });

    // run the viewer in the current thread
#ifdef USE_PANGOLIN_VIEWER
    viewer.run();
#elif USE_SOCKET_PUBLISHER
    publisher.run();
#endif

    thread.join();

    // shutdown the SLAM process
    SLAM.shutdown();

    if (1) {
        // output the trajectories for evaluation
        SLAM.save_frame_trajectory(data_path+"/openvslam_outdata/frame_trajectory.txt", "TUM");
        SLAM.save_keyframe_trajectory(data_path+"/openvslam_outdata/keyframe_trajectory.txt", "TUM");
        // output the tracking times for evaluation
        std::ofstream ofs(data_path+"/openvslam_outdata/track_times.txt", std::ios::out);
        if (ofs.is_open()) {
            for (const auto track_time : track_times) {
                ofs << track_time << std::endl;
            }
            ofs.close();
        }
    }

    if (!map_db_path.empty()) {
        // output the map database
        SLAM.save_map_database(map_db_path);  
    }

    std::sort(track_times.begin(), track_times.end());
    const auto total_track_time = std::accumulate(track_times.begin(), track_times.end(), 0.0);
    std::cout << "median tracking time: " << track_times.at(track_times.size() / 2) << "[s]" << std::endl;
    std::cout << "mean tracking time: " << total_track_time / track_times.size() << "[s]" << std::endl;
}


//========================主函数==============================

int main(int argc, char* argv[]) {


    #ifdef USE_STACK_TRACE_LOGGER
        google::InitGoogleLogging(argv[0]);
        google::InstallFailureSignalHandler();
    #endif

    // create options
    //# 参数输入有顺序 别搞错位了
    //popl::OptionParser op("Allowed options");
    //auto help = op.add<popl::Switch>("h", "help", "produce help message");

    // auto vocab_file_path = op.add<popl::Value<std::string>>("v", "vocab", "vocabulary file path");
    // auto img_dir_path = op.add<popl::Value<std::string>>("i", "img-dir", "directory path which contains images");
    // auto config_file_path = op.add<popl::Value<std::string>>("c", "config", "config file path");
    // auto mask_img_path = op.add<popl::Value<std::string>>("", "mask", "mask image path", "");
    // auto frame_skip = op.add<popl::Value<unsigned int>>("", "frame-skip", "interval of frame skip", 1);
    // auto no_sleep = op.add<popl::Switch>("", "no-sleep", "not wait for next frame in real time");
    // auto auto_term = op.add<popl::Switch>("", "auto-term", "automatically terminate the viewer");
    // auto debug_mode = op.add<popl::Switch>("", "debug", "debug mode");
    // auto eval_log = op.add<popl::Switch>("", "eval-log", "store trajectory and tracking times for evaluation");
    // auto map_db_path = op.add<popl::Value<std::string>>("p", "map-db", "store a map database at this path after SLAM", "");


    string vocab_file_path = argv[1];
    string data_dir_path = argv[2];
    std::string img_dir_path=data_dir_path+"/left/";
    std::string map_db_path =data_dir_path+"/openvslam_outdata/"+argv[4];
    string config_file_path = argv[3];

    string mask_img_path = "";
    unsigned int frame_skip = 1;
    bool no_sleep = 0 ;// 睡眠等待处理完在加载下一张离线图像
    bool auto_term = 0;
    bool debug_mode = 1;
    bool eval_log = 1;


    if (vocab_file_path=="" || img_dir_path=="" || config_file_path=="") {
        std::cerr << "invalid arguments" << std::endl;
        std::cerr << std::endl;
        //std::cerr << op << std::endl;
        return EXIT_FAILURE;
    }

    // setup logger
    spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$");
    if (debug_mode) {
        spdlog::set_level(spdlog::level::debug);
    }
    else {
        spdlog::set_level(spdlog::level::info);
    }

    // load configuration
    std::shared_ptr<openvslam::config> cfg;
    try {
        cfg = std::make_shared<openvslam::config>(config_file_path);
    }
    catch (const std::exception& e) {
        std::cerr << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    #ifdef USE_GOOGLE_PERFTOOLS
        ProfilerStart("slam.prof");
    #endif

    //=================加载图像数据 gps时间戳==========================
  
    // Retrieve paths to images
    vector<string> vstrImageFilenames;  // 图像路径
    vector<string> vTimestamps_string;    // 时间戳  也就是图像名字
    vector<vector<double>> GPSDataList;// 保存GPS数据列表

    // 根据时间戳列表 找图像加载
    LoadImages(data_dir_path, vstrImageFilenames, vTimestamps_string, GPSDataList);
 
    



    // run tracking
    if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {

        mono_tracking(cfg, vocab_file_path,
                img_dir_path, 
                mask_img_path,
                frame_skip, 
                no_sleep, 
                auto_term,
                eval_log, 
                map_db_path,
                data_dir_path,
                vstrImageFilenames,
                vTimestamps_string,
                GPSDataList
                );
    }
    else {
        throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());
    }

    #ifdef USE_GOOGLE_PERFTOOLS
        ProfilerStop();
    #endif

    return EXIT_SUCCESS;
}

　　

 

 

 

save

yi ding yao 

 ofs << fixed 

　　

    
    string path=image_path + "/ORB_SLAM3_out/slam_xyzgps.txt";

    std::ofstream ofs(path, std::ios::out);
    if (!ofs.is_open()) {
        cout << " error path" << path << endl;
    }


        ofs << fixed << std::setprecision(4)
                //<< timestamps.at(frm_id) << " "
                << ni <<" "
                << std::setprecision(8)
                << t_wc(0) << " " <<  t_wc(1) << " " << t_wc(2) << " "
               
                << GPSDataList[ni][1] << " " << GPSDataList[ni][2] << " " << GPSDataList[ni][3]  
                 << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;

       ofs.close();