多传感器融合定位（GPS+Wheel+camera）（1）-读取传感器数据

文章目录

1、读取Kaist数据集到融合系统中

1、读取Kaist数据集到融合系统中

int main(int argc, char** argv) {if (argc != 3) {LOG(ERROR) << "[main]: Please input param_file, data_folder.";return EXIT_FAILURE;}FLAGS_minloglevel = 3;const std::string param_file = argv[1]; //参数文件const std::string data_folder = argv[2];//数据集文件// Create FilterFusion system.FilterFusion::FilterFusionSystem FilterFusion_sys(param_file);  //滤波融合系统构造函数初始化// Load all encoder data to buffer.//加载所有的里程计数据到缓存中std::unordered_map<std::string, std::string> time_encoder_map;if (!LoadSensorData(data_folder + "/sensor_data/encoder.csv", &time_encoder_map)) {LOG(ERROR) << "[main]: Failed to load encoder data.";return EXIT_FAILURE;} // Load all gps data to buffer.//加载所有的GPS数据到缓存中std::unordered_map<std::string, std::string> time_gps_map;if (!LoadSensorData(data_folder + "/sensor_data/gps.csv", &time_gps_map)) {LOG(ERROR) << "[main]: Failed to load gps data.";return EXIT_FAILURE;} //加载时间戳std::ifstream file_data_stamp(data_folder + "/sensor_data/data_stamp.csv");if (!file_data_stamp.is_open()) {LOG(ERROR) << "[main]: Failed to open data_stamp file.";return EXIT_FAILURE;}std::vector<std::string> line_data_vec;line_data_vec.reserve(3);//增加vector 的capacitystd::string line_str, value_str;while (std::getline(file_data_stamp, line_str)) {line_data_vec.clear();std::stringstream ss(line_str);while (std::getline(ss, value_str, ',')) { line_data_vec.push_back(value_str); }constexpr double kToSecond = 1e-9;const std::string& time_str = line_data_vec[0];const double timestamp = std::stod(time_str) * kToSecond; //stod将string类型转换为double类型const std::string& sensor_type = line_data_vec[1];//传感器类型if (sensor_type == "stereo") {  //双目const std::string img_file = data_folder + "/image/stereo_left/" + time_str + ".png";const cv::Mat raw_image = cv::imread(img_file, CV_LOAD_IMAGE_ANYDEPTH);if (raw_image.empty()) {LOG(ERROR) << "[main]: Failed to open image at time: " << time_str;return EXIT_FAILURE;}// Convert raw image to color image.cv::Mat color_img;cv::cvtColor(raw_image, color_img, cv::COLOR_BayerRG2RGB);//COLOR_BayerRG2RGB常用于CMOS CCD相机中// Convert raw image to gray image.cv::Mat gray_img;cv::cvtColor(color_img, gray_img, cv::COLOR_RGB2GRAY);// Feed image to system.FilterFusion_sys.FeedImageData(timestamp, gray_img); //时间戳和图像喂到系统中}//里程计if (sensor_type == "encoder") {//如果没查找到 ，打印失败if (time_encoder_map.find(time_str) == time_encoder_map.end()) {LOG(ERROR) << "[main]: Failed to find encoder data at time: " << time_str;return EXIT_FAILURE;}const std::string& encoder_str = time_encoder_map.at(time_str);std::stringstream enc_ss(encoder_str);line_data_vec.clear();while (std::getline(enc_ss, value_str, ',')) { line_data_vec.push_back(value_str); }const double left_enc_cnt = std::stod(line_data_vec[1]);//左轮 stod 将数据转换成double 类型const double right_enc_cnt = std::stod(line_data_vec[2]);//右轮// Feed wheel data to system.FilterFusion_sys.FeedWheelData(timestamp, left_enc_cnt, right_enc_cnt);}//GPS传感器if (sensor_type == "gps") {if (time_gps_map.find(time_str) == time_gps_map.end()) {LOG(ERROR) << "[main]: Failed to find gps data at time: " << time_str;return EXIT_FAILURE;}const std::string& gps_str = time_gps_map.at(time_str);std::stringstream gps_ss(gps_str);line_data_vec.clear();while (std::getline(gps_ss, value_str, ',')) { line_data_vec.push_back(value_str); }const double lat = std::stod(line_data_vec[1]);//经度const double lon = std::stod(line_data_vec[2]);//纬度const double hei = std::stod(line_data_vec[3]);//海拔Eigen::Matrix3d cov;// cov是位置协方差for (size_t i = 0; i < 9; ++i) {cov.data()[i] = std::stod(line_data_vec[4+i]);}// Feed gps data to system.//GPS数据送到系统中FilterFusion_sys.FeedGpsData(timestamp, lon, lat, hei, cov);}}std::cin.ignore(); //跳过输入中无用的部分，提取有用的部分return EXIT_SUCCESS;
}

加载传感器数据

bool LoadSensorData(const std::string& encoder_file_path, std::unordered_map<std::string, std::string>* time_data_map) {std::ifstream encoder_file(encoder_file_path);if (!encoder_file.is_open()) {LOG(ERROR) << "[LoadSensorData]: Failed to open encoder file.";return false;} std::string line_str, time_str;while (std::getline(encoder_file, line_str)) {std::stringstream ss(line_str);if (!std::getline(ss, time_str, ',')) {LOG(ERROR) << "[LoadSensorData]: Find a bad line in the encoder file.: " << line_str;return false;}time_data_map->emplace(time_str, line_str);}//使用迭代器查询unordered_map中的元素// for (auto it = time_data_map->begin(); it != time_data_map->end();++it){//     std::cout << "it->first" << it->first << "it->second" << it->second//               << std::endl;// }return true;
}

The ‘sensor_data/data_stamp.csv’ file stores the names and timestamps of all sensor data in order.

’sensor_data/data_stamp.csv’:所有传感器的时间戳和名字

参考文献：https://irap.kaist.ac.kr/dataset/dataformat.html

https://github.com/ydsf16/TinyGrapeKit

多传感器融合定位（GPS+Wheel+camera）（1）-读取传感器数据相关推荐

多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...
多传感器融合定位十五-多传感器时空标定(综述)
多传感器融合定位十五-多传感器时空标定 1. 多传感器标定简介 1.1 标定内容及方法 1.2 讲解思路 2. 内参标定 2.1 雷达内参标定 2.2 IMU内参标定 2.3 编码器内参标定 2.4 ...
多传感器融合定位（4-基于滤波的2融合方法）2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析本次作业摘自张松鹏大哥的优秀作业代码下载 https://github.com ...
《多传感器融合定位》惯性导航基础（二）
续: <多传感器融合定位>惯性导航基础(一). 惯性导航基础习题二四.一组对自定义数据进行惯性导航解算验证 1.使用gnss-ins-sim仿真imu运动数据 2.对自定义运动惯性导航解 ...
多传感器融合定位第八章基于滤波的融合方法进阶
多传感器融合定位第八章基于滤波的融合方法进阶参考博客:深蓝学院-多传感器融合定位-第8章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
多传感器融合定位第六章惯性导航结算及误差模型
多传感器融合定位第六章惯性导航结算及误差模型参考博客:深蓝学院-多传感器融合定位-第6章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
【多传感器融合定位】【从零开始做自动驾驶定位_任佬】【所学到的东西汇总】
[多传感器融合定位][从零开始做自动驾驶定位_任佬][所学到的东西汇总] 0 前言 1 开篇 1.1 代码工程的运行 2 数据集 3 软件框架 3.1 运行 3.2 学到的 3.2.1 对消息的订阅和 ...
多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合多传感器融合定位理论基础文章跳转: 多传感器融合定位一-3D激光里 ...
多传感器融合定位十四-基于图优化的定位方法
多传感器融合定位十四-基于图优化的定位方法 1. 基于图优化的定位简介 1.1 核心思路 1.2 定位流程 2. 边缘化原理及应用 2.1 边缘化原理 2.2 从滤波角度理解边缘化 3. 基于kitt ...

多传感器融合定位（GPS+Wheel+camera）（1）-读取传感器数据

文章目录

1、读取Kaist数据集到融合系统中

多传感器融合定位（GPS+Wheel+camera）（1）-读取传感器数据相关推荐

最新文章

热门文章