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();

  

c++ read and save txt的更多相关文章

  1. [JS] save txt file

    (function () { var blob = new Blob(['content'], {type: 'text/plain; charset=utf-8'}), blobUrl = URL. ...

  2. 【matlab】将matlab中数据输出保存为txt或dat格式

    将matlab中数据输出保存为txt或dat格式 总结网上各大论坛,主要有三种方法. 第一种方法:save(最简单基本的) 具体的命令是:用save *.txt -ascii x x为变量 *.txt ...

  3. [matlab]改变矩阵的大小并保存到txt文件

    要完成的任务是,加载一个保存在txt文件中的矩阵, 并把它扩大10倍,并且要再次保存回去 %加载txt文件 >load('Matrix.txt'); %扩大10倍 repmat(Matrix,r ...

  4. MATLAB将矩阵使用.txt文件格式保存

    具体的命令是:用save *.txt -ascii x x为变量 *.txt为文件名,该文件存储于当前工作目录下,再打开就可以 打开后,数据有可能是以指数形式保存的.   看下面这个例子: a =[1 ...

  5. Linux中进行单文件内容的复制

    文件内容复制的常规方法: 开辟一段空间,不断读取文件的内容并写入另一文件当中,这种方法好在安全,一般在类型允许的最大范围内是安全的,缺点就是复制内容的时间长 一次性复制文件的内容,这种方法必须首先获取 ...

  6. TF-IDF算法确定阅读主题词解答英语阅读Title题目

    #include <math.h> #include <time.h> #include <stdlib.h> #include <iostream> ...

  7. 网络电视精灵~分析~~~~~~简单工厂模式,继承和多态,解析XML文档,视频项目

    小总结: 所用技术: 01.C/S架构,数据存储在XML文件中 02.简单工厂模式 03.继承和多态 04.解析XML文档技术 05.深入剖析内存中数据的走向 06.TreeView控件的使用 核心: ...

  8. python python 入门学习之网页数据爬虫搜狐汽车数据库

    自己从事的是汽车行业,所以首先要做的第一个程序是抓取搜狐汽车的销量数据库(http://db.auto.sohu.com/cxdata/): 数据库提供了07年至今的汽车月销量,每个车型对应一个xml ...

  9. Atitit 发帖机系列(6) USRQBN2201 setup spec安装程序的实现规范与标准化解决方案

    Atitit 发帖机系列(6) USRQBN2201 setup spec安装程序的实现规范与标准化解决方案 安装主要解决一个问题,就是resin的内容启动路径以及端口..这里是使用的端口8077 主 ...

  10. python2.1-原理之琐碎技巧

    本系列依据<python学习手册第四版>而写,也算是个学习笔记吧,选择本书的原因在于它不同于第三版,它强调介绍python3.0 ,而会在不同的地方给出2.6版本的区别,:本书侧重介绍原理 ...

随机推荐

  1. 梦想Android版CAD控件(安卓CAD二次开发,安卓CAD控件)2023.02.26更新

    下载地址:https://www.mxdraw.com/ndetail_40240.html1. 增加willBeReturnStart事件2. 增加使用OpenGL缓存3. 优化界面响应时间4. 修 ...

  2. WebRTC 的音频弱网对抗之 NACK

    基础知识 音频的 NACK 机制在 WebRTC 中默认是关闭的. rtcp feedbacknack开启就可以了 WebRTC 的音频数据传输中,尽管对低延时有着很高的要求,但也实现了 NACK,以 ...

  3. SQL执行定时任务JOB,包教包会

    什么是JOB? 数据库中可以定时执行任务的功能组件,那就是JOB. JOB的作用 它可以按我们设置好的参数定时执行查询语句或存储过程,特别适合一些每天,每周,每月,每年这种需要循环执行任务的场景,当然 ...

  4. 使用python做基本的数据处理

    使用python做基本的数据处理 1.常用的基本数据结构 元组.列表.字典.集合.常用的序列函数 1.1基本操作 1.1.1 元组:逗号分开的序列值. tup = tuple (4,5,6) tup ...

  5. vue 绑定样式,跟点击事件的顺序会影响

    <view class="mfst-item" v-for="(item, idx) in majorArr" :key="mfsKey&quo ...

  6. sdp安装及实例

    环境: sdpserver:192.168.1.160 sdpclient:192.168.1.161 安装 yum install gcc gcc-c++ libpcap* libtool* wge ...

  7. How to Fix SSH Failed Permission Denied

    https://phoenixnap.com/kb/ssh-permission-denied-publickey

  8. darkriscv笔记

    1 按照默认设置,前4K空间为ROM,后4K空间是RAM.为什么RAM需要初值,初值从哪儿来? 2 指令分类 LUI: AUTPC: JAL: JALR: BCC : BEQ/BGE /BGEU/BL ...

  9. 2022.11.13 NOIP2022 模拟赛八

    「ROI 2017 Day 2」存储器 无聊的题. 首先 \(s\) 中每一个片段,其在 \(t\) 中对应的字符必然是相同的. 对于 \(t\) 中的每一个片段,考虑检查能否操作出这个片段,实际上只 ...

  10. CF513F2 题解

    题意 传送门 有 \(a+b+1\) 个会动的棋子,在一个大小为 \(n\times m\) 的棋盘上,棋盘上有一些点有障碍.棋子中,有 \(a\) 个红色棋子,\(b\) 个蓝色棋子,和 \(1\) ...