| 48 | } |
| 49 | |
| 50 | std::vector<vio_360::IMUData> LoadIMUData(const std::string& filepath) { |
| 51 | std::vector<vio_360::IMUData> imu_data; |
| 52 | std::ifstream file(filepath); |
| 53 | |
| 54 | if (!file.is_open()) { |
| 55 | LOG_ERROR("Failed to open {}", filepath); |
| 56 | return imu_data; |
| 57 | } |
| 58 | |
| 59 | std::string line; |
| 60 | std::getline(file, line); // Skip header |
| 61 | |
| 62 | while (std::getline(file, line)) { |
| 63 | if (line.empty()) continue; |
| 64 | |
| 65 | std::stringstream ss(line); |
| 66 | std::string item; |
| 67 | std::vector<std::string> tokens; |
| 68 | |
| 69 | while (std::getline(ss, item, ',')) { |
| 70 | tokens.push_back(item); |
| 71 | } |
| 72 | |
| 73 | if (tokens.size() != 7) continue; |
| 74 | |
| 75 | try { |
| 76 | vio_360::IMUData imu; |
| 77 | imu.timestamp = std::stod(tokens[0]); |
| 78 | imu.ax = std::stof(tokens[1]); |
| 79 | imu.ay = std::stof(tokens[2]); |
| 80 | imu.az = std::stof(tokens[3]); |
| 81 | imu.gx = std::stof(tokens[4]); |
| 82 | imu.gy = std::stof(tokens[5]); |
| 83 | imu.gz = std::stof(tokens[6]); |
| 84 | imu_data.push_back(imu); |
| 85 | } catch (...) {} |
| 86 | } |
| 87 | |
| 88 | return imu_data; |
| 89 | } |
| 90 | |
| 91 | std::vector<vio_360::IMUData> GetIMUBetweenTimestamps( |
| 92 | const std::vector<vio_360::IMUData>& all_imu_data, |