From 0f16cadb6068ae7e6a7cbfe140a53b4ae87e7999 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 21 May 2025 16:06:56 -0400 Subject: [PATCH 1/5] EuRoC example with combined IMU --- examples/EurocDataExample.cpp | 478 ++++++++++++++++++++++++++++++++++ 1 file changed, 478 insertions(+) create mode 100644 examples/EurocDataExample.cpp diff --git a/examples/EurocDataExample.cpp b/examples/EurocDataExample.cpp new file mode 100644 index 0000000000..a6041d8dfa --- /dev/null +++ b/examples/EurocDataExample.cpp @@ -0,0 +1,478 @@ +/** + * @file EurocDataExample.cpp + * @brief Example evaluating IMU Preintegration consistency (15-DOF) on EuRoC dataset. + * Mimics the evaluation methodology from Section V-B of + * Fornasier et al., "Equivariant IMU Preintegration with Biases: + * a Galilean Group Approach" (arXiv:2411.05548v4), calculating + * 15-DOF NEES for NavState + Bias. + * @author Matt Kielo, Porter Zach + */ + + #include + #include + #include + #include + + #include + #include + + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + // Define custom types needed for this example + namespace gtsam { + typedef Eigen::Matrix Vector15; + // Define Matrix15x15 type for convenience, avoiding conflict with GTSAM's Matrix15 (1x5) + typedef Eigen::Matrix Matrix15x15; + } + + using namespace gtsam; + using namespace std; + + // --- Configuration --- + // Note: For accurate comparison with paper's Table 1, this path should eventually point + // to the 'mav0' directory INSIDE a specific EuRoC sequence (e.g., MH_01_easy/mav0). + // Using the path structure from user's previous run for now. + const string euroc_dataset_path = "C:/Users/porte/Desktop/projects/works25/euroc/data/MH_01_easy"; // Base path + const string imu_csv_path = euroc_dataset_path + "/mav0/imu0/data.csv"; + const string ground_truth_csv_path = euroc_dataset_path + "/mav0/state_groundtruth_estimate0/data.csv"; + + // Preintegration interval duration (seconds) - match paper's columns + const double deltaTij = 0.5; // Options: 0.2, 0.5, 1.0 + + // --- EuRoC Noise Parameters (VI Sensor From Dataset Spec) --- + // These are the *NOISE DENSITIES* from the EuRoC VI sensor spec (sensor.yaml) + // Units are per sqrt(Hz). Verify these against the sensor.yaml for your specific sequence. + const double gyro_noise_density = 1.6968e-04; // [rad/s/sqrt(Hz)] + const double accel_noise_density = 2.0000e-03; // [m/s^2/sqrt(Hz)] + // These are the *RANDOM WALK* densities + const double gyro_bias_rw_density = 1.9393e-05; // [rad/s^2/sqrt(Hz)] = rad/s/sqrt(Hz)/s + const double accel_bias_rw_density = 3.0000e-03; // [m/s^3/sqrt(Hz)] = m/s^2/sqrt(Hz)/s + + // Integration noise sigma (usually very small or zero) + const double integration_noise_sigma = 1e-8; + + // Gravity vector in navigation frame (ENU convention: Z-up, gravity is negative Z) + const Vector3 gravity_n(0, 0, -9.81); + + // --- Data Structures --- + + struct ImuData { + double timestamp; // seconds + Vector3 omega; // rad/s (gyro) + Vector3 acc; // m/s^2 (accel) + }; + + struct GroundTruthData { + double timestamp; // seconds + NavState navState; + imuBias::ConstantBias bias; + }; + + // --- Helper Functions --- + + // Function to parse IMU CSV line + bool parseImuLine(const string& line, ImuData& data) { + stringstream ss(line); + string segment; + vector seglist; + + while (getline(ss, segment, ',')) { + seglist.push_back(segment); + } + + if (seglist.size() < 7) return false; + + try { + data.timestamp = stod(seglist[0]) * 1e-9; // ns to s + data.omega = Vector3(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); + data.acc = Vector3(stod(seglist[4]), stod(seglist[5]), stod(seglist[6])); + } catch (const std::invalid_argument& e) { + cerr << "Error parsing IMU line (invalid_argument): " << line << endl; + return false; + } catch (const std::out_of_range& e) { + cerr << "Error parsing IMU line (out_of_range): " << line << endl; + return false; + } + return true; + } + + // Function to parse Ground Truth CSV line + bool parseGroundTruthLine(const string& line, GroundTruthData& data) { + stringstream ss(line); + string segment; + vector seglist; + + while (getline(ss, segment, ',')) { + seglist.push_back(segment); + } + + if (seglist.size() < 17) return false; + + try { + data.timestamp = stod(seglist[0]) * 1e-9; // ns to s + Point3 pos(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); + // Quaternion order in EuRoC: w, x, y, z + Quaternion q(stod(seglist[4]), stod(seglist[5]), stod(seglist[6]), stod(seglist[7])); + Rot3 rot = Rot3(q); + Velocity3 vel(stod(seglist[8]), stod(seglist[9]), stod(seglist[10])); + data.navState = NavState(rot, pos, vel); + + Vector3 bias_gyro(stod(seglist[11]), stod(seglist[12]), stod(seglist[13])); + Vector3 bias_acc(stod(seglist[14]), stod(seglist[15]), stod(seglist[16])); + data.bias = imuBias::ConstantBias(bias_acc, bias_gyro); + } catch (const std::invalid_argument& e) { + cerr << "Error parsing Ground Truth line (invalid_argument): " << line << endl; + return false; + } catch (const std::out_of_range& e) { + cerr << "Error parsing Ground Truth line (out_of_range): " << line << endl; + return false; + } + return true; + } + + // Function to load data from CSV + template + bool loadData(const string& filename, vector& data, bool (*parseFunc)(const string&, T&)) { + ifstream file(filename); + if (!file.is_open()) { + cerr << "Error opening file: " << filename << endl; + return false; + } + + string line; + // Skip header line + if (!getline(file, line)) { + cerr << "Error reading header or empty file: " << filename << endl; + return false; + } + + while (getline(file, line)) { + // Skip empty lines or lines starting with '#' (sometimes present) + if (line.empty() || line[0] == '#') continue; + + T entry; + if (parseFunc(line, entry)) { + data.push_back(entry); + } + } + cout << "Loaded " << data.size() << " valid entries from " << filename << endl; + if (data.empty()) { + cerr << "Warning: No valid data loaded from " << filename << endl; + } + return !data.empty(); + } + + // Find index of the data point at or just before the given timestamp + template + size_t findIndexBefore(const vector& sorted_data, double timestamp) { + // Find the first element NOT less than timestamp + auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, + [](const T& element, double value) { + return element.timestamp < value; + }); + + // If iterator points to the beginning, it means timestamp is before or equal to the first element. + if (it == sorted_data.begin()) { + // If the first element's timestamp is >= target, return 0. Otherwise, timestamp is before all data. + if (it != sorted_data.end() && it->timestamp >= timestamp) { + return 0; + } else { + cerr << "Warning: Timestamp " << fixed << setprecision(9) << timestamp + << " is before the first data point " << sorted_data.front().timestamp << endl; + return 0; // Return 0, but be aware it might be before the range + } + } else { + // Iterator points to the first element >= timestamp. We want the one before it. + return distance(sorted_data.begin(), it) - 1; + } + } + + + // Find index of the first data point at or after the given timestamp + template + size_t findIndexAtOrAfter(const vector& sorted_data, double timestamp) { + // Find the first element NOT less than timestamp + auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, + [](const T& element, double value) { + return element.timestamp < value; + }); + // Return the index of this element + return distance(sorted_data.begin(), it); + } + + // Function to calculate the median of a vector + double calculateMedian(vector& values) { + if (values.empty()) { + return numeric_limits::quiet_NaN(); // Return NaN for empty vector + } + // Use nth_element for efficient median calculation without full sort + size_t n = values.size(); + size_t mid = n / 2; + nth_element(values.begin(), values.begin() + mid, values.end()); + + if (n % 2 != 0) { // Odd size + return values[mid]; + } else { // Even size + // Find element just before mid for even case + // Need the value at mid and the max value in the lower half + double mid_val1 = values[mid]; + nth_element(values.begin(), values.begin() + mid - 1, values.end()); + return (values[mid - 1] + mid_val1) / 2.0; + } + } + + // --- Main Evaluation Logic --- + int main(int argc, char* argv[]) { + + cout << "Starting EuRoC Preintegration Evaluation (15-DOF NEES)..." << endl; + cout << "Dataset path: " << euroc_dataset_path << endl; + cout << "Preintegration Interval (deltaTij): " << deltaTij << " s" << endl; + + // 1. Load Data + vector imu_data; + vector gt_data; + + if (!loadData(imu_csv_path, imu_data, parseImuLine)) return 1; + if (!loadData(ground_truth_csv_path, gt_data, parseGroundTruthLine)) return 1; + + if (imu_data.empty() || gt_data.empty()) { + cerr << "IMU or Ground Truth data is empty after loading." << endl; + return 1; + } + + // 2. Setup Preintegration Parameters + // These are calculated from densities assuming a nominal IMU dt. + const double nominal_imu_dt = 0.005; // Assuming 200 Hz + + // Parameters for Combined Preintegration + auto p_combined = PreintegrationCombinedParams::MakeSharedU(gravity_n.norm()); + p_combined->gyroscopeCovariance = pow(gyro_noise_density, 2) / nominal_imu_dt * I_3x3; + p_combined->accelerometerCovariance = pow(accel_noise_density, 2) / nominal_imu_dt * I_3x3; + p_combined->integrationCovariance = pow(integration_noise_sigma, 2) * I_3x3; + p_combined->biasAccCovariance = pow(accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; + p_combined->biasOmegaCovariance = pow(gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; + p_combined->biasAccOmegaInt = I_6x6 * 1e-5; // Example value + p_combined->n_gravity = gravity_n; + + // Optionally print parameters + // cout << "\nUsing Combined Preintegration Parameters:" << endl; + // p_combined->print("Params_Combined"); + + // 3. Process Data in Intervals + vector nees_results_combined; + vector time_diffs; // Store actual integrated time intervals + + double min_time = max(imu_data.front().timestamp, gt_data.front().timestamp); + double max_time = min(imu_data.back().timestamp, gt_data.back().timestamp); + + // Adjust start time slightly to ensure first GT lookup is valid + size_t first_valid_gt_idx = findIndexAtOrAfter(gt_data, min_time); + if (first_valid_gt_idx >= gt_data.size()) { + cerr << "Error: No ground truth data found at or after minimum start time." << endl; + return 1; + } + min_time = gt_data[first_valid_gt_idx].timestamp; + + cout << fixed << setprecision(9); // Use higher precision for timestamps + cout << "Processing data from " << min_time << " s to " << max_time << " s" << endl; + + double current_t_i = min_time; + int skipped_intervals_gt = 0; + int skipped_intervals_imu = 0; + int numerical_issue_count = 0; + int processed_intervals = 0; + const int max_debug_prints = 5; // How many intervals to print debug info for + + while (current_t_i + deltaTij <= max_time) { + double t_j_target = current_t_i + deltaTij; + + // --- Get Ground Truth Data at t_i --- + size_t gt_idx_i = findIndexAtOrAfter(gt_data, current_t_i); + if (gt_idx_i >= gt_data.size() || gt_data[gt_idx_i].timestamp > current_t_i + 0.01) { + current_t_i += deltaTij; + skipped_intervals_gt++; + continue; + } + const GroundTruthData& gt_i = gt_data[gt_idx_i]; + current_t_i = gt_i.timestamp; + t_j_target = current_t_i + deltaTij; + + // --- Get Ground Truth Data at t_j --- + size_t gt_idx_j = findIndexBefore(gt_data, t_j_target); + if (gt_idx_j <= gt_idx_i || gt_idx_j >= gt_data.size() || gt_data[gt_idx_j].timestamp < current_t_i) { + current_t_i += deltaTij; + skipped_intervals_gt++; + continue; + } + const GroundTruthData& gt_j = gt_data[gt_idx_j]; + double actual_t_j = gt_j.timestamp; + + // --- Initialize Preintegrators --- + PreintegratedCombinedMeasurements pim_combined(p_combined, gt_i.bias); + + // --- Integrate IMU Measurements --- + size_t imu_idx_start = findIndexAtOrAfter(imu_data, current_t_i); + size_t imu_idx_end = findIndexAtOrAfter(imu_data, actual_t_j); + + if (imu_idx_start >= imu_data.size() || imu_idx_start >= imu_idx_end ) { + current_t_i += deltaTij; + skipped_intervals_imu++; + continue; + } + + double previous_imu_t = current_t_i; + for (size_t k = imu_idx_start; k < imu_idx_end; ++k) { + const ImuData& imu = imu_data[k]; + double current_imu_t = imu.timestamp; + double dt = current_imu_t - previous_imu_t; + + if (dt <= 1e-9) { + previous_imu_t = current_imu_t; + continue; + } + + // Integrate PIM(s) + pim_combined.integrateMeasurement(imu.acc, imu.omega, dt); + + previous_imu_t = current_imu_t; + } + + double final_dt = actual_t_j - previous_imu_t; + if (final_dt > 1e-9 && imu_idx_end > imu_idx_start) { + const ImuData& last_imu = imu_data[imu_idx_end - 1]; + pim_combined.integrateMeasurement(last_imu.acc, last_imu.omega, final_dt); + } + + // Store the actual integrated time duration + time_diffs.push_back(pim_combined.deltaTij()); + + if (pim_combined.deltaTij() < 1e-6) { + current_t_i += deltaTij; + skipped_intervals_imu++; + continue; + } + + // --- Predict State and Calculate Errors --- + NavState estimated_state_j_combined = pim_combined.predict(gt_i.navState, gt_i.bias); + + Vector9 error_nav_combined = gt_j.navState.localCoordinates(estimated_state_j_combined); + + Vector6 error_bias = gt_j.bias.vector() - gt_i.bias.vector(); + + Vector15 error15_combined; + error15_combined << error_nav_combined, error_bias; + + // --- Get Covariances --- + Matrix15x15 P15_combined = pim_combined.preintMeasCov(); + + // Extract covariance(s) + bool extraction_ok = true; + + // Check Combined Covariance size + if (P15_combined.rows() != 15 || P15_combined.cols() != 15) { + cerr << "Error: Expected 15x15 Combined covariance matrix, got " + << P15_combined.rows() << "x" << P15_combined.cols() << " at t_i=" << current_t_i << endl; + numerical_issue_count++; + extraction_ok = false; // Skip NEES calculation if cov is bad + } + + // --- Calculate 15-DOF NEES if covariance(s) are valid --- + if (extraction_ok) { + try { + // Regularize and Invert Combined Covariance + Matrix15x15 P15_combined_reg = P15_combined + Matrix15x15::Identity() * 1e-9; + Matrix15x15 P15_combined_inv = P15_combined_reg.inverse(); + double nees15_combined = error15_combined.transpose() * P15_combined_inv * error15_combined; + + if (!isnan(nees15_combined) && !isinf(nees15_combined) && nees15_combined >= 0) { + nees_results_combined.push_back(nees15_combined); + } else { + numerical_issue_count++; + } + } catch (const std::exception& e) { + numerical_issue_count++; + } + + // Only increment processed intervals if NEES calculations were attempted without prior errors + processed_intervals++; + + } else { + // Covariance extraction failed, skip NEES and move to next interval + current_t_i += deltaTij; + continue; + } + + // Move to the start of the next non-overlapping interval + current_t_i += deltaTij; + + } // end while loop over intervals + + // 4. Calculate Median NEES and other stats + cout << "\nProcessed " << processed_intervals << " valid intervals." << endl; + cout << "Skipped " << skipped_intervals_gt << " intervals due to GT data gaps." << endl; + cout << "Skipped " << skipped_intervals_imu << " intervals due to IMU data gaps." << endl; + cout << "Skipped " << numerical_issue_count << " NEES calculations due to numerical/extraction issues." << endl; + + if (nees_results_combined.empty()) { + cerr << "No valid NEES results were calculated for one or both methods." << endl; + cerr << "Combined NEES count: " << nees_results_combined.size() << endl; + return 1; + } + + // Calculate Combined (Baseline) Stats + double median_nees_combined = calculateMedian(nees_results_combined); + double mean_nees_combined = 0; + double nees_sum_sq_combined = 0; + int valid_nees_count_combined = 0; + for(double n : nees_results_combined) { + if (isfinite(n)) { + mean_nees_combined += n; + nees_sum_sq_combined += n*n; + valid_nees_count_combined++; + } + } + double nees_std_dev_combined = 0; + if (valid_nees_count_combined > 0) { + mean_nees_combined /= valid_nees_count_combined; + if (valid_nees_count_combined > 1) { + nees_std_dev_combined = sqrt((nees_sum_sq_combined - (mean_nees_combined * mean_nees_combined * valid_nees_count_combined)) / (valid_nees_count_combined - 1)); + } + } + + // Calculate Time Stats + double median_dt = calculateMedian(time_diffs); + double mean_dt = 0; + int valid_dt_count = 0; + for(double d : time_diffs) { + if (isfinite(d)) { + mean_dt += d; + valid_dt_count++; + } + } + if (valid_dt_count > 0) mean_dt /= valid_dt_count; + + cout << "\n--- Results (15-DOF NEES) ---" << endl; + cout << "Dataset: " << euroc_dataset_path << endl; + cout << "Target Preintegration Interval (deltaTij): " << fixed << setprecision(5) << deltaTij << " s" << endl; + cout << "Actual Median Integrated Interval: " << fixed << setprecision(5) << median_dt << " s" << endl; + cout << "Actual Mean Integrated Interval: " << fixed << setprecision(5) << mean_dt << " s" << endl; + + cout << "\n--- Combined Preintegration (Baseline) ---" << endl; + cout << "Number of Valid NEES Intervals: " << valid_nees_count_combined << endl; + cout << "Median 15-DOF NEES: " << fixed << setprecision(3) << median_nees_combined << endl; + cout << "Mean 15-DOF NEES: " << fixed << setprecision(3) << mean_nees_combined << endl; + cout << "StdDev 15-DOF NEES: " << fixed << setprecision(3) << nees_std_dev_combined << endl; + cout << "--------------------------------------------" << endl; + + return 0; + } + \ No newline at end of file From 2a920cd33a04434d1472e5fdd8972db3af15720f Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 21 May 2025 16:08:45 -0400 Subject: [PATCH 2/5] Table output like Delama et al --- examples/EurocDataExample.cpp | 893 ++++++++++++++++++++++------------ 1 file changed, 582 insertions(+), 311 deletions(-) diff --git a/examples/EurocDataExample.cpp b/examples/EurocDataExample.cpp index a6041d8dfa..0937f37b7d 100644 --- a/examples/EurocDataExample.cpp +++ b/examples/EurocDataExample.cpp @@ -6,15 +6,17 @@ * a Galilean Group Approach" (arXiv:2411.05548v4), calculating * 15-DOF NEES for NavState + Bias. * @author Matt Kielo, Porter Zach + * @modifier AI Assistant (Refactoring and Enhancements) */ #include #include - #include - #include + #include + #include #include #include + #include // For I_3x3, I_6x6 #include #include @@ -23,9 +25,10 @@ #include #include #include - #include - #include - #include + #include + #include + #include + #include // For results table // Define custom types needed for this example namespace gtsam { @@ -38,24 +41,26 @@ using namespace std; // --- Configuration --- - // Note: For accurate comparison with paper's Table 1, this path should eventually point - // to the 'mav0' directory INSIDE a specific EuRoC sequence (e.g., MH_01_easy/mav0). - // Using the path structure from user's previous run for now. - const string euroc_dataset_path = "C:/Users/porte/Desktop/projects/works25/euroc/data/MH_01_easy"; // Base path - const string imu_csv_path = euroc_dataset_path + "/mav0/imu0/data.csv"; - const string ground_truth_csv_path = euroc_dataset_path + "/mav0/state_groundtruth_estimate0/data.csv"; - - // Preintegration interval duration (seconds) - match paper's columns - const double deltaTij = 0.5; // Options: 0.2, 0.5, 1.0 - - // --- EuRoC Noise Parameters (VI Sensor From Dataset Spec) --- - // These are the *NOISE DENSITIES* from the EuRoC VI sensor spec (sensor.yaml) - // Units are per sqrt(Hz). Verify these against the sensor.yaml for your specific sequence. - const double gyro_noise_density = 1.6968e-04; // [rad/s/sqrt(Hz)] - const double accel_noise_density = 2.0000e-03; // [m/s^2/sqrt(Hz)] - // These are the *RANDOM WALK* densities - const double gyro_bias_rw_density = 1.9393e-05; // [rad/s^2/sqrt(Hz)] = rad/s/sqrt(Hz)/s - const double accel_bias_rw_density = 3.0000e-03; // [m/s^3/sqrt(Hz)] = m/s^2/sqrt(Hz)/s + // Base path for EuRoC datasets. Individual dataset folders (e.g., MH_01) should be inside this. + const string base_euroc_data_path = "C:/Users/porte/Desktop/projects/works25/euroc/data"; + + // List of datasets to process + const vector dataset_sequences = { + "MH_01", + // "MH_02", + // "MH_03", + // "MH_04", + // "MH_05", + // "V1_01", + // "V1_02", + // "V1_03", + // "V2_01", + "V2_02" + // "V2_03" // This one has different GT format (no bias) + }; + + // List of preintegration interval durations (seconds) + const vector deltaTij_values = {0.2, 0.5, 1.0}; // Integration noise sigma (usually very small or zero) const double integration_noise_sigma = 1e-8; @@ -63,6 +68,7 @@ // Gravity vector in navigation frame (ENU convention: Z-up, gravity is negative Z) const Vector3 gravity_n(0, 0, -9.81); + // --- Data Structures --- struct ImuData { @@ -77,69 +83,208 @@ imuBias::ConstantBias bias; }; - // --- Helper Functions --- + struct SensorParams { + double gyro_noise_density = 0.0; + double gyro_bias_rw_density = 0.0; + double accel_noise_density = 0.0; + double accel_bias_rw_density = 0.0; + Pose3 body_P_sensor; // T_BS: IMU frame in body frame + double rate_hz = 200.0; // Default, will be overridden by YAML if present + }; + + struct IntervalEvaluationResult { + double nees = numeric_limits::quiet_NaN(); + bool success = false; + double actual_dt = 0.0; + }; + + struct AggregatedRunResults { + string dataset_name; + double deltaTij_target = 0.0; + + double median_nees = numeric_limits::quiet_NaN(); + double mean_nees = numeric_limits::quiet_NaN(); + double std_dev_nees = numeric_limits::quiet_NaN(); + + double median_actual_dt = numeric_limits::quiet_NaN(); + double mean_actual_dt = numeric_limits::quiet_NaN(); + + int processed_intervals = 0; + int skipped_intervals_gt = 0; + int skipped_intervals_imu = 0; + int numerical_issue_count = 0; + + void print_summary() const { + cout << " Summary for " << dataset_name << " (deltaTij: " << fixed << setprecision(1) << deltaTij_target << "s):" << endl; + cout << " Processed Intervals: " << processed_intervals << endl; + if (skipped_intervals_gt > 0) cout << " Skipped (GT Gaps): " << skipped_intervals_gt << endl; + if (skipped_intervals_imu > 0) cout << " Skipped (IMU Gaps): " << skipped_intervals_imu << endl; + if (numerical_issue_count > 0) cout << " Skipped (Numerical Issues): " << numerical_issue_count << endl; + + if (processed_intervals > 0) { + cout << " Median Actual dt: " << fixed << setprecision(5) << median_actual_dt << " s" << endl; + cout << " Mean Actual dt: " << fixed << setprecision(5) << mean_actual_dt << " s" << endl; + cout << " Median 15-DOF NEES: " << fixed << setprecision(3) << median_nees << endl; + cout << " Mean 15-DOF NEES: " << fixed << setprecision(3) << mean_nees << endl; + cout << " StdDev 15-DOF NEES: " << fixed << setprecision(3) << std_dev_nees << endl; + } else { + cout << " No NEES results to report." << endl; + } + cout << " -----------------------------------" << endl; + } + }; + + + // --- String Helper --- + string trim(const string& str, const string& whitespace = " \t\n\r\f\v") { + const auto strBegin = str.find_first_not_of(whitespace); + if (strBegin == string::npos) return ""; // no content + const auto strEnd = str.find_last_not_of(whitespace); + const auto strRange = strEnd - strBegin + 1; + return str.substr(strBegin, strRange); + } + + // --- YAML Parsing Helper --- + bool parseSensorYaml(const string& yaml_path, SensorParams& params) { + ifstream file(yaml_path); + if (!file.is_open()) { + cerr << "Error opening sensor YAML file: " << yaml_path << endl; + return false; + } + + string line; + Matrix4 T_BS_mat = Matrix4::Identity(); + bool t_bs_data_mode = false; + vector t_bs_coeffs; + + auto extract_value = [&](const string& l, const string& key) { + size_t key_pos = l.find(key); + if (key_pos != string::npos) { + string val_str = l.substr(key_pos + key.length()); + return trim(val_str); + } + return string(""); + }; + + while (getline(file, line)) { + line = trim(line); + if (line.empty() || line[0] == '#') continue; + + if (t_bs_data_mode) { + stringstream ss_line(line); + string val_token; + while(getline(ss_line, val_token, ',')){ + val_token.erase(remove(val_token.begin(), val_token.end(), '['), val_token.end()); + val_token.erase(remove(val_token.begin(), val_token.end(), ']'), val_token.end()); + val_token = trim(val_token); + if(!val_token.empty()){ + try { + t_bs_coeffs.push_back(stod(val_token)); + } catch (const std::exception& e) { + cerr << "Warning: Could not parse T_BS data value: " << val_token << endl; + } + } + } + if (t_bs_coeffs.size() >= 16) { + t_bs_data_mode = false; // Done collecting + } + } else { + string val_str; + val_str = extract_value(line, "gyroscope_noise_density:"); + if (!val_str.empty()) params.gyro_noise_density = stod(val_str); + + val_str = extract_value(line, "gyroscope_random_walk:"); + if (!val_str.empty()) params.gyro_bias_rw_density = stod(val_str); + + val_str = extract_value(line, "accelerometer_noise_density:"); + if (!val_str.empty()) params.accel_noise_density = stod(val_str); + + val_str = extract_value(line, "accelerometer_random_walk:"); + if (!val_str.empty()) params.accel_bias_rw_density = stod(val_str); + + val_str = extract_value(line, "rate_hz:"); + if (!val_str.empty()) params.rate_hz = stod(val_str); + + if (line.find("T_BS:") != string::npos) { + // Next lines might be rows, cols, then data + } else if (line.find("data:") != string::npos && t_bs_coeffs.empty()) { + t_bs_data_mode = true; + // Remove "data:" part and process rest of the line + line = line.substr(line.find("data:") + 5); + stringstream ss_line(line); + string val_token; + while(getline(ss_line, val_token, ',')){ + val_token.erase(remove(val_token.begin(), val_token.end(), '['), val_token.end()); + val_token.erase(remove(val_token.begin(), val_token.end(), ']'), val_token.end()); + val_token = trim(val_token); + if(!val_token.empty()){ + try { + t_bs_coeffs.push_back(stod(val_token)); + } catch (const std::exception& e) { + cerr << "Warning: Could not parse T_BS data value: " << val_token << endl; + } + } + } + } + } + } + + if (t_bs_coeffs.size() >= 16) { + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + T_BS_mat(i, j) = t_bs_coeffs[i * 4 + j]; + } + } + params.body_P_sensor = Pose3(T_BS_mat); + } else { + cout << "Warning: T_BS data not fully parsed or missing from YAML. Using Identity." << endl; + params.body_P_sensor = Pose3(); // Identity + } + + // Basic check if critical params were loaded + if (params.gyro_noise_density == 0.0 || params.accel_noise_density == 0.0) { + cerr << "Warning: Some noise densities are zero after parsing YAML. Check file content." << endl; + // Allow continuation, but this is usually an issue. + } + return true; + } + + // --- Helper Functions (from original, largely unchanged) --- - // Function to parse IMU CSV line bool parseImuLine(const string& line, ImuData& data) { stringstream ss(line); string segment; vector seglist; - - while (getline(ss, segment, ',')) { - seglist.push_back(segment); - } - + while (getline(ss, segment, ',')) seglist.push_back(segment); if (seglist.size() < 7) return false; - try { data.timestamp = stod(seglist[0]) * 1e-9; // ns to s data.omega = Vector3(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); data.acc = Vector3(stod(seglist[4]), stod(seglist[5]), stod(seglist[6])); - } catch (const std::invalid_argument& e) { - cerr << "Error parsing IMU line (invalid_argument): " << line << endl; - return false; - } catch (const std::out_of_range& e) { - cerr << "Error parsing IMU line (out_of_range): " << line << endl; - return false; - } + } catch (const std::exception& e) { return false; } return true; } - // Function to parse Ground Truth CSV line bool parseGroundTruthLine(const string& line, GroundTruthData& data) { stringstream ss(line); string segment; vector seglist; - - while (getline(ss, segment, ',')) { - seglist.push_back(segment); - } - - if (seglist.size() < 17) return false; - + while (getline(ss, segment, ',')) seglist.push_back(segment); + if (seglist.size() < 17) return false; // EuRoC GT has 17 fields (timestamp, p, q, v, bias_w, bias_a) try { data.timestamp = stod(seglist[0]) * 1e-9; // ns to s Point3 pos(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); - // Quaternion order in EuRoC: w, x, y, z - Quaternion q(stod(seglist[4]), stod(seglist[5]), stod(seglist[6]), stod(seglist[7])); + Quaternion q(stod(seglist[4]), stod(seglist[5]), stod(seglist[6]), stod(seglist[7])); // w, x, y, z Rot3 rot = Rot3(q); Velocity3 vel(stod(seglist[8]), stod(seglist[9]), stod(seglist[10])); data.navState = NavState(rot, pos, vel); - Vector3 bias_gyro(stod(seglist[11]), stod(seglist[12]), stod(seglist[13])); Vector3 bias_acc(stod(seglist[14]), stod(seglist[15]), stod(seglist[16])); data.bias = imuBias::ConstantBias(bias_acc, bias_gyro); - } catch (const std::invalid_argument& e) { - cerr << "Error parsing Ground Truth line (invalid_argument): " << line << endl; - return false; - } catch (const std::out_of_range& e) { - cerr << "Error parsing Ground Truth line (out_of_range): " << line << endl; - return false; - } + } catch (const std::exception& e) { return false; } return true; } - // Function to load data from CSV template bool loadData(const string& filename, vector& data, bool (*parseFunc)(const string&, T&)) { ifstream file(filename); @@ -147,332 +292,458 @@ cerr << "Error opening file: " << filename << endl; return false; } - string line; - // Skip header line - if (!getline(file, line)) { - cerr << "Error reading header or empty file: " << filename << endl; - return false; - } - + if (!getline(file, line)) { /* Skip header */ } while (getline(file, line)) { - // Skip empty lines or lines starting with '#' (sometimes present) if (line.empty() || line[0] == '#') continue; - T entry; - if (parseFunc(line, entry)) { - data.push_back(entry); - } + if (parseFunc(line, entry)) data.push_back(entry); } cout << "Loaded " << data.size() << " valid entries from " << filename << endl; - if (data.empty()) { - cerr << "Warning: No valid data loaded from " << filename << endl; - } return !data.empty(); } - // Find index of the data point at or just before the given timestamp template size_t findIndexBefore(const vector& sorted_data, double timestamp) { - // Find the first element NOT less than timestamp auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, - [](const T& element, double value) { - return element.timestamp < value; - }); - - // If iterator points to the beginning, it means timestamp is before or equal to the first element. + [](const T& element, double value) { return element.timestamp < value; }); if (it == sorted_data.begin()) { - // If the first element's timestamp is >= target, return 0. Otherwise, timestamp is before all data. - if (it != sorted_data.end() && it->timestamp >= timestamp) { - return 0; - } else { - cerr << "Warning: Timestamp " << fixed << setprecision(9) << timestamp - << " is before the first data point " << sorted_data.front().timestamp << endl; - return 0; // Return 0, but be aware it might be before the range + if (it != sorted_data.end() && it->timestamp >= timestamp) return 0; + else { + // cerr << "Warning: Timestamp " << fixed << setprecision(9) << timestamp + // << " is before the first data point " << sorted_data.front().timestamp << endl; + return 0; } - } else { - // Iterator points to the first element >= timestamp. We want the one before it. - return distance(sorted_data.begin(), it) - 1; } + return distance(sorted_data.begin(), it) - 1; } - - // Find index of the first data point at or after the given timestamp template size_t findIndexAtOrAfter(const vector& sorted_data, double timestamp) { - // Find the first element NOT less than timestamp auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, - [](const T& element, double value) { - return element.timestamp < value; - }); - // Return the index of this element + [](const T& element, double value) { return element.timestamp < value; }); return distance(sorted_data.begin(), it); } - // Function to calculate the median of a vector - double calculateMedian(vector& values) { - if (values.empty()) { - return numeric_limits::quiet_NaN(); // Return NaN for empty vector - } - // Use nth_element for efficient median calculation without full sort + double calculateMedian(vector& values) { // Note: modifies values vector + if (values.empty()) return numeric_limits::quiet_NaN(); size_t n = values.size(); size_t mid = n / 2; nth_element(values.begin(), values.begin() + mid, values.end()); - - if (n % 2 != 0) { // Odd size - return values[mid]; - } else { // Even size - // Find element just before mid for even case - // Need the value at mid and the max value in the lower half - double mid_val1 = values[mid]; - nth_element(values.begin(), values.begin() + mid - 1, values.end()); + if (n % 2 != 0) return values[mid]; + else { + if (n == 0) return numeric_limits::quiet_NaN(); // Should be caught by empty() check + if (mid == 0) return values[mid]; // Only one element effectively if n=1, covered by n%2 !=0 + // For even size, ensure mid-1 is valid. nth_element for mid is done. + // Now find element at mid-1 for the other part of average. + // The values vector is partially sorted up to mid. + // We need the (mid-1)th element from the original sorted sequence. + double mid_val1 = values[mid]; // This is the (n/2)-th element (0-indexed) + // Find the maximum of elements before 'mid' (which is values[mid-1] after sorting up to mid-1) + nth_element(values.begin(), values.begin() + mid - 1, values.begin() + mid); // Sort within the lower half return (values[mid - 1] + mid_val1) / 2.0; } } - // --- Main Evaluation Logic --- - int main(int argc, char* argv[]) { + // --- Core Evaluation Logic --- - cout << "Starting EuRoC Preintegration Evaluation (15-DOF NEES)..." << endl; - cout << "Dataset path: " << euroc_dataset_path << endl; - cout << "Preintegration Interval (deltaTij): " << deltaTij << " s" << endl; - - // 1. Load Data - vector imu_data; - vector gt_data; + shared_ptr createCombinedImuParams( + const SensorParams& sensor_params, + const Vector3& gravity_n_vec, + double integration_sigma) { - if (!loadData(imu_csv_path, imu_data, parseImuLine)) return 1; - if (!loadData(ground_truth_csv_path, gt_data, parseGroundTruthLine)) return 1; - - if (imu_data.empty() || gt_data.empty()) { - cerr << "IMU or Ground Truth data is empty after loading." << endl; - return 1; + double nominal_imu_dt = 1.0 / sensor_params.rate_hz; + if (nominal_imu_dt <= 1e-9) { // Avoid division by zero if rate_hz is 0 + cerr << "Warning: IMU rate_hz is " << sensor_params.rate_hz << ", using default nominal_dt of 0.005s for covariance calculation." << endl; + nominal_imu_dt = 0.005; } - // 2. Setup Preintegration Parameters - // These are calculated from densities assuming a nominal IMU dt. - const double nominal_imu_dt = 0.005; // Assuming 200 Hz - - // Parameters for Combined Preintegration - auto p_combined = PreintegrationCombinedParams::MakeSharedU(gravity_n.norm()); - p_combined->gyroscopeCovariance = pow(gyro_noise_density, 2) / nominal_imu_dt * I_3x3; - p_combined->accelerometerCovariance = pow(accel_noise_density, 2) / nominal_imu_dt * I_3x3; - p_combined->integrationCovariance = pow(integration_noise_sigma, 2) * I_3x3; - p_combined->biasAccCovariance = pow(accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; - p_combined->biasOmegaCovariance = pow(gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; - p_combined->biasAccOmegaInt = I_6x6 * 1e-5; // Example value - p_combined->n_gravity = gravity_n; - - // Optionally print parameters - // cout << "\nUsing Combined Preintegration Parameters:" << endl; - // p_combined->print("Params_Combined"); - - // 3. Process Data in Intervals - vector nees_results_combined; - vector time_diffs; // Store actual integrated time intervals - - double min_time = max(imu_data.front().timestamp, gt_data.front().timestamp); - double max_time = min(imu_data.back().timestamp, gt_data.back().timestamp); - - // Adjust start time slightly to ensure first GT lookup is valid - size_t first_valid_gt_idx = findIndexAtOrAfter(gt_data, min_time); - if (first_valid_gt_idx >= gt_data.size()) { - cerr << "Error: No ground truth data found at or after minimum start time." << endl; - return 1; - } - min_time = gt_data[first_valid_gt_idx].timestamp; + shared_ptr p_base = + PreintegrationCombinedParams::MakeSharedU( + gravity_n_vec.norm() + ); + + shared_ptr p = + static_pointer_cast(p_base); - cout << fixed << setprecision(9); // Use higher precision for timestamps - cout << "Processing data from " << min_time << " s to " << max_time << " s" << endl; + p->n_gravity = gravity_n_vec; // Set actual gravity vector (overwrites default direction if magnitude was same) - double current_t_i = min_time; - int skipped_intervals_gt = 0; - int skipped_intervals_imu = 0; - int numerical_issue_count = 0; - int processed_intervals = 0; - const int max_debug_prints = 5; // How many intervals to print debug info for + p->gyroscopeCovariance = pow(sensor_params.gyro_noise_density, 2) / nominal_imu_dt * I_3x3; + p->accelerometerCovariance = pow(sensor_params.accel_noise_density, 2) / nominal_imu_dt * I_3x3; + p->integrationCovariance = pow(integration_sigma, 2) * I_3x3; + p->biasAccCovariance = pow(sensor_params.accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; + p->biasOmegaCovariance = pow(sensor_params.gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; + p->biasAccOmegaInt = I_6x6 * 1e-5; // Default, as in original script - while (current_t_i + deltaTij <= max_time) { - double t_j_target = current_t_i + deltaTij; + return p; + } - // --- Get Ground Truth Data at t_i --- - size_t gt_idx_i = findIndexAtOrAfter(gt_data, current_t_i); - if (gt_idx_i >= gt_data.size() || gt_data[gt_idx_i].timestamp > current_t_i + 0.01) { - current_t_i += deltaTij; - skipped_intervals_gt++; + IntervalEvaluationResult evaluateCombinedImuForInterval( + const GroundTruthData& gt_i, + const GroundTruthData& gt_j, + const vector& all_imu_data, // Full IMU data vector + size_t imu_idx_start_global, // Start index in all_imu_data + size_t imu_idx_end_global, // End index (exclusive) in all_imu_data + const shared_ptr& pim_params) { + + IntervalEvaluationResult result; + result.success = false; + + PreintegratedCombinedMeasurements pim_combined(pim_params, gt_i.bias); + + double previous_imu_t = gt_i.timestamp; // Start integration from gt_i timestamp + + for (size_t k = imu_idx_start_global; k < imu_idx_end_global; ++k) { + const ImuData& imu = all_imu_data[k]; + // Ensure IMU measurement is within [gt_i.timestamp, gt_j.timestamp] + // This check is mostly handled by findIndexAtOrAfter for start/end, but good for dt calc + if (imu.timestamp < gt_i.timestamp || imu.timestamp > gt_j.timestamp + 1e-9) { // Add small tolerance for gt_j.timestamp + // This case should be rare if indices are chosen carefully, + // but if it happens, adjust previous_imu_t to avoid large dt + if (imu.timestamp < previous_imu_t && imu.timestamp >= gt_i.timestamp) previous_imu_t = imu.timestamp; continue; } - const GroundTruthData& gt_i = gt_data[gt_idx_i]; - current_t_i = gt_i.timestamp; - t_j_target = current_t_i + deltaTij; - - // --- Get Ground Truth Data at t_j --- - size_t gt_idx_j = findIndexBefore(gt_data, t_j_target); - if (gt_idx_j <= gt_idx_i || gt_idx_j >= gt_data.size() || gt_data[gt_idx_j].timestamp < current_t_i) { - current_t_i += deltaTij; - skipped_intervals_gt++; - continue; - } - const GroundTruthData& gt_j = gt_data[gt_idx_j]; - double actual_t_j = gt_j.timestamp; - // --- Initialize Preintegrators --- - PreintegratedCombinedMeasurements pim_combined(p_combined, gt_i.bias); - // --- Integrate IMU Measurements --- - size_t imu_idx_start = findIndexAtOrAfter(imu_data, current_t_i); - size_t imu_idx_end = findIndexAtOrAfter(imu_data, actual_t_j); + double current_imu_t = imu.timestamp; + double dt = current_imu_t - previous_imu_t; - if (imu_idx_start >= imu_data.size() || imu_idx_start >= imu_idx_end ) { - current_t_i += deltaTij; - skipped_intervals_imu++; + if (dt < 1e-9) { // Effectively zero dt or out of order (should not happen with sorted data) + if (dt < 0) { /* cerr << "Warning: Negative IMU dt " << dt << endl; */ } + previous_imu_t = current_imu_t; continue; } + if (dt > 0.1) { // Warn about large dt, might indicate issues + // cerr << "Warning: Large IMU dt of " << dt << "s at t=" << current_imu_t << endl; + } - double previous_imu_t = current_t_i; - for (size_t k = imu_idx_start; k < imu_idx_end; ++k) { - const ImuData& imu = imu_data[k]; - double current_imu_t = imu.timestamp; - double dt = current_imu_t - previous_imu_t; - - if (dt <= 1e-9) { - previous_imu_t = current_imu_t; - continue; - } - // Integrate PIM(s) - pim_combined.integrateMeasurement(imu.acc, imu.omega, dt); + pim_combined.integrateMeasurement(imu.acc, imu.omega, dt); + previous_imu_t = current_imu_t; + } - previous_imu_t = current_imu_t; - } + // Integrate from the last IMU measurement up to gt_j.timestamp + double final_dt = gt_j.timestamp - previous_imu_t; + if (final_dt > 1e-9 && imu_idx_end_global > imu_idx_start_global && imu_idx_end_global <= all_imu_data.size()) { + // Use the last IMU measurement that was processed in the loop, which is all_imu_data[imu_idx_end_global - 1] + // if imu_idx_end_global points one past the last element to include. + const ImuData& last_imu_in_segment = all_imu_data[imu_idx_end_global -1]; + pim_combined.integrateMeasurement(last_imu_in_segment.acc, last_imu_in_segment.omega, final_dt); + } - double final_dt = actual_t_j - previous_imu_t; - if (final_dt > 1e-9 && imu_idx_end > imu_idx_start) { - const ImuData& last_imu = imu_data[imu_idx_end - 1]; - pim_combined.integrateMeasurement(last_imu.acc, last_imu.omega, final_dt); - } - // Store the actual integrated time duration - time_diffs.push_back(pim_combined.deltaTij()); + result.actual_dt = pim_combined.deltaTij(); + if (result.actual_dt < 1e-6) { // Too short interval + return result; + } - if (pim_combined.deltaTij() < 1e-6) { - current_t_i += deltaTij; - skipped_intervals_imu++; - continue; - } + NavState estimated_state_j = pim_combined.predict(gt_i.navState, gt_i.bias); + Vector9 error_nav = NavState::Logmap(gt_j.navState.inverse() * estimated_state_j); // More robust error for NavState + Vector6 error_bias = gt_j.bias.vector() - gt_i.bias.vector(); // Bias error is over the interval - // --- Predict State and Calculate Errors --- - NavState estimated_state_j_combined = pim_combined.predict(gt_i.navState, gt_i.bias); + Vector15 error15; + error15 << error_nav, error_bias; - Vector9 error_nav_combined = gt_j.navState.localCoordinates(estimated_state_j_combined); + Matrix15x15 P15 = pim_combined.preintMeasCov(); + if (P15.rows() != 15 || P15.cols() != 15) { + // cerr << "Error: Covariance matrix size mismatch." << endl; + return result; // Numerical issue + } - Vector6 error_bias = gt_j.bias.vector() - gt_i.bias.vector(); - - Vector15 error15_combined; - error15_combined << error_nav_combined, error_bias; + try { + Matrix15x15 P15_reg = P15 + Matrix15x15::Identity() * 1e-9; // Regularization + Matrix15x15 P15_inv = P15_reg.inverse(); + double nees_val = error15.transpose() * P15_inv * error15; - // --- Get Covariances --- - Matrix15x15 P15_combined = pim_combined.preintMeasCov(); + if (!isnan(nees_val) && !isinf(nees_val) && nees_val >= 0) { + result.nees = nees_val; + result.success = true; + } + } catch (const std::exception& e) { + // cerr << "Exception during NEES calculation: " << e.what() << endl; + return result; // Numerical issue + } + return result; + } - // Extract covariance(s) - bool extraction_ok = true; + void aggregateAndStoreResults( + const string& dataset_name, double deltaTij_target, + const vector& nees_values_list_const, // const to allow copying + const vector& actual_dt_list_const, // const to allow copying + int num_processed, int num_skipped_gt, int num_skipped_imu, int num_numerical_issues, + map>& final_results_table) { + + AggregatedRunResults agg_results; + agg_results.dataset_name = dataset_name; + agg_results.deltaTij_target = deltaTij_target; + agg_results.processed_intervals = num_processed; + agg_results.skipped_intervals_gt = num_skipped_gt; + agg_results.skipped_intervals_imu = num_skipped_imu; + agg_results.numerical_issue_count = num_numerical_issues; + + vector nees_values_list_mutable = nees_values_list_const; // mutable copy for median + vector actual_dt_list_mutable = actual_dt_list_const; // mutable copy for median + + if (!nees_values_list_const.empty()) { + agg_results.median_nees = calculateMedian(nees_values_list_mutable); // Modifies nees_values_list_mutable + + double sum_nees = 0; + double sum_sq_nees = 0; + for (double n : nees_values_list_const) { + sum_nees += n; + sum_sq_nees += n * n; + } + agg_results.mean_nees = sum_nees / nees_values_list_const.size(); + if (nees_values_list_const.size() > 1) { + // variance = E[X^2] - (E[X])^2 + double variance = (sum_sq_nees / nees_values_list_const.size()) - (agg_results.mean_nees * agg_results.mean_nees); + // Ensure variance is non-negative due to potential floating point inaccuracies + agg_results.std_dev_nees = sqrt(max(0.0, variance)); + } else { + agg_results.std_dev_nees = 0; + } + } - // Check Combined Covariance size - if (P15_combined.rows() != 15 || P15_combined.cols() != 15) { - cerr << "Error: Expected 15x15 Combined covariance matrix, got " - << P15_combined.rows() << "x" << P15_combined.cols() << " at t_i=" << current_t_i << endl; - numerical_issue_count++; - extraction_ok = false; // Skip NEES calculation if cov is bad + if (!actual_dt_list_const.empty()) { + agg_results.median_actual_dt = calculateMedian(actual_dt_list_mutable); + double sum_dt = 0; + for (double dt_val : actual_dt_list_const) { + sum_dt += dt_val; } + agg_results.mean_actual_dt = sum_dt / actual_dt_list_const.size(); + } + + final_results_table[dataset_name][deltaTij_target] = agg_results; + agg_results.print_summary(); + } - // --- Calculate 15-DOF NEES if covariance(s) are valid --- - if (extraction_ok) { - try { - // Regularize and Invert Combined Covariance - Matrix15x15 P15_combined_reg = P15_combined + Matrix15x15::Identity() * 1e-9; - Matrix15x15 P15_combined_inv = P15_combined_reg.inverse(); - double nees15_combined = error15_combined.transpose() * P15_combined_inv * error15_combined; + void printFinalTable(const map>& final_results_table, + const vector& datasets, const vector& deltas) { + cout << "\n\n--- Overall Results Table (Median 15-DOF NEES) ---" << endl; - if (!isnan(nees15_combined) && !isinf(nees15_combined) && nees15_combined >= 0) { - nees_results_combined.push_back(nees15_combined); + // Header + cout << setw(20) << left << "Dataset"; + for (double dt_val : deltas) { + stringstream ss; + ss << "dT=" << fixed << setprecision(1) << dt_val << "s"; + cout << setw(12) << right << ss.str(); + } + cout << endl; + cout << string(20 + deltas.size() * 12, '-') << endl; + + // Data rows + for (const string& dataset_name : datasets) { + cout << setw(20) << left << dataset_name; + auto it_dataset = final_results_table.find(dataset_name); + if (it_dataset != final_results_table.end()) { + for (double dt_val : deltas) { + auto it_delta = it_dataset->second.find(dt_val); + if (it_delta != it_dataset->second.end()) { + if (it_delta->second.processed_intervals > 0 && isfinite(it_delta->second.median_nees)) { + cout << setw(12) << right << fixed << setprecision(3) << it_delta->second.median_nees; + } else { + cout << setw(12) << right << "N/A"; + } } else { - numerical_issue_count++; + cout << setw(12) << right << "-"; // No data for this deltaT } - } catch (const std::exception& e) { - numerical_issue_count++; } - - // Only increment processed intervals if NEES calculations were attempted without prior errors - processed_intervals++; - } else { - // Covariance extraction failed, skip NEES and move to next interval - current_t_i += deltaTij; - continue; + for (size_t i=0; i> all_run_results; - // 4. Calculate Median NEES and other stats - cout << "\nProcessed " << processed_intervals << " valid intervals." << endl; - cout << "Skipped " << skipped_intervals_gt << " intervals due to GT data gaps." << endl; - cout << "Skipped " << skipped_intervals_imu << " intervals due to IMU data gaps." << endl; - cout << "Skipped " << numerical_issue_count << " NEES calculations due to numerical/extraction issues." << endl; + for (const string& dataset_name : dataset_sequences) { + cout << "\n====================================================" << endl; + cout << "Processing Dataset: " << dataset_name << endl; + cout << "====================================================" << endl; - if (nees_results_combined.empty()) { - cerr << "No valid NEES results were calculated for one or both methods." << endl; - cerr << "Combined NEES count: " << nees_results_combined.size() << endl; - return 1; - } + string current_dataset_path = base_euroc_data_path + "/" + dataset_name; + string imu_csv_file = current_dataset_path + "/mav0/imu0/data.csv"; + string ground_truth_csv_file = current_dataset_path + "/mav0/state_groundtruth_estimate0/data.csv"; + string sensor_yaml_file = current_dataset_path + "/mav0/imu0/sensor.yaml"; - // Calculate Combined (Baseline) Stats - double median_nees_combined = calculateMedian(nees_results_combined); - double mean_nees_combined = 0; - double nees_sum_sq_combined = 0; - int valid_nees_count_combined = 0; - for(double n : nees_results_combined) { - if (isfinite(n)) { - mean_nees_combined += n; - nees_sum_sq_combined += n*n; - valid_nees_count_combined++; + // 1. Load Sensor Parameters from YAML + SensorParams sensor_params; + if (!parseSensorYaml(sensor_yaml_file, sensor_params)) { + cerr << "Failed to load sensor params for " << dataset_name << ". Skipping dataset." << endl; + continue; } - } - double nees_std_dev_combined = 0; - if (valid_nees_count_combined > 0) { - mean_nees_combined /= valid_nees_count_combined; - if (valid_nees_count_combined > 1) { - nees_std_dev_combined = sqrt((nees_sum_sq_combined - (mean_nees_combined * mean_nees_combined * valid_nees_count_combined)) / (valid_nees_count_combined - 1)); + cout << " Loaded Sensor Params for " << dataset_name << ":" << endl; + cout << " Gyro Noise Density: " << scientific << sensor_params.gyro_noise_density << endl; + cout << " Gyro Bias RW Density: " << sensor_params.gyro_bias_rw_density << endl; + cout << " Accel Noise Density: " << sensor_params.accel_noise_density << endl; + cout << " Accel Bias RW Density: " << sensor_params.accel_bias_rw_density << endl; + cout << " IMU Rate (Hz): " << fixed << setprecision(1) << sensor_params.rate_hz << endl; + // cout << " Body_P_Sensor (T_BS): \n" << sensor_params.body_P_sensor.matrix() << endl; + + + // 2. Load IMU and GT Data + vector imu_data; + vector gt_data; + if (!loadData(imu_csv_file, imu_data, parseImuLine) || imu_data.empty()) { + cerr << "Failed to load or empty IMU data for " << dataset_name << ". Skipping." << endl; + continue; } - } - - // Calculate Time Stats - double median_dt = calculateMedian(time_diffs); - double mean_dt = 0; - int valid_dt_count = 0; - for(double d : time_diffs) { - if (isfinite(d)) { - mean_dt += d; - valid_dt_count++; + if (!loadData(ground_truth_csv_file, gt_data, parseGroundTruthLine) || gt_data.empty()) { + cerr << "Failed to load or empty GT data for " << dataset_name << ". Skipping." << endl; + continue; } - } - if (valid_dt_count > 0) mean_dt /= valid_dt_count; - - cout << "\n--- Results (15-DOF NEES) ---" << endl; - cout << "Dataset: " << euroc_dataset_path << endl; - cout << "Target Preintegration Interval (deltaTij): " << fixed << setprecision(5) << deltaTij << " s" << endl; - cout << "Actual Median Integrated Interval: " << fixed << setprecision(5) << median_dt << " s" << endl; - cout << "Actual Mean Integrated Interval: " << fixed << setprecision(5) << mean_dt << " s" << endl; - - cout << "\n--- Combined Preintegration (Baseline) ---" << endl; - cout << "Number of Valid NEES Intervals: " << valid_nees_count_combined << endl; - cout << "Median 15-DOF NEES: " << fixed << setprecision(3) << median_nees_combined << endl; - cout << "Mean 15-DOF NEES: " << fixed << setprecision(3) << mean_nees_combined << endl; - cout << "StdDev 15-DOF NEES: " << fixed << setprecision(3) << nees_std_dev_combined << endl; - cout << "--------------------------------------------" << endl; + + if (gt_data.front().bias.gyroscope().norm() < 1e-9 && gt_data.front().bias.accelerometer().norm() < 1e-9 && + gt_data.back().bias.gyroscope().norm() < 1e-9 && gt_data.back().bias.accelerometer().norm() < 1e-9) { + cout << "Warning: Ground truth biases are all zero for " << dataset_name << ". Bias error might be consistently zero if initial bias guess is also zero." << endl; + } + + + for (double deltaTij_target : deltaTij_values) { + cout << "\n --- Processing with Target deltaTij: " << fixed << setprecision(1) << deltaTij_target << " s ---" << endl; + + // 3. Setup Preintegration Parameters for this run + auto p_combined = createCombinedImuParams(sensor_params, gravity_n, integration_noise_sigma); + // p_combined->print("CombinedParams"); // For debugging + + vector current_run_nees_values; + vector current_run_actual_dt_values; + int processed_intervals_count = 0; + int skipped_gt_count = 0; + int skipped_imu_count = 0; + int numerical_issues_count = 0; + + double min_sync_time = max(imu_data.front().timestamp, gt_data.front().timestamp); + double max_sync_time = min(imu_data.back().timestamp, gt_data.back().timestamp); + + size_t first_valid_gt_idx = findIndexAtOrAfter(gt_data, min_sync_time); + if (first_valid_gt_idx >= gt_data.size()) { + cerr << " Error: No ground truth data found at or after minimum start time for " << dataset_name << ". Skipping deltaTij " << deltaTij_target << endl; + continue; + } + double current_t_i = gt_data[first_valid_gt_idx].timestamp; + + cout << fixed << setprecision(9); // Higher precision for timestamps in logs + // cout << " Processing data from " << current_t_i << " s to " << max_sync_time << " s" << endl; + + + while (current_t_i + deltaTij_target <= max_sync_time + 1e-7) { // Add small tolerance for max_sync_time comparison + double t_j_nominal_end = current_t_i + deltaTij_target; + + size_t gt_idx_i = findIndexAtOrAfter(gt_data, current_t_i - 1e-7); // Allow slight mismatch for current_t_i lookup + if (gt_idx_i >= gt_data.size() || abs(gt_data[gt_idx_i].timestamp - current_t_i) > 0.01 ) { // If no GT close to current_t_i + // Try to advance current_t_i to the next GT sample if it's far off, or to next window + size_t next_gt_after_current = findIndexAtOrAfter(gt_data, current_t_i + 1e-7); + if(next_gt_after_current < gt_data.size() && gt_data[next_gt_after_current].timestamp < t_j_nominal_end) { + current_t_i = gt_data[next_gt_after_current].timestamp; + } else { + current_t_i = t_j_nominal_end; // Advance to next theoretical window start + } + skipped_gt_count++; + continue; + } + const GroundTruthData& gt_i_data = gt_data[gt_idx_i]; + // Sync current_t_i to actual GT timestamp for this interval's start + // This is crucial: the interval truly starts at gt_i_data.timestamp + double actual_interval_t_i = gt_i_data.timestamp; + double actual_interval_t_j_target = actual_interval_t_i + deltaTij_target; + + + size_t gt_idx_j = findIndexBefore(gt_data, actual_interval_t_j_target + 1e-7); // Find GT at or just BEFORE t_j_target + if (gt_idx_j <= gt_idx_i || gt_idx_j >= gt_data.size() || gt_data[gt_idx_j].timestamp < actual_interval_t_i + 0.1 * deltaTij_target) { // Ensure t_j is meaningfully after t_i + current_t_i = t_j_nominal_end; + skipped_gt_count++; + continue; + } + const GroundTruthData& gt_j_data = gt_data[gt_idx_j]; + double actual_interval_t_j_gt_synced = gt_j_data.timestamp; // This is the actual end time of the interval based on GT availability + + // Find IMU data for [actual_interval_t_i, actual_interval_t_j_gt_synced] + size_t imu_idx_start = findIndexAtOrAfter(imu_data, actual_interval_t_i - 1e-7); // inclusive start + size_t imu_idx_end = findIndexAtOrAfter(imu_data, actual_interval_t_j_gt_synced + 1e-7); // make it inclusive for last measurement by finding one *after* + + // Ensure imu_idx_end points one past the last IMU measurement *strictly before or at* actual_interval_t_j_gt_synced. + // The original findIndexAtOrAfter gets the first element >= timestamp. + // If imu_data[imu_idx_end-1].timestamp is what we want as the last IMU data point if its timestamp is <= actual_interval_t_j_gt_synced + + // Refined imu_idx_end logic: we want IMU data up to and including measurements at actual_interval_t_j_gt_synced. + // findIndexAtOrAfter(data, ts) gives index `k` such that data[k].ts >= ts. + // So, loop from imu_idx_start up to (but not including) imu_idx_end + // This means measurements from imu_data[imu_idx_start]...imu_data[imu_idx_end-1] will be processed. + // The last IMU data point whose timestamp is <= actual_interval_t_j_gt_synced. + + if (imu_idx_start >= imu_data.size() || imu_idx_start >= imu_idx_end || (imu_idx_end == imu_idx_start && imu_data[imu_idx_start].timestamp > actual_interval_t_j_gt_synced ) || (imu_idx_end > imu_idx_start && imu_data[imu_idx_start].timestamp > actual_interval_t_j_gt_synced + 1e-7) ) { + current_t_i = t_j_nominal_end; + skipped_imu_count++; + continue; + } + // Ensure at least one IMU measurement is processed if imu_idx_start == imu_idx_end + if (imu_idx_start == imu_idx_end) { // Only one IMU message could cover the interval + if (imu_data[imu_idx_start].timestamp < actual_interval_t_i - 1e-7 || imu_data[imu_idx_start].timestamp > actual_interval_t_j_gt_synced + 1e-7) { + current_t_i = t_j_nominal_end; + skipped_imu_count++; + continue; + } + // if imu_idx_start is valid, imu_idx_end should be imu_idx_start + 1 to process this one measurement + // This case needs careful handling in integrate loop or here. + // For now, let's ensure imu_idx_end > imu_idx_start for the loop. + // The integration loop structure might need adjustment if only one IMU point is in range for a very short interval. + // But usually deltaTij is large enough for multiple IMU points. + // If imu_idx_start == imu_idx_end, it means no IMU data strictly within, or only one point at the boundary. + // The current loop `for (size_t k = imu_idx_start; k < imu_idx_end; ++k)` will not run if start == end. + // This needs to be at least 1 for the loop to run. + if (imu_idx_end <= imu_idx_start) { // ensure at least one measurement can be processed by the loop + if (imu_idx_end < imu_data.size() && imu_data[imu_idx_end].timestamp <= actual_interval_t_j_gt_synced + 1e-7) { + // This means findIndexAtOrAfter found a point at actual_interval_t_j_gt_synced, so imu_idx_end should be that index + 1 + // This logic is getting complex, the loop in evaluateCombinedImuForInterval needs to be robust. + } else { + current_t_i = t_j_nominal_end; + skipped_imu_count++; + continue; + } + } + } + + // --- Evaluate Preintegration Method(s) for this interval --- + // Pass gt_i_data (synced t_i) and gt_j_data (synced t_j) + IntervalEvaluationResult combined_eval = evaluateCombinedImuForInterval( + gt_i_data, gt_j_data, imu_data, imu_idx_start, imu_idx_end, p_combined); + + if (combined_eval.success) { + current_run_nees_values.push_back(combined_eval.nees); + current_run_actual_dt_values.push_back(combined_eval.actual_dt); + processed_intervals_count++; + } else { + numerical_issues_count++; + } + + // Advance current_t_i for the next *non-overlapping* window + // Start next window based on the *actual start* of the current one + target duration + current_t_i = actual_interval_t_i + deltaTij_target; + } // End while loop over intervals + + // 4. Aggregate and store results for this (dataset, deltaTij) run + aggregateAndStoreResults(dataset_name, deltaTij_target, + current_run_nees_values, current_run_actual_dt_values, + processed_intervals_count, skipped_gt_count, + skipped_imu_count, numerical_issues_count, + all_run_results); + + } // End loop over deltaTij_values + } // End loop over dataset_names + + // 5. Print Final Summary Table + printFinalTable(all_run_results, dataset_sequences, deltaTij_values); + + cout << "\nEvaluation finished." << endl; return 0; - } - \ No newline at end of file + } \ No newline at end of file From 77a6033bd70642db19f404ece15bf630542dd991 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 21 May 2025 16:09:37 -0400 Subject: [PATCH 3/5] Combined yields reasonable NEES --- examples/EurocDataExample.cpp | 1037 +++++++++++++-------------------- 1 file changed, 409 insertions(+), 628 deletions(-) diff --git a/examples/EurocDataExample.cpp b/examples/EurocDataExample.cpp index 0937f37b7d..5a89d4a6d8 100644 --- a/examples/EurocDataExample.cpp +++ b/examples/EurocDataExample.cpp @@ -5,18 +5,22 @@ * Fornasier et al., "Equivariant IMU Preintegration with Biases: * a Galilean Group Approach" (arXiv:2411.05548v4), calculating * 15-DOF NEES for NavState + Bias. + * This version is refactored to support multiple IMU preintegration techniques. * @author Matt Kielo, Porter Zach - * @modifier AI Assistant (Refactoring and Enhancements) + * @modifier AI Assistant (Refactoring and Enhancements for Multiple Techniques) */ #include #include - #include + #include #include + #include + #include + #include // For PreintegratedRotationParams definition #include #include - #include // For I_3x3, I_6x6 + #include #include #include @@ -28,12 +32,11 @@ #include #include #include - #include // For results table + #include + #include - // Define custom types needed for this example namespace gtsam { typedef Eigen::Matrix Vector15; - // Define Matrix15x15 type for convenience, avoiding conflict with GTSAM's Matrix15 (1x5) typedef Eigen::Matrix Matrix15x15; } @@ -41,709 +44,487 @@ using namespace std; // --- Configuration --- - // Base path for EuRoC datasets. Individual dataset folders (e.g., MH_01) should be inside this. - const string base_euroc_data_path = "C:/Users/porte/Desktop/projects/works25/euroc/data"; - - // List of datasets to process - const vector dataset_sequences = { - "MH_01", - // "MH_02", - // "MH_03", - // "MH_04", - // "MH_05", - // "V1_01", - // "V1_02", - // "V1_03", - // "V2_01", - "V2_02" - // "V2_03" // This one has different GT format (no bias) - }; - - // List of preintegration interval durations (seconds) + const string base_euroc_data_path = "C:/Users/porte/Desktop/projects/works25/euroc/data"; // PLEASE UPDATE THIS PATH + const vector dataset_sequences = { "MH_01", "V2_02" }; const vector deltaTij_values = {0.2, 0.5, 1.0}; - - // Integration noise sigma (usually very small or zero) const double integration_noise_sigma = 1e-8; - - // Gravity vector in navigation frame (ENU convention: Z-up, gravity is negative Z) const Vector3 gravity_n(0, 0, -9.81); - - // --- Data Structures --- - - struct ImuData { - double timestamp; // seconds - Vector3 omega; // rad/s (gyro) - Vector3 acc; // m/s^2 (accel) - }; - - struct GroundTruthData { - double timestamp; // seconds - NavState navState; - imuBias::ConstantBias bias; - }; - + // --- Data Structures (condensed for brevity, no changes in structure) --- + struct ImuData { double timestamp; Vector3 omega; Vector3 acc; }; + struct GroundTruthData { double timestamp; NavState navState; imuBias::ConstantBias bias; }; struct SensorParams { - double gyro_noise_density = 0.0; - double gyro_bias_rw_density = 0.0; - double accel_noise_density = 0.0; - double accel_bias_rw_density = 0.0; - Pose3 body_P_sensor; // T_BS: IMU frame in body frame - double rate_hz = 200.0; // Default, will be overridden by YAML if present - }; - - struct IntervalEvaluationResult { - double nees = numeric_limits::quiet_NaN(); - bool success = false; - double actual_dt = 0.0; + double gyro_noise_density = 0.0; double gyro_bias_rw_density = 0.0; + double accel_noise_density = 0.0; double accel_bias_rw_density = 0.0; + Pose3 body_P_sensor; double rate_hz = 200.0; }; - + struct IntervalEvaluationResult { double nees = numeric_limits::quiet_NaN(); bool success = false; double actual_dt = 0.0; }; struct AggregatedRunResults { - string dataset_name; - double deltaTij_target = 0.0; - - double median_nees = numeric_limits::quiet_NaN(); - double mean_nees = numeric_limits::quiet_NaN(); + string dataset_name; double deltaTij_target = 0.0; + double median_nees = numeric_limits::quiet_NaN(); double mean_nees = numeric_limits::quiet_NaN(); double std_dev_nees = numeric_limits::quiet_NaN(); - - double median_actual_dt = numeric_limits::quiet_NaN(); - double mean_actual_dt = numeric_limits::quiet_NaN(); - - int processed_intervals = 0; - int skipped_intervals_gt = 0; - int skipped_intervals_imu = 0; - int numerical_issue_count = 0; - - void print_summary() const { + double median_actual_dt = numeric_limits::quiet_NaN(); double mean_actual_dt = numeric_limits::quiet_NaN(); + int processed_intervals = 0; int skipped_intervals_gt = 0; + int skipped_intervals_imu = 0; int numerical_issue_count = 0; + void print_summary() const { /* ... (implementation as before) ... */ cout << " Summary for " << dataset_name << " (deltaTij: " << fixed << setprecision(1) << deltaTij_target << "s):" << endl; cout << " Processed Intervals: " << processed_intervals << endl; if (skipped_intervals_gt > 0) cout << " Skipped (GT Gaps): " << skipped_intervals_gt << endl; if (skipped_intervals_imu > 0) cout << " Skipped (IMU Gaps): " << skipped_intervals_imu << endl; if (numerical_issue_count > 0) cout << " Skipped (Numerical Issues): " << numerical_issue_count << endl; - if (processed_intervals > 0) { cout << " Median Actual dt: " << fixed << setprecision(5) << median_actual_dt << " s" << endl; cout << " Mean Actual dt: " << fixed << setprecision(5) << mean_actual_dt << " s" << endl; cout << " Median 15-DOF NEES: " << fixed << setprecision(3) << median_nees << endl; cout << " Mean 15-DOF NEES: " << fixed << setprecision(3) << mean_nees << endl; cout << " StdDev 15-DOF NEES: " << fixed << setprecision(3) << std_dev_nees << endl; - } else { - cout << " No NEES results to report." << endl; - } + } else { cout << " No NEES results to report." << endl; } cout << " -----------------------------------" << endl; } }; - - // --- String Helper --- - string trim(const string& str, const string& whitespace = " \t\n\r\f\v") { - const auto strBegin = str.find_first_not_of(whitespace); - if (strBegin == string::npos) return ""; // no content - const auto strEnd = str.find_last_not_of(whitespace); - const auto strRange = strEnd - strBegin + 1; - return str.substr(strBegin, strRange); + // --- Helper Functions (condensed, no changes in logic) --- + string trim(const string& str, const string& whitespace = " \t\n\r\f\v") { /* ... */ + const auto strBegin = str.find_first_not_of(whitespace); if (strBegin == string::npos) return ""; + const auto strEnd = str.find_last_not_of(whitespace); const auto strRange = strEnd - strBegin + 1; return str.substr(strBegin, strRange); } - - // --- YAML Parsing Helper --- - bool parseSensorYaml(const string& yaml_path, SensorParams& params) { - ifstream file(yaml_path); - if (!file.is_open()) { - cerr << "Error opening sensor YAML file: " << yaml_path << endl; - return false; - } - - string line; - Matrix4 T_BS_mat = Matrix4::Identity(); - bool t_bs_data_mode = false; - vector t_bs_coeffs; - - auto extract_value = [&](const string& l, const string& key) { - size_t key_pos = l.find(key); - if (key_pos != string::npos) { - string val_str = l.substr(key_pos + key.length()); - return trim(val_str); - } - return string(""); - }; - + bool parseSensorYaml(const string& yaml_path, SensorParams& params) { /* ... */ + ifstream file(yaml_path); if (!file.is_open()) { cerr << "Error opening sensor YAML file: " << yaml_path << endl; return false; } + string line; Matrix4 T_BS_mat = Matrix4::Identity(); bool t_bs_data_mode = false; vector t_bs_coeffs; + auto extract_value = [&](const string& l, const string& key) { size_t key_pos = l.find(key); if (key_pos != string::npos) { string val_str = l.substr(key_pos + key.length()); return trim(val_str); } return string(""); }; while (getline(file, line)) { - line = trim(line); - if (line.empty() || line[0] == '#') continue; - - if (t_bs_data_mode) { - stringstream ss_line(line); - string val_token; - while(getline(ss_line, val_token, ',')){ - val_token.erase(remove(val_token.begin(), val_token.end(), '['), val_token.end()); - val_token.erase(remove(val_token.begin(), val_token.end(), ']'), val_token.end()); - val_token = trim(val_token); - if(!val_token.empty()){ - try { - t_bs_coeffs.push_back(stod(val_token)); - } catch (const std::exception& e) { - cerr << "Warning: Could not parse T_BS data value: " << val_token << endl; - } - } - } - if (t_bs_coeffs.size() >= 16) { - t_bs_data_mode = false; // Done collecting - } + line = trim(line); if (line.empty() || line[0] == '#') continue; + if (t_bs_data_mode) { stringstream ss_line(line); string val_token; while(getline(ss_line, val_token, ',')){ val_token.erase(remove(val_token.begin(), val_token.end(), '['), val_token.end()); val_token.erase(remove(val_token.begin(), val_token.end(), ']'), val_token.end()); val_token = trim(val_token); if(!val_token.empty()){ try { t_bs_coeffs.push_back(stod(val_token)); } catch (const std::exception&) {}}} if (t_bs_coeffs.size() >= 16) { t_bs_data_mode = false; } } else { string val_str; - val_str = extract_value(line, "gyroscope_noise_density:"); - if (!val_str.empty()) params.gyro_noise_density = stod(val_str); - - val_str = extract_value(line, "gyroscope_random_walk:"); - if (!val_str.empty()) params.gyro_bias_rw_density = stod(val_str); - - val_str = extract_value(line, "accelerometer_noise_density:"); - if (!val_str.empty()) params.accel_noise_density = stod(val_str); - - val_str = extract_value(line, "accelerometer_random_walk:"); - if (!val_str.empty()) params.accel_bias_rw_density = stod(val_str); - - val_str = extract_value(line, "rate_hz:"); - if (!val_str.empty()) params.rate_hz = stod(val_str); - - if (line.find("T_BS:") != string::npos) { - // Next lines might be rows, cols, then data - } else if (line.find("data:") != string::npos && t_bs_coeffs.empty()) { - t_bs_data_mode = true; - // Remove "data:" part and process rest of the line - line = line.substr(line.find("data:") + 5); - stringstream ss_line(line); - string val_token; - while(getline(ss_line, val_token, ',')){ - val_token.erase(remove(val_token.begin(), val_token.end(), '['), val_token.end()); - val_token.erase(remove(val_token.begin(), val_token.end(), ']'), val_token.end()); - val_token = trim(val_token); - if(!val_token.empty()){ - try { - t_bs_coeffs.push_back(stod(val_token)); - } catch (const std::exception& e) { - cerr << "Warning: Could not parse T_BS data value: " << val_token << endl; - } - } - } - } - } - } - - if (t_bs_coeffs.size() >= 16) { - for (int i = 0; i < 4; ++i) { - for (int j = 0; j < 4; ++j) { - T_BS_mat(i, j) = t_bs_coeffs[i * 4 + j]; - } + val_str = extract_value(line, "gyroscope_noise_density:"); if (!val_str.empty()) params.gyro_noise_density = stod(val_str); + val_str = extract_value(line, "gyroscope_random_walk:"); if (!val_str.empty()) params.gyro_bias_rw_density = stod(val_str); + val_str = extract_value(line, "accelerometer_noise_density:"); if (!val_str.empty()) params.accel_noise_density = stod(val_str); + val_str = extract_value(line, "accelerometer_random_walk:"); if (!val_str.empty()) params.accel_bias_rw_density = stod(val_str); + val_str = extract_value(line, "rate_hz:"); if (!val_str.empty()) params.rate_hz = stod(val_str); + if (line.find("T_BS:") != string::npos) {} else if (line.find("data:") != string::npos && t_bs_coeffs.empty()) { t_bs_data_mode = true; line = line.substr(line.find("data:") + 5); stringstream ss_line(line); string val_token; while(getline(ss_line, val_token, ',')){ val_token.erase(remove(val_token.begin(), val_token.end(), '['), val_token.end()); val_token.erase(remove(val_token.begin(), val_token.end(), ']'), val_token.end()); val_token = trim(val_token); if(!val_token.empty()){ try { t_bs_coeffs.push_back(stod(val_token)); } catch (const std::exception&) {}}}} } - params.body_P_sensor = Pose3(T_BS_mat); - } else { - cout << "Warning: T_BS data not fully parsed or missing from YAML. Using Identity." << endl; - params.body_P_sensor = Pose3(); // Identity - } - - // Basic check if critical params were loaded - if (params.gyro_noise_density == 0.0 || params.accel_noise_density == 0.0) { - cerr << "Warning: Some noise densities are zero after parsing YAML. Check file content." << endl; - // Allow continuation, but this is usually an issue. } + if (t_bs_coeffs.size() >= 16) { for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { T_BS_mat(i, j) = t_bs_coeffs[i * 4 + j]; } } params.body_P_sensor = Pose3(T_BS_mat); } else { params.body_P_sensor = Pose3(); } return true; } - - // --- Helper Functions (from original, largely unchanged) --- - - bool parseImuLine(const string& line, ImuData& data) { - stringstream ss(line); - string segment; - vector seglist; - while (getline(ss, segment, ',')) seglist.push_back(segment); - if (seglist.size() < 7) return false; - try { - data.timestamp = stod(seglist[0]) * 1e-9; // ns to s - data.omega = Vector3(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); - data.acc = Vector3(stod(seglist[4]), stod(seglist[5]), stod(seglist[6])); - } catch (const std::exception& e) { return false; } - return true; + bool parseImuLine(const string& line, ImuData& data) { /* ... */ + stringstream ss(line); string segment; vector seglist; while (getline(ss, segment, ',')) seglist.push_back(segment); if (seglist.size() < 7) return false; + try { data.timestamp = stod(seglist[0]) * 1e-9; data.omega = Vector3(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); data.acc = Vector3(stod(seglist[4]), stod(seglist[5]), stod(seglist[6])); } catch (const std::exception&) { return false; } return true; } - - bool parseGroundTruthLine(const string& line, GroundTruthData& data) { - stringstream ss(line); - string segment; - vector seglist; - while (getline(ss, segment, ',')) seglist.push_back(segment); - if (seglist.size() < 17) return false; // EuRoC GT has 17 fields (timestamp, p, q, v, bias_w, bias_a) - try { - data.timestamp = stod(seglist[0]) * 1e-9; // ns to s - Point3 pos(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); - Quaternion q(stod(seglist[4]), stod(seglist[5]), stod(seglist[6]), stod(seglist[7])); // w, x, y, z - Rot3 rot = Rot3(q); - Velocity3 vel(stod(seglist[8]), stod(seglist[9]), stod(seglist[10])); - data.navState = NavState(rot, pos, vel); - Vector3 bias_gyro(stod(seglist[11]), stod(seglist[12]), stod(seglist[13])); - Vector3 bias_acc(stod(seglist[14]), stod(seglist[15]), stod(seglist[16])); - data.bias = imuBias::ConstantBias(bias_acc, bias_gyro); - } catch (const std::exception& e) { return false; } - return true; + bool parseGroundTruthLine(const string& line, GroundTruthData& data) { /* ... */ + stringstream ss(line); string segment; vector seglist; while (getline(ss, segment, ',')) seglist.push_back(segment); if (seglist.size() < 17) return false; + try { data.timestamp = stod(seglist[0]) * 1e-9; Point3 pos(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); Quaternion q(stod(seglist[4]), stod(seglist[5]), stod(seglist[6]), stod(seglist[7])); Rot3 rot = Rot3(q); Velocity3 vel(stod(seglist[8]), stod(seglist[9]), stod(seglist[10])); data.navState = NavState(rot, pos, vel); Vector3 bias_gyro(stod(seglist[11]), stod(seglist[12]), stod(seglist[13])); Vector3 bias_acc(stod(seglist[14]), stod(seglist[15]), stod(seglist[16])); data.bias = imuBias::ConstantBias(bias_acc, bias_gyro); } catch (const std::exception&) { return false; } return true; } - - template - bool loadData(const string& filename, vector& data, bool (*parseFunc)(const string&, T&)) { - ifstream file(filename); - if (!file.is_open()) { - cerr << "Error opening file: " << filename << endl; - return false; - } - string line; - if (!getline(file, line)) { /* Skip header */ } - while (getline(file, line)) { - if (line.empty() || line[0] == '#') continue; - T entry; - if (parseFunc(line, entry)) data.push_back(entry); - } - cout << "Loaded " << data.size() << " valid entries from " << filename << endl; - return !data.empty(); + template bool loadData(const string& filename, vector& data, bool (*parseFunc)(const string&, T&)) { /* ... */ + ifstream file(filename); if (!file.is_open()) { return false; } string line; if (!getline(file, line)) { return false; } + while (getline(file, line)) { if (line.empty() || line[0] == '#') continue; T entry; if (parseFunc(line, entry)) data.push_back(entry); } return !data.empty(); } - - template - size_t findIndexBefore(const vector& sorted_data, double timestamp) { - auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, - [](const T& element, double value) { return element.timestamp < value; }); - if (it == sorted_data.begin()) { - if (it != sorted_data.end() && it->timestamp >= timestamp) return 0; - else { - // cerr << "Warning: Timestamp " << fixed << setprecision(9) << timestamp - // << " is before the first data point " << sorted_data.front().timestamp << endl; - return 0; - } - } - return distance(sorted_data.begin(), it) - 1; + template size_t findIndexBefore(const vector& sorted_data, double timestamp) { /* ... */ + auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, [](const T& e, double v) { return e.timestamp < v; }); if (it == sorted_data.begin()) { return 0; } return distance(sorted_data.begin(), it) - 1; } - - template - size_t findIndexAtOrAfter(const vector& sorted_data, double timestamp) { - auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, - [](const T& element, double value) { return element.timestamp < value; }); - return distance(sorted_data.begin(), it); + template size_t findIndexAtOrAfter(const vector& sorted_data, double timestamp) { /* ... */ + auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, [](const T& e, double v) { return e.timestamp < v; }); return distance(sorted_data.begin(), it); } - - double calculateMedian(vector& values) { // Note: modifies values vector - if (values.empty()) return numeric_limits::quiet_NaN(); - size_t n = values.size(); - size_t mid = n / 2; - nth_element(values.begin(), values.begin() + mid, values.end()); - if (n % 2 != 0) return values[mid]; - else { - if (n == 0) return numeric_limits::quiet_NaN(); // Should be caught by empty() check - if (mid == 0) return values[mid]; // Only one element effectively if n=1, covered by n%2 !=0 - // For even size, ensure mid-1 is valid. nth_element for mid is done. - // Now find element at mid-1 for the other part of average. - // The values vector is partially sorted up to mid. - // We need the (mid-1)th element from the original sorted sequence. - double mid_val1 = values[mid]; // This is the (n/2)-th element (0-indexed) - // Find the maximum of elements before 'mid' (which is values[mid-1] after sorting up to mid-1) - nth_element(values.begin(), values.begin() + mid - 1, values.begin() + mid); // Sort within the lower half - return (values[mid - 1] + mid_val1) / 2.0; - } + double calculateMedian(vector& values) { /* ... */ + if (values.empty()) return numeric_limits::quiet_NaN(); size_t n = values.size(); size_t mid = n / 2; nth_element(values.begin(), values.begin() + mid, values.end()); + if (n % 2 != 0) return values[mid]; else { if (mid == 0) return values[mid]; double m1 = values[mid]; nth_element(values.begin(), values.begin() + mid - 1, values.begin() + mid); return (values[mid - 1] + m1) / 2.0; } } - // --- Core Evaluation Logic --- - - shared_ptr createCombinedImuParams( - const SensorParams& sensor_params, - const Vector3& gravity_n_vec, - double integration_sigma) { - - double nominal_imu_dt = 1.0 / sensor_params.rate_hz; - if (nominal_imu_dt <= 1e-9) { // Avoid division by zero if rate_hz is 0 - cerr << "Warning: IMU rate_hz is " << sensor_params.rate_hz << ", using default nominal_dt of 0.005s for covariance calculation." << endl; - nominal_imu_dt = 0.005; + // --- IMU Preintegration Technique Abstraction --- + class ImuPreintegrationTechnique { + public: + virtual ~ImuPreintegrationTechnique() = default; + virtual string name() const = 0; + + virtual shared_ptr createPreintegrationParams( + const SensorParams& sensor_params, const Vector3& gravity_n_vec, + double integration_sigma, const Vector3& body_omega_coriolis) const = 0; + + virtual IntervalEvaluationResult evaluateInterval( + const GroundTruthData& gt_i, const GroundTruthData& gt_j, + const vector& all_imu_data, size_t imu_idx_start_global, size_t imu_idx_end_global, + const shared_ptr& pim_params_base, + const imuBias::ConstantBias& initial_bias, const SensorParams& sensor_params + ) const = 0; + + protected: + // Sets parameters common to PreintegrationParams and its base PreintegratedRotationParams + void setupBasePreintegratedRotationParams(PreintegratedRotationParams* p_rot, const SensorParams& sensor_params, + const Vector3& body_omega_coriolis) const { + double nominal_imu_dt = 1.0 / sensor_params.rate_hz; + if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; + + p_rot->gyroscopeCovariance = pow(sensor_params.gyro_noise_density, 2) / nominal_imu_dt * I_3x3; + if (body_omega_coriolis.norm() > 1e-9) { // Only set if non-zero, otherwise keep default std::nullopt + p_rot->omegaCoriolis = body_omega_coriolis; + } + p_rot->body_P_sensor = sensor_params.body_P_sensor; // This is an optional } + }; - shared_ptr p_base = - PreintegrationCombinedParams::MakeSharedU( - gravity_n_vec.norm() - ); - - shared_ptr p = - static_pointer_cast(p_base); - - p->n_gravity = gravity_n_vec; // Set actual gravity vector (overwrites default direction if magnitude was same) - - p->gyroscopeCovariance = pow(sensor_params.gyro_noise_density, 2) / nominal_imu_dt * I_3x3; - p->accelerometerCovariance = pow(sensor_params.accel_noise_density, 2) / nominal_imu_dt * I_3x3; - p->integrationCovariance = pow(integration_sigma, 2) * I_3x3; - p->biasAccCovariance = pow(sensor_params.accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; - p->biasOmegaCovariance = pow(sensor_params.gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; - p->biasAccOmegaInt = I_6x6 * 1e-5; // Default, as in original script + // --- Combined IMU Factor Technique --- + class CombinedImuTechnique : public ImuPreintegrationTechnique { + public: + string name() const override { return "CombinedIMU (Fornasier et al. style)"; } - return p; - } + shared_ptr createPreintegrationParams( + const SensorParams& sensor_params, const Vector3& gravity_n_vec, + double integration_sigma, const Vector3& body_omega_coriolis /* body_omega_coriolis effectively unused for this technique to match original */) const override { + + // PreintegrationCombinedParams constructor takes gravity magnitude + auto p_combined = PreintegrationCombinedParams::MakeSharedU(gravity_n_vec.norm()); + + // MATCH ORIGINAL SCRIPT'S n_gravity SETTING: + // Original script assigned the potentially non-unit gravity_n_vec to p_combined->n_gravity + // after MakeSharedU (which sets n_gravity to default unit vector and g to norm). + p_combined->n_gravity = gravity_n_vec; + + // MATCH ORIGINAL SCRIPT'S COVARIANCE SETTINGS: + // (and ensure body_P_sensor and omegaCoriolis remain default/unset on p_combined, + // as original script did not explicitly set these on the PreintegrationParams object) + double nominal_imu_dt = 1.0 / sensor_params.rate_hz; + if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; // Defensive, as in original + + p_combined->gyroscopeCovariance = pow(sensor_params.gyro_noise_density, 2) / nominal_imu_dt * I_3x3; + p_combined->accelerometerCovariance = pow(sensor_params.accel_noise_density, 2) / nominal_imu_dt * I_3x3; + p_combined->integrationCovariance = pow(integration_sigma, 2) * I_3x3; + p_combined->biasAccCovariance = pow(sensor_params.accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; + p_combined->biasOmegaCovariance = pow(sensor_params.gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; + p_combined->biasAccOmegaInt = I_6x6 * 1e-5; // Default, as in original script + + // body_P_sensor and omegaCoriolis will be std::nullopt by default from + // PreintegrationCombinedParams::MakeSharedU. This is consistent with the original + // script not explicitly setting these on its PreintegrationCombinedParams object. + + return p_combined; + } - IntervalEvaluationResult evaluateCombinedImuForInterval( - const GroundTruthData& gt_i, - const GroundTruthData& gt_j, - const vector& all_imu_data, // Full IMU data vector - size_t imu_idx_start_global, // Start index in all_imu_data - size_t imu_idx_end_global, // End index (exclusive) in all_imu_data - const shared_ptr& pim_params) { - - IntervalEvaluationResult result; - result.success = false; - - PreintegratedCombinedMeasurements pim_combined(pim_params, gt_i.bias); - - double previous_imu_t = gt_i.timestamp; // Start integration from gt_i timestamp - - for (size_t k = imu_idx_start_global; k < imu_idx_end_global; ++k) { - const ImuData& imu = all_imu_data[k]; - // Ensure IMU measurement is within [gt_i.timestamp, gt_j.timestamp] - // This check is mostly handled by findIndexAtOrAfter for start/end, but good for dt calc - if (imu.timestamp < gt_i.timestamp || imu.timestamp > gt_j.timestamp + 1e-9) { // Add small tolerance for gt_j.timestamp - // This case should be rare if indices are chosen carefully, - // but if it happens, adjust previous_imu_t to avoid large dt - if (imu.timestamp < previous_imu_t && imu.timestamp >= gt_i.timestamp) previous_imu_t = imu.timestamp; - continue; + IntervalEvaluationResult evaluateInterval( + const GroundTruthData& gt_i, const GroundTruthData& gt_j, + const vector& all_imu_data, size_t imu_idx_start_global, size_t imu_idx_end_global, + const shared_ptr& pim_params_base, + const imuBias::ConstantBias& initial_bias, const SensorParams& /* sensor_params - not directly used here after PIM setup */ + ) const override { + + auto pim_params = static_pointer_cast(pim_params_base); + IntervalEvaluationResult result; result.success = false; + PreintegratedCombinedMeasurements pim_combined(pim_params, initial_bias); // initial_bias is gt_i.bias + + double previous_imu_t = gt_i.timestamp; + for (size_t k = imu_idx_start_global; k < imu_idx_end_global; ++k) { + const ImuData& imu = all_imu_data[k]; + // MATCHING ORIGINAL SCRIPT'S TIMESTAMP CHECK (stricter lower bound) + if (imu.timestamp < gt_i.timestamp || imu.timestamp > gt_j.timestamp + 1e-9) { + if (imu.timestamp < previous_imu_t && imu.timestamp >= gt_i.timestamp) previous_imu_t = imu.timestamp; // Match original condition + continue; + } + double current_imu_t = imu.timestamp; double dt = current_imu_t - previous_imu_t; + if (dt < 1e-9) { // Effectively zero dt or out of order + if (dt < 0 && abs(dt) > 1e-7) { /* cerr << "Warning: Negative IMU dt " << dt << endl; */ } // Logging diff is ok + previous_imu_t = current_imu_t; + continue; + } + pim_combined.integrateMeasurement(imu.acc, imu.omega, dt); + previous_imu_t = current_imu_t; } - - double current_imu_t = imu.timestamp; - double dt = current_imu_t - previous_imu_t; - - if (dt < 1e-9) { // Effectively zero dt or out of order (should not happen with sorted data) - if (dt < 0) { /* cerr << "Warning: Negative IMU dt " << dt << endl; */ } - previous_imu_t = current_imu_t; - continue; + double final_dt = gt_j.timestamp - previous_imu_t; + if (final_dt > 1e-9 && imu_idx_end_global > imu_idx_start_global && imu_idx_end_global <= all_imu_data.size()) { + const ImuData& last_imu_in_segment = all_imu_data[imu_idx_end_global - 1]; + pim_combined.integrateMeasurement(last_imu_in_segment.acc, last_imu_in_segment.omega, final_dt); } - if (dt > 0.1) { // Warn about large dt, might indicate issues - // cerr << "Warning: Large IMU dt of " << dt << "s at t=" << current_imu_t << endl; - } - - - pim_combined.integrateMeasurement(imu.acc, imu.omega, dt); - previous_imu_t = current_imu_t; - } - - // Integrate from the last IMU measurement up to gt_j.timestamp - double final_dt = gt_j.timestamp - previous_imu_t; - if (final_dt > 1e-9 && imu_idx_end_global > imu_idx_start_global && imu_idx_end_global <= all_imu_data.size()) { - // Use the last IMU measurement that was processed in the loop, which is all_imu_data[imu_idx_end_global - 1] - // if imu_idx_end_global points one past the last element to include. - const ImuData& last_imu_in_segment = all_imu_data[imu_idx_end_global -1]; - pim_combined.integrateMeasurement(last_imu_in_segment.acc, last_imu_in_segment.omega, final_dt); - } - + + result.actual_dt = pim_combined.deltaTij(); + if (result.actual_dt < 1e-6) return result; - result.actual_dt = pim_combined.deltaTij(); - if (result.actual_dt < 1e-6) { // Too short interval + NavState estimated_state_j = pim_combined.predict(gt_i.navState, initial_bias); + Vector9 error_nav = NavState::Logmap(gt_j.navState.inverse() * estimated_state_j); + + // MATCH ORIGINAL SCRIPT'S BIAS ERROR LOGIC + Vector6 error_bias = gt_j.bias.vector() - initial_bias.vector(); // initial_bias is gt_i.bias + + Vector15 error15; error15 << error_nav, error_bias; + Matrix15x15 P15 = pim_combined.preintMeasCov(); + if (P15.rows() != 15 || P15.cols() != 15) return result; + try { + Matrix15x15 P15_reg = P15 + Matrix15x15::Identity() * 1e-9; + Matrix15x15 P15_inv = P15_reg.inverse(); + double nees_val = error15.transpose() * P15_inv * error15; + if (!isnan(nees_val) && !isinf(nees_val) && nees_val >= 0) { + result.nees = nees_val; result.success = true; + } + } catch (const std::exception&) { } return result; } + }; - NavState estimated_state_j = pim_combined.predict(gt_i.navState, gt_i.bias); - Vector9 error_nav = NavState::Logmap(gt_j.navState.inverse() * estimated_state_j); // More robust error for NavState - Vector6 error_bias = gt_j.bias.vector() - gt_i.bias.vector(); // Bias error is over the interval - - Vector15 error15; - error15 << error_nav, error_bias; - - Matrix15x15 P15 = pim_combined.preintMeasCov(); - if (P15.rows() != 15 || P15.cols() != 15) { - // cerr << "Error: Covariance matrix size mismatch." << endl; - return result; // Numerical issue - } - - try { - Matrix15x15 P15_reg = P15 + Matrix15x15::Identity() * 1e-9; // Regularization - Matrix15x15 P15_inv = P15_reg.inverse(); - double nees_val = error15.transpose() * P15_inv * error15; + // --- Manifold IMU Preintegration Technique --- + class ManifoldImuTechnique : public ImuPreintegrationTechnique { + public: + string name() const override { return "ManifoldIMU (Forster et al. style)"; } - if (!isnan(nees_val) && !isinf(nees_val) && nees_val >= 0) { - result.nees = nees_val; - result.success = true; + shared_ptr createPreintegrationParams( + const SensorParams& sensor_params, const Vector3& gravity_n_vec, + double /* integration_sigma - not used by standard PIM */, const Vector3& body_omega_coriolis) const override { + + auto p_manifold = PreintegrationParams::MakeSharedU(gravity_n_vec.norm()); + + if (gravity_n_vec.norm() > 1e-9) { + p_manifold->n_gravity = gravity_n_vec.normalized(); + } else { + p_manifold->n_gravity = Vector3(0,0,-1); } - } catch (const std::exception& e) { - // cerr << "Exception during NEES calculation: " << e.what() << endl; - return result; // Numerical issue + + // For ManifoldIMU, using setupBasePreintegratedRotationParams is standard + // as it sets body_P_sensor and omegaCoriolis if needed. + setupBasePreintegratedRotationParams(static_cast(p_manifold.get()), + sensor_params, body_omega_coriolis); + + double nominal_imu_dt = 1.0 / sensor_params.rate_hz; + if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; + p_manifold->accelerometerCovariance = pow(sensor_params.accel_noise_density, 2) / nominal_imu_dt * I_3x3; + p_manifold->integrationCovariance = Matrix33::Zero(); // Standard PIM has no integration covariance term like CombinedPIM + // Note: biasAccCovariance and biasOmegaCovariance are not members of PreintegrationParams. + // They will be used directly from sensor_params in evaluateInterval for covariance construction. + return p_manifold; } - return result; - } - void aggregateAndStoreResults( - const string& dataset_name, double deltaTij_target, - const vector& nees_values_list_const, // const to allow copying - const vector& actual_dt_list_const, // const to allow copying - int num_processed, int num_skipped_gt, int num_skipped_imu, int num_numerical_issues, - map>& final_results_table) { - - AggregatedRunResults agg_results; - agg_results.dataset_name = dataset_name; - agg_results.deltaTij_target = deltaTij_target; - agg_results.processed_intervals = num_processed; - agg_results.skipped_intervals_gt = num_skipped_gt; - agg_results.skipped_intervals_imu = num_skipped_imu; - agg_results.numerical_issue_count = num_numerical_issues; - - vector nees_values_list_mutable = nees_values_list_const; // mutable copy for median - vector actual_dt_list_mutable = actual_dt_list_const; // mutable copy for median - - if (!nees_values_list_const.empty()) { - agg_results.median_nees = calculateMedian(nees_values_list_mutable); // Modifies nees_values_list_mutable - - double sum_nees = 0; - double sum_sq_nees = 0; - for (double n : nees_values_list_const) { - sum_nees += n; - sum_sq_nees += n * n; - } - agg_results.mean_nees = sum_nees / nees_values_list_const.size(); - if (nees_values_list_const.size() > 1) { - // variance = E[X^2] - (E[X])^2 - double variance = (sum_sq_nees / nees_values_list_const.size()) - (agg_results.mean_nees * agg_results.mean_nees); - // Ensure variance is non-negative due to potential floating point inaccuracies - agg_results.std_dev_nees = sqrt(max(0.0, variance)); - } else { - agg_results.std_dev_nees = 0; + IntervalEvaluationResult evaluateInterval( + const GroundTruthData& gt_i, const GroundTruthData& gt_j, + const vector& all_imu_data, size_t imu_idx_start_global, size_t imu_idx_end_global, + const shared_ptr& pim_params_base, + const imuBias::ConstantBias& initial_bias, const SensorParams& sensor_params + ) const override { + + IntervalEvaluationResult result; result.success = false; + PreintegratedImuMeasurements pim_manifold(pim_params_base, initial_bias); // initial_bias is gt_i.bias + + double previous_imu_t = gt_i.timestamp; + for (size_t k = imu_idx_start_global; k < imu_idx_end_global; ++k) { + const ImuData& imu = all_imu_data[k]; + if (imu.timestamp < gt_i.timestamp -1e-9 || imu.timestamp > gt_j.timestamp + 1e-9) { // Original range slightly different, but this is for Manifold + if (imu.timestamp < previous_imu_t && imu.timestamp >= gt_i.timestamp-1e-9) previous_imu_t = imu.timestamp; + continue; + } + double current_imu_t = imu.timestamp; double dt = current_imu_t - previous_imu_t; + if (dt < 1e-9) { + if (dt < 0 && abs(dt) > 1e-7) { /* cerr << "Warning: Negative IMU dt " << dt << endl; */ } + previous_imu_t = current_imu_t; + continue; + } + pim_manifold.integrateMeasurement(imu.acc, imu.omega, dt); + previous_imu_t = current_imu_t; } - } - if (!actual_dt_list_const.empty()) { - agg_results.median_actual_dt = calculateMedian(actual_dt_list_mutable); - double sum_dt = 0; - for (double dt_val : actual_dt_list_const) { - sum_dt += dt_val; + double final_dt = gt_j.timestamp - previous_imu_t; + if (final_dt > 1e-9 && imu_idx_end_global > imu_idx_start_global && imu_idx_end_global <= all_imu_data.size()) { + const ImuData& last_imu_in_segment = all_imu_data[imu_idx_end_global - 1]; + pim_manifold.integrateMeasurement(last_imu_in_segment.acc, last_imu_in_segment.omega, final_dt); } - agg_results.mean_actual_dt = sum_dt / actual_dt_list_const.size(); - } - - final_results_table[dataset_name][deltaTij_target] = agg_results; - agg_results.print_summary(); - } + + result.actual_dt = pim_manifold.deltaTij(); + if (result.actual_dt < 1e-6) return result; - void printFinalTable(const map>& final_results_table, - const vector& datasets, const vector& deltas) { - cout << "\n\n--- Overall Results Table (Median 15-DOF NEES) ---" << endl; + NavState estimated_state_j = pim_manifold.predict(gt_i.navState, initial_bias); + Vector9 error_nav = NavState::Logmap(gt_j.navState.inverse() * estimated_state_j); + Vector6 error_bias = gt_j.bias.vector() - initial_bias.vector(); // Bias error is (actual bias_j) - (actual bias_i) - // Header - cout << setw(20) << left << "Dataset"; - for (double dt_val : deltas) { - stringstream ss; - ss << "dT=" << fixed << setprecision(1) << dt_val << "s"; - cout << setw(12) << right << ss.str(); - } - cout << endl; - cout << string(20 + deltas.size() * 12, '-') << endl; - - // Data rows - for (const string& dataset_name : datasets) { - cout << setw(20) << left << dataset_name; - auto it_dataset = final_results_table.find(dataset_name); - if (it_dataset != final_results_table.end()) { - for (double dt_val : deltas) { - auto it_delta = it_dataset->second.find(dt_val); - if (it_delta != it_dataset->second.end()) { - if (it_delta->second.processed_intervals > 0 && isfinite(it_delta->second.median_nees)) { - cout << setw(12) << right << fixed << setprecision(3) << it_delta->second.median_nees; - } else { - cout << setw(12) << right << "N/A"; - } - } else { - cout << setw(12) << right << "-"; // No data for this deltaT - } + Vector15 error15; error15 << error_nav, error_bias; + Matrix15x15 P15 = Matrix15x15::Zero(); + P15.block<9,9>(0,0) = pim_manifold.preintMeasCov(); // This is 9x9 covariance from standard PIM + + double dt_ij = pim_manifold.deltaTij(); + // Use sensor_params directly for bias random walk variances for Manifold PIM's 15-DOF covariance + double acc_bias_rw_var_comp = pow(sensor_params.accel_bias_rw_density, 2) * dt_ij; + double gyro_bias_rw_var_comp = pow(sensor_params.gyro_bias_rw_density, 2) * dt_ij; + P15.block<3,3>(9,9) = Matrix33::Identity() * acc_bias_rw_var_comp; + P15.block<3,3>(12,12) = Matrix33::Identity() * gyro_bias_rw_var_comp; + // Off-diagonal blocks between NavState and Bias are zero for this simple construction + + if (P15.rows() != 15 || P15.cols() != 15) return result; + try { + Matrix15x15 P15_reg = P15 + Matrix15x15::Identity() * 1e-9; + Matrix15x15 P15_inv = P15_reg.inverse(); + double nees_val = error15.transpose() * P15_inv * error15; + if (!isnan(nees_val) && !isinf(nees_val) && nees_val >= 0) { + result.nees = nees_val; result.success = true; } - } else { - for (size_t i=0; i& nees_l, const vector& dt_l, + int proc, int skip_gt, int skip_imu, int num_iss, map>& tbl) { /* ... */ + AggregatedRunResults agg_r; agg_r.dataset_name = d_name; agg_r.deltaTij_target = dT_target; + agg_r.processed_intervals = proc; agg_r.skipped_intervals_gt = skip_gt; agg_r.skipped_intervals_imu = skip_imu; agg_r.numerical_issue_count = num_iss; + vector nees_m = nees_l; vector dt_m = dt_l; + if (!nees_l.empty()) { agg_r.median_nees = calculateMedian(nees_m); double s_n = 0, s_sq_n = 0; for (double n : nees_l) { s_n += n; s_sq_n += n * n; } agg_r.mean_nees = s_n / nees_l.size(); if (nees_l.size() > 1) { double var = (s_sq_n / nees_l.size()) - (agg_r.mean_nees * agg_r.mean_nees); agg_r.std_dev_nees = sqrt(max(0.0, var)); } else { agg_r.std_dev_nees = 0; }} + if (!dt_l.empty()) { agg_r.median_actual_dt = calculateMedian(dt_m); double s_dt = 0; for (double val : dt_l) s_dt += val; agg_r.mean_actual_dt = s_dt / dt_l.size(); } + tbl[d_name][dT_target] = agg_r; agg_r.print_summary(); + } + void printSingleTechniqueTable(const map>& tbl, + const vector& dsets, const vector& deltas, const string& t_name) { /* ... */ + cout << "\n\n--- Results Table for Technique: " << t_name << " (Median 15-DOF NEES) ---" << endl; + cout << setw(20) << left << "Dataset"; for (double dv : deltas) { stringstream ss; ss << "dT=" << fixed << setprecision(1) << dv << "s"; cout << setw(12) << right << ss.str(); } cout << endl; cout << string(20 + deltas.size() * 12, '-') << endl; + for (const string& d_name : dsets) { cout << setw(20) << left << d_name; auto it_d = tbl.find(d_name); + if (it_d != tbl.end()) { for (double dv : deltas) { auto it_dl = it_d->second.find(dv); if (it_dl != it_d->second.end()) { if (it_dl->second.processed_intervals > 0 && isfinite(it_dl->second.median_nees)) { cout << setw(12) << right << fixed << setprecision(3) << it_dl->second.median_nees; } else { cout << setw(12) << right << "N/A"; }} else { cout << setw(12) << right << "-"; }}} + else { for (size_t i=0; i> all_run_results; - - for (const string& dataset_name : dataset_sequences) { - cout << "\n====================================================" << endl; - cout << "Processing Dataset: " << dataset_name << endl; - cout << "====================================================" << endl; + Vector3 body_omega_Coriolis = Vector3::Zero(); // Set to non-zero if evaluating on rotating frame - string current_dataset_path = base_euroc_data_path + "/" + dataset_name; - string imu_csv_file = current_dataset_path + "/mav0/imu0/data.csv"; - string ground_truth_csv_file = current_dataset_path + "/mav0/state_groundtruth_estimate0/data.csv"; - string sensor_yaml_file = current_dataset_path + "/mav0/imu0/sensor.yaml"; + vector> techniques; + techniques.push_back(shared_ptr(new CombinedImuTechnique())); + techniques.push_back(shared_ptr(new ManifoldImuTechnique())); - // 1. Load Sensor Parameters from YAML - SensorParams sensor_params; - if (!parseSensorYaml(sensor_yaml_file, sensor_params)) { - cerr << "Failed to load sensor params for " << dataset_name << ". Skipping dataset." << endl; - continue; - } - cout << " Loaded Sensor Params for " << dataset_name << ":" << endl; - cout << " Gyro Noise Density: " << scientific << sensor_params.gyro_noise_density << endl; - cout << " Gyro Bias RW Density: " << sensor_params.gyro_bias_rw_density << endl; - cout << " Accel Noise Density: " << sensor_params.accel_noise_density << endl; - cout << " Accel Bias RW Density: " << sensor_params.accel_bias_rw_density << endl; - cout << " IMU Rate (Hz): " << fixed << setprecision(1) << sensor_params.rate_hz << endl; - // cout << " Body_P_Sensor (T_BS): \n" << sensor_params.body_P_sensor.matrix() << endl; - - - // 2. Load IMU and GT Data - vector imu_data; - vector gt_data; - if (!loadData(imu_csv_file, imu_data, parseImuLine) || imu_data.empty()) { - cerr << "Failed to load or empty IMU data for " << dataset_name << ". Skipping." << endl; - continue; - } - if (!loadData(ground_truth_csv_file, gt_data, parseGroundTruthLine) || gt_data.empty()) { - cerr << "Failed to load or empty GT data for " << dataset_name << ". Skipping." << endl; - continue; - } - - if (gt_data.front().bias.gyroscope().norm() < 1e-9 && gt_data.front().bias.accelerometer().norm() < 1e-9 && - gt_data.back().bias.gyroscope().norm() < 1e-9 && gt_data.back().bias.accelerometer().norm() < 1e-9) { - cout << "Warning: Ground truth biases are all zero for " << dataset_name << ". Bias error might be consistently zero if initial bias guess is also zero." << endl; - } + map>> all_results_by_technique; + for (const auto& tech_ptr : techniques) { + cout << "\n\n<<<<< Evaluating Technique: " << tech_ptr->name() << " >>>>>" << endl; + map> current_technique_run_results; - for (double deltaTij_target : deltaTij_values) { - cout << "\n --- Processing with Target deltaTij: " << fixed << setprecision(1) << deltaTij_target << " s ---" << endl; + for (const string& dataset_name : dataset_sequences) { + cout << "\n====================================================" << endl; + cout << "Processing Dataset: " << dataset_name << " for technique: " << tech_ptr->name() << endl; + cout << "====================================================" << endl; - // 3. Setup Preintegration Parameters for this run - auto p_combined = createCombinedImuParams(sensor_params, gravity_n, integration_noise_sigma); - // p_combined->print("CombinedParams"); // For debugging + string current_dataset_path = base_euroc_data_path + "/" + dataset_name; + string imu_csv = current_dataset_path + "/mav0/imu0/data.csv"; + string gt_csv = current_dataset_path + "/mav0/state_groundtruth_estimate0/data.csv"; + string sensor_yaml = current_dataset_path + "/mav0/imu0/sensor.yaml"; - vector current_run_nees_values; - vector current_run_actual_dt_values; - int processed_intervals_count = 0; - int skipped_gt_count = 0; - int skipped_imu_count = 0; - int numerical_issues_count = 0; - - double min_sync_time = max(imu_data.front().timestamp, gt_data.front().timestamp); - double max_sync_time = min(imu_data.back().timestamp, gt_data.back().timestamp); + SensorParams sensor_params; + if (!parseSensorYaml(sensor_yaml, sensor_params)) { + cerr << "Failed to load sensor params for " << dataset_name << ". Skipping." << endl; + continue; + } + // Print sensor params only once per dataset (e.g., for the first technique) + if (tech_ptr == techniques.front()) { + cout << " Loaded Sensor Params for " << dataset_name << ":" << endl; + cout << " Gyro Noise Density: " << scientific << sensor_params.gyro_noise_density << endl; + cout << " Gyro Bias RW Density: " << sensor_params.gyro_bias_rw_density << endl; + cout << " Accel Noise Density: " << sensor_params.accel_noise_density << endl; + cout << " Accel Bias RW Density: " << sensor_params.accel_bias_rw_density << endl; + cout << " IMU Rate (Hz): " << fixed << setprecision(1) << sensor_params.rate_hz << endl; + } - size_t first_valid_gt_idx = findIndexAtOrAfter(gt_data, min_sync_time); - if (first_valid_gt_idx >= gt_data.size()) { - cerr << " Error: No ground truth data found at or after minimum start time for " << dataset_name << ". Skipping deltaTij " << deltaTij_target << endl; + vector imu_data; vector gt_data; + if (!loadData(imu_csv, imu_data, parseImuLine) || imu_data.empty()) { + cerr << "Failed to load or empty IMU data for " << dataset_name << ". Skipping." << endl; + continue; + } + if (!loadData(gt_csv, gt_data, parseGroundTruthLine) || gt_data.empty()){ + cerr << "Failed to load or empty GT data for " << dataset_name << ". Skipping." << endl; continue; } - double current_t_i = gt_data[first_valid_gt_idx].timestamp; - cout << fixed << setprecision(9); // Higher precision for timestamps in logs - // cout << " Processing data from " << current_t_i << " s to " << max_sync_time << " s" << endl; - - - while (current_t_i + deltaTij_target <= max_sync_time + 1e-7) { // Add small tolerance for max_sync_time comparison - double t_j_nominal_end = current_t_i + deltaTij_target; + if (gt_data.front().bias.vector().norm() < 1e-9 && gt_data.back().bias.vector().norm() < 1e-9) { + cout << "Warning: Ground truth biases appear to be all zero for " << dataset_name << "." << endl; + } - size_t gt_idx_i = findIndexAtOrAfter(gt_data, current_t_i - 1e-7); // Allow slight mismatch for current_t_i lookup - if (gt_idx_i >= gt_data.size() || abs(gt_data[gt_idx_i].timestamp - current_t_i) > 0.01 ) { // If no GT close to current_t_i - // Try to advance current_t_i to the next GT sample if it's far off, or to next window - size_t next_gt_after_current = findIndexAtOrAfter(gt_data, current_t_i + 1e-7); - if(next_gt_after_current < gt_data.size() && gt_data[next_gt_after_current].timestamp < t_j_nominal_end) { - current_t_i = gt_data[next_gt_after_current].timestamp; - } else { - current_t_i = t_j_nominal_end; // Advance to next theoretical window start - } - skipped_gt_count++; - continue; - } - const GroundTruthData& gt_i_data = gt_data[gt_idx_i]; - // Sync current_t_i to actual GT timestamp for this interval's start - // This is crucial: the interval truly starts at gt_i_data.timestamp - double actual_interval_t_i = gt_i_data.timestamp; - double actual_interval_t_j_target = actual_interval_t_i + deltaTij_target; - - - size_t gt_idx_j = findIndexBefore(gt_data, actual_interval_t_j_target + 1e-7); // Find GT at or just BEFORE t_j_target - if (gt_idx_j <= gt_idx_i || gt_idx_j >= gt_data.size() || gt_data[gt_idx_j].timestamp < actual_interval_t_i + 0.1 * deltaTij_target) { // Ensure t_j is meaningfully after t_i - current_t_i = t_j_nominal_end; - skipped_gt_count++; - continue; - } - const GroundTruthData& gt_j_data = gt_data[gt_idx_j]; - double actual_interval_t_j_gt_synced = gt_j_data.timestamp; // This is the actual end time of the interval based on GT availability + for (double deltaTij_target : deltaTij_values) { + cout << "\n --- Processing with Target deltaTij: " << fixed << setprecision(1) << deltaTij_target << " s ---" << endl; + shared_ptr pim_params = + tech_ptr->createPreintegrationParams(sensor_params, gravity_n, integration_noise_sigma, body_omega_Coriolis); + + vector nees_vals; vector actual_dt_vals; + int proc_int = 0; int skip_gt = 0; int skip_imu = 0; int num_iss = 0; - // Find IMU data for [actual_interval_t_i, actual_interval_t_j_gt_synced] - size_t imu_idx_start = findIndexAtOrAfter(imu_data, actual_interval_t_i - 1e-7); // inclusive start - size_t imu_idx_end = findIndexAtOrAfter(imu_data, actual_interval_t_j_gt_synced + 1e-7); // make it inclusive for last measurement by finding one *after* + double min_t = max(imu_data.front().timestamp, gt_data.front().timestamp); + double max_t = min(imu_data.back().timestamp, gt_data.back().timestamp); + size_t first_gt_idx = findIndexAtOrAfter(gt_data, min_t); - // Ensure imu_idx_end points one past the last IMU measurement *strictly before or at* actual_interval_t_j_gt_synced. - // The original findIndexAtOrAfter gets the first element >= timestamp. - // If imu_data[imu_idx_end-1].timestamp is what we want as the last IMU data point if its timestamp is <= actual_interval_t_j_gt_synced - - // Refined imu_idx_end logic: we want IMU data up to and including measurements at actual_interval_t_j_gt_synced. - // findIndexAtOrAfter(data, ts) gives index `k` such that data[k].ts >= ts. - // So, loop from imu_idx_start up to (but not including) imu_idx_end - // This means measurements from imu_data[imu_idx_start]...imu_data[imu_idx_end-1] will be processed. - // The last IMU data point whose timestamp is <= actual_interval_t_j_gt_synced. - - if (imu_idx_start >= imu_data.size() || imu_idx_start >= imu_idx_end || (imu_idx_end == imu_idx_start && imu_data[imu_idx_start].timestamp > actual_interval_t_j_gt_synced ) || (imu_idx_end > imu_idx_start && imu_data[imu_idx_start].timestamp > actual_interval_t_j_gt_synced + 1e-7) ) { - current_t_i = t_j_nominal_end; - skipped_imu_count++; - continue; + if (first_gt_idx >= gt_data.size()) { + cerr << " Error: No ground truth data found at or after minimum start time for " << dataset_name << ". Skipping deltaTij " << deltaTij_target << endl; + continue; } - // Ensure at least one IMU measurement is processed if imu_idx_start == imu_idx_end - if (imu_idx_start == imu_idx_end) { // Only one IMU message could cover the interval - if (imu_data[imu_idx_start].timestamp < actual_interval_t_i - 1e-7 || imu_data[imu_idx_start].timestamp > actual_interval_t_j_gt_synced + 1e-7) { - current_t_i = t_j_nominal_end; - skipped_imu_count++; - continue; + double current_t_i = gt_data[first_gt_idx].timestamp; + + while (current_t_i + deltaTij_target <= max_t + 1e-7) { // Loop over time windows + double t_j_nom = current_t_i + deltaTij_target; // Nominal end time for this window + size_t gt_i_idx = findIndexAtOrAfter(gt_data, current_t_i - 1e-7); // Find GT at current_t_i + + // Check validity of GT at start of interval + if (gt_i_idx >= gt_data.size() || abs(gt_data[gt_i_idx].timestamp - current_t_i) > 0.02) { // If no GT close to current_t_i or too far + size_t next_gt = findIndexAtOrAfter(gt_data, current_t_i + 1e-7); // try to find next available GT + if(next_gt < gt_data.size() && gt_data[next_gt].timestamp < t_j_nom) { // If next GT is within current nominal window + current_t_i = gt_data[next_gt].timestamp; // Advance to it + } else { + current_t_i = t_j_nom; // Else, advance to next window's start + } + skip_gt++; continue; } - // if imu_idx_start is valid, imu_idx_end should be imu_idx_start + 1 to process this one measurement - // This case needs careful handling in integrate loop or here. - // For now, let's ensure imu_idx_end > imu_idx_start for the loop. - // The integration loop structure might need adjustment if only one IMU point is in range for a very short interval. - // But usually deltaTij is large enough for multiple IMU points. - // If imu_idx_start == imu_idx_end, it means no IMU data strictly within, or only one point at the boundary. - // The current loop `for (size_t k = imu_idx_start; k < imu_idx_end; ++k)` will not run if start == end. - // This needs to be at least 1 for the loop to run. - if (imu_idx_end <= imu_idx_start) { // ensure at least one measurement can be processed by the loop - if (imu_idx_end < imu_data.size() && imu_data[imu_idx_end].timestamp <= actual_interval_t_j_gt_synced + 1e-7) { - // This means findIndexAtOrAfter found a point at actual_interval_t_j_gt_synced, so imu_idx_end should be that index + 1 - // This logic is getting complex, the loop in evaluateCombinedImuForInterval needs to be robust. - } else { - current_t_i = t_j_nominal_end; - skipped_imu_count++; - continue; - } + const GroundTruthData& gt_i_d = gt_data[gt_i_idx]; + double act_t_i = gt_i_d.timestamp; // Actual start time of interval is this GT's timestamp + double act_t_j_target = act_t_i + deltaTij_target; // Target end time based on actual start + + // Find GT at or just BEFORE act_t_j_target + size_t gt_j_idx = findIndexBefore(gt_data, act_t_j_target + 1e-7); + + // Check validity of GT at end of interval + if (gt_j_idx <= gt_i_idx || gt_j_idx >= gt_data.size() || gt_data[gt_j_idx].timestamp < act_t_i + 0.05 * deltaTij_target) { // Ensure t_j is meaningfully after t_i + current_t_i = t_j_nom; skip_gt++; continue; } - } - - - // --- Evaluate Preintegration Method(s) for this interval --- - // Pass gt_i_data (synced t_i) and gt_j_data (synced t_j) - IntervalEvaluationResult combined_eval = evaluateCombinedImuForInterval( - gt_i_data, gt_j_data, imu_data, imu_idx_start, imu_idx_end, p_combined); - - if (combined_eval.success) { - current_run_nees_values.push_back(combined_eval.nees); - current_run_actual_dt_values.push_back(combined_eval.actual_dt); - processed_intervals_count++; - } else { - numerical_issues_count++; - } - - // Advance current_t_i for the next *non-overlapping* window - // Start next window based on the *actual start* of the current one + target duration - current_t_i = actual_interval_t_i + deltaTij_target; - } // End while loop over intervals - - // 4. Aggregate and store results for this (dataset, deltaTij) run - aggregateAndStoreResults(dataset_name, deltaTij_target, - current_run_nees_values, current_run_actual_dt_values, - processed_intervals_count, skipped_gt_count, - skipped_imu_count, numerical_issues_count, - all_run_results); - - } // End loop over deltaTij_values - } // End loop over dataset_names - - // 5. Print Final Summary Table - printFinalTable(all_run_results, dataset_sequences, deltaTij_values); - - cout << "\nEvaluation finished." << endl; - return 0; + const GroundTruthData& gt_j_d = gt_data[gt_j_idx]; + double act_t_j_gt = gt_j_d.timestamp; // Actual end time of interval, synced to this GT + + // Find IMU data for [act_t_i, act_t_j_gt] + size_t imu_start_idx = findIndexAtOrAfter(imu_data, act_t_i - 1e-7); // Inclusive start + size_t imu_end_idx = findIndexAtOrAfter(imu_data, act_t_j_gt + 1e-7); // Exclusive end (points to first element >= act_t_j_gt + tol) + + // Check for valid IMU data range + bool no_valid_imu = imu_start_idx >= imu_data.size() || // Start index out of bounds + (imu_end_idx <= imu_start_idx && // No elements in [start, end) range, AND + !(imu_start_idx < imu_data.size() && // start_idx is valid + imu_data[imu_start_idx].timestamp >= act_t_i -1e-7 && // and its timestamp is within the interval + imu_data[imu_start_idx].timestamp <= act_t_j_gt + 1e-7) + ) || + (imu_end_idx > imu_start_idx && imu_data[imu_start_idx].timestamp > act_t_j_gt + 1e-7); // First IMU is already past interval end + + if (no_valid_imu){ current_t_i = t_j_nom; skip_imu++; continue; } + + // Handle case where only one IMU measurement might cover the interval + if (imu_end_idx <= imu_start_idx) { + if (imu_start_idx < imu_data.size() && + imu_data[imu_start_idx].timestamp >= act_t_i - 1e-7 && + imu_data[imu_start_idx].timestamp <= act_t_j_gt + 1e-7) { + imu_end_idx = imu_start_idx + 1; // Ensure at least one measurement is processed if it's the only one in range + } else { + current_t_i = t_j_nom; skip_imu++; continue; + } + } + + // Evaluate for this interval + IntervalEvaluationResult eval_res = tech_ptr->evaluateInterval( + gt_i_d, gt_j_d, imu_data, imu_start_idx, imu_end_idx, + pim_params, gt_i_d.bias, sensor_params); + + if (eval_res.success) { + nees_vals.push_back(eval_res.nees); actual_dt_vals.push_back(eval_res.actual_dt); proc_int++; + } else { num_iss++; } + + // Advance current_t_i for the next non-overlapping window, based on actual start + target duration + current_t_i = act_t_i + deltaTij_target; + } + aggregateAndStoreResults(dataset_name, deltaTij_target, nees_vals, actual_dt_vals, proc_int, skip_gt, skip_imu, num_iss, current_technique_run_results); + } + } + all_results_by_technique[tech_ptr->name()] = current_technique_run_results; + } + + // Print final summary tables for each technique + for (const auto& tech_ptr : techniques) { + printSingleTechniqueTable(all_results_by_technique[tech_ptr->name()], dataset_sequences, deltaTij_values, tech_ptr->name()); + } + cout << "\nEvaluation finished." << endl; return 0; } \ No newline at end of file From c01a109812a176ead7186cdd4820849b115e24f2 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 21 May 2025 16:10:14 -0400 Subject: [PATCH 4/5] Combined, Manifold with reasonable NEES --- examples/EurocDataExample.cpp | 126 +++++++++++++++------------------- 1 file changed, 56 insertions(+), 70 deletions(-) diff --git a/examples/EurocDataExample.cpp b/examples/EurocDataExample.cpp index 5a89d4a6d8..1fb6abc8ec 100644 --- a/examples/EurocDataExample.cpp +++ b/examples/EurocDataExample.cpp @@ -1,13 +1,7 @@ /** * @file EurocDataExample.cpp * @brief Example evaluating IMU Preintegration consistency (15-DOF) on EuRoC dataset. - * Mimics the evaluation methodology from Section V-B of - * Fornasier et al., "Equivariant IMU Preintegration with Biases: - * a Galilean Group Approach" (arXiv:2411.05548v4), calculating - * 15-DOF NEES for NavState + Bias. - * This version is refactored to support multiple IMU preintegration techniques. * @author Matt Kielo, Porter Zach - * @modifier AI Assistant (Refactoring and Enhancements for Multiple Techniques) */ #include @@ -16,7 +10,7 @@ #include #include #include - #include // For PreintegratedRotationParams definition + #include #include #include @@ -33,24 +27,24 @@ #include #include #include - #include - - namespace gtsam { - typedef Eigen::Matrix Vector15; - typedef Eigen::Matrix Matrix15x15; - } + #include using namespace gtsam; using namespace std; + typedef Eigen::Matrix Vector15; + typedef Eigen::Matrix Matrix15x15; + // --- Configuration --- - const string base_euroc_data_path = "C:/Users/porte/Desktop/projects/works25/euroc/data"; // PLEASE UPDATE THIS PATH + // Update this path with the location of your EuRoC data, which should contain + // folders like "MH_01", "V2_02", etc. + const string base_euroc_data_path = "C:/gtsam/euroc/data"; const vector dataset_sequences = { "MH_01", "V2_02" }; const vector deltaTij_values = {0.2, 0.5, 1.0}; const double integration_noise_sigma = 1e-8; const Vector3 gravity_n(0, 0, -9.81); - // --- Data Structures (condensed for brevity, no changes in structure) --- + // --- Data Structures (condensed for brevity) --- struct ImuData { double timestamp; Vector3 omega; Vector3 acc; }; struct GroundTruthData { double timestamp; NavState navState; imuBias::ConstantBias bias; }; struct SensorParams { @@ -66,7 +60,7 @@ double median_actual_dt = numeric_limits::quiet_NaN(); double mean_actual_dt = numeric_limits::quiet_NaN(); int processed_intervals = 0; int skipped_intervals_gt = 0; int skipped_intervals_imu = 0; int numerical_issue_count = 0; - void print_summary() const { /* ... (implementation as before) ... */ + void print_summary() const { cout << " Summary for " << dataset_name << " (deltaTij: " << fixed << setprecision(1) << deltaTij_target << "s):" << endl; cout << " Processed Intervals: " << processed_intervals << endl; if (skipped_intervals_gt > 0) cout << " Skipped (GT Gaps): " << skipped_intervals_gt << endl; @@ -83,12 +77,12 @@ } }; - // --- Helper Functions (condensed, no changes in logic) --- - string trim(const string& str, const string& whitespace = " \t\n\r\f\v") { /* ... */ + // --- Helper Functions (condensed) --- + string trim(const string& str, const string& whitespace = " \t\n\r\f\v") { const auto strBegin = str.find_first_not_of(whitespace); if (strBegin == string::npos) return ""; const auto strEnd = str.find_last_not_of(whitespace); const auto strRange = strEnd - strBegin + 1; return str.substr(strBegin, strRange); } - bool parseSensorYaml(const string& yaml_path, SensorParams& params) { /* ... */ + bool parseSensorYaml(const string& yaml_path, SensorParams& params) { ifstream file(yaml_path); if (!file.is_open()) { cerr << "Error opening sensor YAML file: " << yaml_path << endl; return false; } string line; Matrix4 T_BS_mat = Matrix4::Identity(); bool t_bs_data_mode = false; vector t_bs_coeffs; auto extract_value = [&](const string& l, const string& key) { size_t key_pos = l.find(key); if (key_pos != string::npos) { string val_str = l.substr(key_pos + key.length()); return trim(val_str); } return string(""); }; @@ -108,25 +102,25 @@ if (t_bs_coeffs.size() >= 16) { for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { T_BS_mat(i, j) = t_bs_coeffs[i * 4 + j]; } } params.body_P_sensor = Pose3(T_BS_mat); } else { params.body_P_sensor = Pose3(); } return true; } - bool parseImuLine(const string& line, ImuData& data) { /* ... */ + bool parseImuLine(const string& line, ImuData& data) { stringstream ss(line); string segment; vector seglist; while (getline(ss, segment, ',')) seglist.push_back(segment); if (seglist.size() < 7) return false; try { data.timestamp = stod(seglist[0]) * 1e-9; data.omega = Vector3(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); data.acc = Vector3(stod(seglist[4]), stod(seglist[5]), stod(seglist[6])); } catch (const std::exception&) { return false; } return true; } - bool parseGroundTruthLine(const string& line, GroundTruthData& data) { /* ... */ + bool parseGroundTruthLine(const string& line, GroundTruthData& data) { stringstream ss(line); string segment; vector seglist; while (getline(ss, segment, ',')) seglist.push_back(segment); if (seglist.size() < 17) return false; try { data.timestamp = stod(seglist[0]) * 1e-9; Point3 pos(stod(seglist[1]), stod(seglist[2]), stod(seglist[3])); Quaternion q(stod(seglist[4]), stod(seglist[5]), stod(seglist[6]), stod(seglist[7])); Rot3 rot = Rot3(q); Velocity3 vel(stod(seglist[8]), stod(seglist[9]), stod(seglist[10])); data.navState = NavState(rot, pos, vel); Vector3 bias_gyro(stod(seglist[11]), stod(seglist[12]), stod(seglist[13])); Vector3 bias_acc(stod(seglist[14]), stod(seglist[15]), stod(seglist[16])); data.bias = imuBias::ConstantBias(bias_acc, bias_gyro); } catch (const std::exception&) { return false; } return true; } - template bool loadData(const string& filename, vector& data, bool (*parseFunc)(const string&, T&)) { /* ... */ + template bool loadData(const string& filename, vector& data, bool (*parseFunc)(const string&, T&)) { ifstream file(filename); if (!file.is_open()) { return false; } string line; if (!getline(file, line)) { return false; } while (getline(file, line)) { if (line.empty() || line[0] == '#') continue; T entry; if (parseFunc(line, entry)) data.push_back(entry); } return !data.empty(); } - template size_t findIndexBefore(const vector& sorted_data, double timestamp) { /* ... */ + template size_t findIndexBefore(const vector& sorted_data, double timestamp) { auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, [](const T& e, double v) { return e.timestamp < v; }); if (it == sorted_data.begin()) { return 0; } return distance(sorted_data.begin(), it) - 1; } - template size_t findIndexAtOrAfter(const vector& sorted_data, double timestamp) { /* ... */ + template size_t findIndexAtOrAfter(const vector& sorted_data, double timestamp) { auto it = lower_bound(sorted_data.begin(), sorted_data.end(), timestamp, [](const T& e, double v) { return e.timestamp < v; }); return distance(sorted_data.begin(), it); } - double calculateMedian(vector& values) { /* ... */ + double calculateMedian(vector& values) { if (values.empty()) return numeric_limits::quiet_NaN(); size_t n = values.size(); size_t mid = n / 2; nth_element(values.begin(), values.begin() + mid, values.end()); if (n % 2 != 0) return values[mid]; else { if (mid == 0) return values[mid]; double m1 = values[mid]; nth_element(values.begin(), values.begin() + mid - 1, values.begin() + mid); return (values[mid - 1] + m1) / 2.0; } } @@ -151,7 +145,7 @@ protected: // Sets parameters common to PreintegrationParams and its base PreintegratedRotationParams void setupBasePreintegratedRotationParams(PreintegratedRotationParams* p_rot, const SensorParams& sensor_params, - const Vector3& body_omega_coriolis) const { + const Vector3& body_omega_coriolis) const { double nominal_imu_dt = 1.0 / sensor_params.rate_hz; if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; @@ -166,32 +160,24 @@ // --- Combined IMU Factor Technique --- class CombinedImuTechnique : public ImuPreintegrationTechnique { public: - string name() const override { return "CombinedIMU (Fornasier et al. style)"; } + string name() const override { return "CombinedIMU"; } shared_ptr createPreintegrationParams( const SensorParams& sensor_params, const Vector3& gravity_n_vec, - double integration_sigma, const Vector3& body_omega_coriolis /* body_omega_coriolis effectively unused for this technique to match original */) const override { + double integration_sigma, const Vector3& body_omega_coriolis /* body_omega_coriolis effectively unused for this technique */) const override { // PreintegrationCombinedParams constructor takes gravity magnitude auto p_combined = PreintegrationCombinedParams::MakeSharedU(gravity_n_vec.norm()); - // MATCH ORIGINAL SCRIPT'S n_gravity SETTING: - // Original script assigned the potentially non-unit gravity_n_vec to p_combined->n_gravity - // after MakeSharedU (which sets n_gravity to default unit vector and g to norm). - p_combined->n_gravity = gravity_n_vec; - - // MATCH ORIGINAL SCRIPT'S COVARIANCE SETTINGS: - // (and ensure body_P_sensor and omegaCoriolis remain default/unset on p_combined, - // as original script did not explicitly set these on the PreintegrationParams object) double nominal_imu_dt = 1.0 / sensor_params.rate_hz; - if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; // Defensive, as in original + if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; p_combined->gyroscopeCovariance = pow(sensor_params.gyro_noise_density, 2) / nominal_imu_dt * I_3x3; p_combined->accelerometerCovariance = pow(sensor_params.accel_noise_density, 2) / nominal_imu_dt * I_3x3; p_combined->integrationCovariance = pow(integration_sigma, 2) * I_3x3; p_combined->biasAccCovariance = pow(sensor_params.accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; p_combined->biasOmegaCovariance = pow(sensor_params.gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; - p_combined->biasAccOmegaInt = I_6x6 * 1e-5; // Default, as in original script + p_combined->biasAccOmegaInt = I_6x6 * 1e-5; // Default // body_P_sensor and omegaCoriolis will be std::nullopt by default from // PreintegrationCombinedParams::MakeSharedU. This is consistent with the original @@ -214,14 +200,12 @@ double previous_imu_t = gt_i.timestamp; for (size_t k = imu_idx_start_global; k < imu_idx_end_global; ++k) { const ImuData& imu = all_imu_data[k]; - // MATCHING ORIGINAL SCRIPT'S TIMESTAMP CHECK (stricter lower bound) if (imu.timestamp < gt_i.timestamp || imu.timestamp > gt_j.timestamp + 1e-9) { - if (imu.timestamp < previous_imu_t && imu.timestamp >= gt_i.timestamp) previous_imu_t = imu.timestamp; // Match original condition continue; } double current_imu_t = imu.timestamp; double dt = current_imu_t - previous_imu_t; if (dt < 1e-9) { // Effectively zero dt or out of order - if (dt < 0 && abs(dt) > 1e-7) { /* cerr << "Warning: Negative IMU dt " << dt << endl; */ } // Logging diff is ok + if (dt < 0 && abs(dt) > 1e-7) { cerr << "Warning: Negative IMU dt " << dt << endl; } previous_imu_t = current_imu_t; continue; } @@ -238,10 +222,13 @@ result.actual_dt = pim_combined.deltaTij(); if (result.actual_dt < 1e-6) return result; + // For CombinedIMU, predict returns NavStateAndBias. + // The error should be calculated against both parts if using pim_combined.preintMeasCov() directly. + // To match the pattern of error_bias = gt_j.bias - initial_bias (gt_i.bias), + // ensure pim_combined.preintMeasCov() is consistent with this error definition. NavState estimated_state_j = pim_combined.predict(gt_i.navState, initial_bias); Vector9 error_nav = NavState::Logmap(gt_j.navState.inverse() * estimated_state_j); - // MATCH ORIGINAL SCRIPT'S BIAS ERROR LOGIC Vector6 error_bias = gt_j.bias.vector() - initial_bias.vector(); // initial_bias is gt_i.bias Vector15 error15; error15 << error_nav, error_bias; @@ -262,24 +249,18 @@ // --- Manifold IMU Preintegration Technique --- class ManifoldImuTechnique : public ImuPreintegrationTechnique { public: - string name() const override { return "ManifoldIMU (Forster et al. style)"; } + string name() const override { return "ManifoldIMU"; } shared_ptr createPreintegrationParams( const SensorParams& sensor_params, const Vector3& gravity_n_vec, double /* integration_sigma - not used by standard PIM */, const Vector3& body_omega_coriolis) const override { auto p_manifold = PreintegrationParams::MakeSharedU(gravity_n_vec.norm()); - - if (gravity_n_vec.norm() > 1e-9) { - p_manifold->n_gravity = gravity_n_vec.normalized(); - } else { - p_manifold->n_gravity = Vector3(0,0,-1); - } // For ManifoldIMU, using setupBasePreintegratedRotationParams is standard // as it sets body_P_sensor and omegaCoriolis if needed. setupBasePreintegratedRotationParams(static_cast(p_manifold.get()), - sensor_params, body_omega_coriolis); + sensor_params, body_omega_coriolis); double nominal_imu_dt = 1.0 / sensor_params.rate_hz; if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; @@ -303,14 +284,13 @@ double previous_imu_t = gt_i.timestamp; for (size_t k = imu_idx_start_global; k < imu_idx_end_global; ++k) { const ImuData& imu = all_imu_data[k]; - if (imu.timestamp < gt_i.timestamp -1e-9 || imu.timestamp > gt_j.timestamp + 1e-9) { // Original range slightly different, but this is for Manifold - if (imu.timestamp < previous_imu_t && imu.timestamp >= gt_i.timestamp-1e-9) previous_imu_t = imu.timestamp; + if (imu.timestamp < gt_i.timestamp -1e-9 || imu.timestamp > gt_j.timestamp + 1e-9) { continue; } double current_imu_t = imu.timestamp; double dt = current_imu_t - previous_imu_t; - if (dt < 1e-9) { - if (dt < 0 && abs(dt) > 1e-7) { /* cerr << "Warning: Negative IMU dt " << dt << endl; */ } - previous_imu_t = current_imu_t; + if (dt < 1e-9) { // Effectively zero dt or out of order (assuming data is sorted, dt < 0 should not happen often) + if (dt < 0 && abs(dt) > 1e-7) { cerr << "Warning: Negative IMU dt " << dt << endl; } + previous_imu_t = current_imu_t; // Advance to skip this measurement for dt calculation of next continue; } pim_manifold.integrateMeasurement(imu.acc, imu.omega, dt); @@ -340,7 +320,8 @@ double gyro_bias_rw_var_comp = pow(sensor_params.gyro_bias_rw_density, 2) * dt_ij; P15.block<3,3>(9,9) = Matrix33::Identity() * acc_bias_rw_var_comp; P15.block<3,3>(12,12) = Matrix33::Identity() * gyro_bias_rw_var_comp; - // Off-diagonal blocks between NavState and Bias are zero for this simple construction + // Off-diagonal blocks between NavState and Bias are zero for this simple construction, + // assuming independence between IMU measurement noise (affecting error_nav) and bias random walk noise (affecting error_bias). if (P15.rows() != 15 || P15.cols() != 15) return result; try { @@ -355,10 +336,10 @@ } }; - // --- Results Aggregation and Printing (condensed, no changes in logic) --- + // --- Results Aggregation and Printing (condensed) --- void aggregateAndStoreResults( const string& d_name, double dT_target, const vector& nees_l, const vector& dt_l, - int proc, int skip_gt, int skip_imu, int num_iss, map>& tbl) { /* ... */ + int proc, int skip_gt, int skip_imu, int num_iss, map>& tbl) { AggregatedRunResults agg_r; agg_r.dataset_name = d_name; agg_r.deltaTij_target = dT_target; agg_r.processed_intervals = proc; agg_r.skipped_intervals_gt = skip_gt; agg_r.skipped_intervals_imu = skip_imu; agg_r.numerical_issue_count = num_iss; vector nees_m = nees_l; vector dt_m = dt_l; @@ -367,7 +348,7 @@ tbl[d_name][dT_target] = agg_r; agg_r.print_summary(); } void printSingleTechniqueTable(const map>& tbl, - const vector& dsets, const vector& deltas, const string& t_name) { /* ... */ + const vector& dsets, const vector& deltas, const string& t_name) { cout << "\n\n--- Results Table for Technique: " << t_name << " (Median 15-DOF NEES) ---" << endl; cout << setw(20) << left << "Dataset"; for (double dv : deltas) { stringstream ss; ss << "dT=" << fixed << setprecision(1) << dv << "s"; cout << setw(12) << right << ss.str(); } cout << endl; cout << string(20 + deltas.size() * 12, '-') << endl; for (const string& d_name : dsets) { cout << setw(20) << left << d_name; auto it_d = tbl.find(d_name); @@ -383,6 +364,11 @@ Vector3 body_omega_Coriolis = Vector3::Zero(); // Set to non-zero if evaluating on rotating frame + // --- List of Preintegration Techniques to Evaluate --- + // **TO ADD A NEW TECHNIQUE**: + // 1. Define YourNewTechniqueClass inheriting from ImuPreintegrationTechnique. + // 2. Implement its virtual methods (name, createPreintegrationParams, evaluateInterval). + // 3. Add to this list: techniques.push_back(shared_ptr(new YourNewTechniqueClass())); vector> techniques; techniques.push_back(shared_ptr(new CombinedImuTechnique())); techniques.push_back(shared_ptr(new ManifoldImuTechnique())); @@ -408,14 +394,14 @@ cerr << "Failed to load sensor params for " << dataset_name << ". Skipping." << endl; continue; } - // Print sensor params only once per dataset (e.g., for the first technique) + // Print sensor params only once per dataset if (tech_ptr == techniques.front()) { - cout << " Loaded Sensor Params for " << dataset_name << ":" << endl; - cout << " Gyro Noise Density: " << scientific << sensor_params.gyro_noise_density << endl; - cout << " Gyro Bias RW Density: " << sensor_params.gyro_bias_rw_density << endl; - cout << " Accel Noise Density: " << sensor_params.accel_noise_density << endl; - cout << " Accel Bias RW Density: " << sensor_params.accel_bias_rw_density << endl; - cout << " IMU Rate (Hz): " << fixed << setprecision(1) << sensor_params.rate_hz << endl; + cout << " Loaded Sensor Params for " << dataset_name << ":" << endl; + cout << " Gyro Noise Density: " << scientific << sensor_params.gyro_noise_density << endl; + cout << " Gyro Bias RW Density: " << sensor_params.gyro_bias_rw_density << endl; + cout << " Accel Noise Density: " << sensor_params.accel_noise_density << endl; + cout << " Accel Bias RW Density: " << sensor_params.accel_bias_rw_density << endl; + cout << " IMU Rate (Hz): " << fixed << setprecision(1) << sensor_params.rate_hz << endl; } vector imu_data; vector gt_data; @@ -429,7 +415,7 @@ } if (gt_data.front().bias.vector().norm() < 1e-9 && gt_data.back().bias.vector().norm() < 1e-9) { - cout << "Warning: Ground truth biases appear to be all zero for " << dataset_name << "." << endl; + cout << "Warning: Ground truth biases appear to be all zero for " << dataset_name << "." << endl; } for (double deltaTij_target : deltaTij_values) { @@ -485,9 +471,9 @@ // Check for valid IMU data range bool no_valid_imu = imu_start_idx >= imu_data.size() || // Start index out of bounds (imu_end_idx <= imu_start_idx && // No elements in [start, end) range, AND - !(imu_start_idx < imu_data.size() && // start_idx is valid - imu_data[imu_start_idx].timestamp >= act_t_i -1e-7 && // and its timestamp is within the interval - imu_data[imu_start_idx].timestamp <= act_t_j_gt + 1e-7) + !(imu_start_idx < imu_data.size() && // start_idx is valid + imu_data[imu_start_idx].timestamp >= act_t_i -1e-7 && // and its timestamp is within the interval + imu_data[imu_start_idx].timestamp <= act_t_j_gt + 1e-7) ) || (imu_end_idx > imu_start_idx && imu_data[imu_start_idx].timestamp > act_t_j_gt + 1e-7); // First IMU is already past interval end From e9daa6483f4464bd7726c286493e770017626cb6 Mon Sep 17 00:00:00 2001 From: p-zach Date: Wed, 21 May 2025 16:10:41 -0400 Subject: [PATCH 5/5] Clean up and fix minor errors --- examples/EurocDataExample.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/examples/EurocDataExample.cpp b/examples/EurocDataExample.cpp index 1fb6abc8ec..2927220b61 100644 --- a/examples/EurocDataExample.cpp +++ b/examples/EurocDataExample.cpp @@ -1,6 +1,6 @@ /** * @file EurocDataExample.cpp - * @brief Example evaluating IMU Preintegration consistency (15-DOF) on EuRoC dataset. + * @brief Example evaluating IMU preintegration techniques on EuRoC dataset. * @author Matt Kielo, Porter Zach */ @@ -36,10 +36,9 @@ typedef Eigen::Matrix Matrix15x15; // --- Configuration --- - // Update this path with the location of your EuRoC data, which should contain - // folders like "MH_01", "V2_02", etc. + // Update this path with the location of your EuRoC data, which should contain folders like "MH_01", "V2_02", etc. const string base_euroc_data_path = "C:/gtsam/euroc/data"; - const vector dataset_sequences = { "MH_01", "V2_02" }; + const vector dataset_sequences = { "MH_01", /* "MH_02", "MH_03", "MH_04", "MH_05", "V1_01", "V1_02", "V1_03", "V2_01", */ "V2_02" /*, "V2_03" */ }; const vector deltaTij_values = {0.2, 0.5, 1.0}; const double integration_noise_sigma = 1e-8; const Vector3 gravity_n(0, 0, -9.81); @@ -168,21 +167,20 @@ // PreintegrationCombinedParams constructor takes gravity magnitude auto p_combined = PreintegrationCombinedParams::MakeSharedU(gravity_n_vec.norm()); + + // Set gyroscopeCovariance, omegaCoriolis, body_P_sensor + setupBasePreintegratedRotationParams(static_cast(p_combined.get()), + sensor_params, body_omega_coriolis); double nominal_imu_dt = 1.0 / sensor_params.rate_hz; if (nominal_imu_dt <= 1e-9) nominal_imu_dt = 0.005; - p_combined->gyroscopeCovariance = pow(sensor_params.gyro_noise_density, 2) / nominal_imu_dt * I_3x3; p_combined->accelerometerCovariance = pow(sensor_params.accel_noise_density, 2) / nominal_imu_dt * I_3x3; p_combined->integrationCovariance = pow(integration_sigma, 2) * I_3x3; p_combined->biasAccCovariance = pow(sensor_params.accel_bias_rw_density, 2) * nominal_imu_dt * I_3x3; p_combined->biasOmegaCovariance = pow(sensor_params.gyro_bias_rw_density, 2) * nominal_imu_dt * I_3x3; p_combined->biasAccOmegaInt = I_6x6 * 1e-5; // Default - // body_P_sensor and omegaCoriolis will be std::nullopt by default from - // PreintegrationCombinedParams::MakeSharedU. This is consistent with the original - // script not explicitly setting these on its PreintegrationCombinedParams object. - return p_combined; } @@ -257,8 +255,7 @@ auto p_manifold = PreintegrationParams::MakeSharedU(gravity_n_vec.norm()); - // For ManifoldIMU, using setupBasePreintegratedRotationParams is standard - // as it sets body_P_sensor and omegaCoriolis if needed. + // Set gyroscopeCovariance, omegaCoriolis, body_P_sensor setupBasePreintegratedRotationParams(static_cast(p_manifold.get()), sensor_params, body_omega_coriolis); @@ -365,7 +362,7 @@ Vector3 body_omega_Coriolis = Vector3::Zero(); // Set to non-zero if evaluating on rotating frame // --- List of Preintegration Techniques to Evaluate --- - // **TO ADD A NEW TECHNIQUE**: + // TO ADD A NEW TECHNIQUE: // 1. Define YourNewTechniqueClass inheriting from ImuPreintegrationTechnique. // 2. Implement its virtual methods (name, createPreintegrationParams, evaluateInterval). // 3. Add to this list: techniques.push_back(shared_ptr(new YourNewTechniqueClass())); @@ -376,7 +373,7 @@ map>> all_results_by_technique; for (const auto& tech_ptr : techniques) { - cout << "\n\n<<<<< Evaluating Technique: " << tech_ptr->name() << " >>>>>" << endl; + cout << "\n<<<<< Evaluating Technique: " << tech_ptr->name() << " >>>>>" << endl; map> current_technique_run_results; for (const string& dataset_name : dataset_sequences) {