diff --git a/eval/Dataset.cpp b/eval/Dataset.cpp index c224707..398f4be 100644 --- a/eval/Dataset.cpp +++ b/eval/Dataset.cpp @@ -41,9 +41,17 @@ void Dataset::setImuCovariances(const std::shared_ptrsetBiasOmegaCovariance(Matrix3::Identity() * std::pow(noise.sigmaGyroBias, 2)); } -std::shared_ptr Dataset::configureImuParams(double alpha) const { - auto params = PreintegrationCombinedParams::MakeSharedD(getGravity()); - params->n_gravity = Vector3(0, 0, -getGravity()); +std::shared_ptr Dataset::configureImuParams(double alpha, + const std::shared_ptr& customParams) const { + + std::shared_ptr params; + if (customParams) { + params = customParams; + } else { + params = PreintegrationCombinedParams::MakeSharedD(getGravity()); + params->n_gravity = Vector3(0, 0, -getGravity()); + } + setImuCovariances(params, computeNoiseParams(alpha)); return params; } diff --git a/eval/Dataset.h b/eval/Dataset.h index dba4f31..37169ec 100644 --- a/eval/Dataset.h +++ b/eval/Dataset.h @@ -84,9 +84,11 @@ class Dataset { /** * @brief Configure IMU preintegration parameters for this dataset * @param alpha Scaling factor for noise parameters (default: 3.0) + * @param customParams Optional custom parameters to use as base * @return Configured preintegration parameters */ - std::shared_ptr configureImuParams(double alpha = 3.0) const; + std::shared_ptr configureImuParams(double alpha = 3.0, + const std::shared_ptr& customParams = nullptr) const; private: std::vector states_; diff --git a/eval/EKFNEESEvaluator.cpp b/eval/EKFNEESEvaluator.cpp new file mode 100644 index 0000000..c97b7cd --- /dev/null +++ b/eval/EKFNEESEvaluator.cpp @@ -0,0 +1,403 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file EKFNEESEvaluator.cpp + * @brief Implementation of NEES evaluator for EKF comparison + * @author Alec Kain + */ + +#include "EKFNEESEvaluator.h" +#include +#include +#include + +using namespace std; + +namespace gtsam { + +EKFNEESEvaluator::EKFNEESEvaluator(const Dataset& dataset) : dataset_(dataset) {} + +double EKFNEESEvaluator::computeTimestep() const { + const auto& states = dataset_.getStates(); + return states[1].timestamp - states[0].timestamp; +} + +NEESEvaluator::NEESResults EKFNEESEvaluator::runGal3ImuEKF(double interval, double alpha) const { + auto params = dataset_.configureImuParams(alpha); + double dt = computeTimestep(); + return processTimeWindowWithGal3EKF(params, interval, dt, "default"); +} + +NEESEvaluator::NEESResults EKFNEESEvaluator::runGal3ImuEKF(double interval, double alpha, + const std::string& datasetName) const { + auto params = dataset_.configureImuParams(alpha); + double dt = computeTimestep(); + return processTimeWindowWithGal3EKF(params, interval, dt, datasetName); +} + +NEESEvaluator::NEESResults EKFNEESEvaluator::runNavStateImuEKF(double interval, double alpha) const { + auto params = dataset_.configureImuParams(alpha); + double dt = computeTimestep(); + return processTimeWindowWithNavStateEKF(params, interval, dt, "default"); +} + +NEESEvaluator::NEESResults EKFNEESEvaluator::runNavStateImuEKF(double interval, double alpha, + const std::string& datasetName) const { + auto params = dataset_.configureImuParams(alpha); + double dt = computeTimestep(); + return processTimeWindowWithNavStateEKF(params, interval, dt, datasetName); +} + +std::optional EKFNEESEvaluator::computeNEES(const Vector& error, + const Matrix& covarianceMatrix) const { + try { + const size_t degreesOfFreedom = error.size(); + const Matrix covarianceInverse = covarianceMatrix.inverse(); + const double nees = (error.transpose() * covarianceInverse * error)(0, 0) / degreesOfFreedom; + return (nees > 0.0) ? std::optional(nees) : std::nullopt; + } catch (...) { + return std::nullopt; + } +} + +Gal3 EKFNEESEvaluator::convertToGal3(const NavState& navState, double time) const { + return Gal3(navState.pose().rotation(), + navState.pose().translation(), + navState.velocity(), time); +} + +Gal3ImuEKF EKFNEESEvaluator::initializeGal3EKF( + const NavState& initialState, + const std::shared_ptr& params) const { + Gal3 initialGal3State = convertToGal3(initialState, 0.0); + + /// Start with zero initial covariance - perfect knowledge + Matrix initialCovariance = Matrix::Zero(10, 10); + initialCovariance(9, 9) = 0.0; + + return Gal3ImuEKF(initialGal3State, initialCovariance, params, + Gal3ImuEKF::TRACK_TIME_NO_COVARIANCE); +} + +NavStateImuEKF EKFNEESEvaluator::initializeNavStateEKF( + const NavState& initialState, + const std::shared_ptr& params) const { + Matrix9 initialCovariance = Matrix9::Zero(); + + auto navStateParams = std::make_shared(params->n_gravity); + navStateParams->accelerometerCovariance = params->accelerometerCovariance; + navStateParams->gyroscopeCovariance = params->gyroscopeCovariance; + navStateParams->integrationCovariance = params->integrationCovariance; + navStateParams->use2ndOrderCoriolis = params->use2ndOrderCoriolis; + + return NavStateImuEKF(initialState, initialCovariance, navStateParams); +} + +Vector9 EKFNEESEvaluator::computeGal3Error(const Gal3& predicted, const Gal3& groundTruth) const { + Vector10 error10 = predicted.logmap(groundTruth); + return error10.head<9>(); +} + +Matrix9 EKFNEESEvaluator::extractNavigationCovariance(const Gal3ImuEKF& ekf) const { + Matrix ekfCovariance = ekf.covariance(); + return ekfCovariance.block<9, 9>(0, 0); +} + +TrajectoryValidator::TrajectoryPoint EKFNEESEvaluator::createGroundTruthPoint( + const NavState& navState, double timestamp) const { + TrajectoryValidator::TrajectoryPoint point; + point.timestamp = timestamp; + point.position = navState.position(); + point.velocity = navState.velocity(); + point.rpy = navState.attitude().rpy() * 180.0 / M_PI; + return point; +} + +TrajectoryValidator::TrajectoryPoint EKFNEESEvaluator::createPredictedPointFromGal3( + const Gal3& gal3State, const Matrix9& covariance, double timestamp) const { + TrajectoryValidator::TrajectoryPoint point; + point.timestamp = timestamp; + point.position = gal3State.translation(); + point.velocity = gal3State.velocity(); + point.rpy = gal3State.attitude().rpy() * 180.0 / M_PI; + point.covariance = covariance; + return point; +} + +TrajectoryValidator::TrajectoryPoint EKFNEESEvaluator::createPredictedPointFromNavState( + const NavState& navState, const Matrix9& covariance, double timestamp) const { + TrajectoryValidator::TrajectoryPoint point; + point.timestamp = timestamp; + point.position = navState.position(); + point.velocity = navState.velocity(); + point.rpy = navState.attitude().rpy() * 180.0 / M_PI; + point.covariance = covariance; + return point; +} + +void EKFNEESEvaluator::exportTrajectoryResults( + const vector& groundTruthTrajectory, + const vector& predictedTrajectory, + const vector& errorTrajectory, + const std::string& filterName, + const std::string& datasetName, + const std::string& preintegrationTime) const { + + std::string filename = filterName + "_trajectory_" + datasetName + "_" + preintegrationTime + ".csv"; + std::ofstream file(filename); + + if (!file.is_open()) { + std::cerr << "Failed to open file: " << filename << std::endl; + return; + } + + // CSV header with acceleration columns + file << "timestamp,gt_x,gt_y,gt_z,gt_vx,gt_vy,gt_vz,gt_roll,gt_pitch,gt_yaw," + << "gt_ax,gt_ay,gt_az," // Add acceleration columns + << "pred_x,pred_y,pred_z,pred_vx,pred_vy,pred_vz,pred_roll,pred_pitch,pred_yaw," + << "pred_ax,pred_ay,pred_az," // Add predicted acceleration columns + << "err_x,err_y,err_z,err_vx,err_vy,err_vz,err_roll,err_pitch,err_yaw\n"; + + for (size_t i = 0; i < groundTruthTrajectory.size(); i++) { + const auto& gt = groundTruthTrajectory[i]; + const auto& pred = predictedTrajectory[i]; + const auto& err = errorTrajectory[i]; + + // Get acceleration from IMU data at this timestamp + Vector3 gt_accel = Vector3::Zero(); + Vector3 pred_accel = Vector3::Zero(); + + // Find corresponding IMU measurement + const auto& imuData = dataset_.getImuData(); + for (size_t j = 0; j < imuData.size(); j++) { + if (std::abs(imuData[j].timestamp - gt.timestamp) < 0.001) { // 1ms tolerance + gt_accel = imuData[j].acc; + pred_accel = imuData[j].acc; // For now, use same (can compute predicted later) + break; + } + } + + // Ground truth + file << std::fixed << std::setprecision(6) + << gt.timestamp << "," + << gt.position.x() << "," << gt.position.y() << "," << gt.position.z() << "," + << gt.velocity.x() << "," << gt.velocity.y() << "," << gt.velocity.z() << "," + << gt.rpy.x() << "," << gt.rpy.y() << "," << gt.rpy.z() << "," + << gt_accel.x() << "," << gt_accel.y() << "," << gt_accel.z() << ","; + + // Prediction + file << pred.position.x() << "," << pred.position.y() << "," << pred.position.z() << "," + << pred.velocity.x() << "," << pred.velocity.y() << "," << pred.velocity.z() << "," + << pred.rpy.x() << "," << pred.rpy.y() << "," << pred.rpy.z() << "," + << pred_accel.x() << "," << pred_accel.y() << "," << pred_accel.z() << ","; + + // Error (9D) + file << err[0] << "," << err[1] << "," << err[2] << "," // position error + << err[3] << "," << err[4] << "," << err[5] << "," // velocity error + << err[6] << "," << err[7] << "," << err[8] << "\n"; // orientation error + } + + file.close(); + std::cout << "✓ Exported trajectory: " << filename << std::endl; +} + +void EKFNEESEvaluator::exportNEESValues( + const std::string& filterName, + const std::string& datasetName, + const std::string& preintegrationTime, + const std::vector& trajectory, + const std::vector& neesValues) const { + + std::string filename = filterName + "_nees_" + datasetName + "_" + preintegrationTime + ".csv"; + std::ofstream file(filename); + if (!file.is_open()) { + std::cerr << "Failed to open NEES file: " << filename << std::endl; + return; + } + + file << "timestamp,nees\n"; + file << std::fixed << std::setprecision(6); + + for (size_t i = 0; i < neesValues.size(); ++i) { + size_t trajIdx = std::min(i * (trajectory.size() / neesValues.size()), + trajectory.size() - 1); + file << trajectory[trajIdx].timestamp << "," << neesValues[i] << "\n"; + } + + file.close(); + std::cout << "✓ Exported NEES values to " << filename << std::endl; +} + +NEESEvaluator::NEESResults EKFNEESEvaluator::processTimeWindowWithGal3EKF( + const std::shared_ptr& params, + double preintegrationTime, double dt, + const std::string& datasetName) const { + + vector neesValues; + vector groundTruthTrajectory; + vector predictedTrajectory; + vector errorTrajectory; + + const auto& states = dataset_.getStates(); + const auto& imuData = dataset_.getImuData(); + + /// Store COMPLETE ground truth trajectory (all points) + std::cout << "Storing complete ground truth: " << states.size() << " points" << std::endl; + for (size_t i = 0; i < states.size(); i++) { + groundTruthTrajectory.push_back( + createGroundTruthPoint(states[i].navState, states[i].timestamp)); + } + + /// Compute steps per window + const int stepsPerWindow = static_cast(preintegrationTime / dt + 0.5); + const int numCompleteWindows = (states.size() - 1) / stepsPerWindow; // Integer division + + /// Calculate actual end index (exclude incomplete final window) + const size_t actualEndIndex = numCompleteWindows * stepsPerWindow; + + std::cout << "Gal3 " << datasetName << " @ " << preintegrationTime + << "s: " << stepsPerWindow << " steps/window, " + << numCompleteWindows << " complete windows, " + << "ending at index " << actualEndIndex << "/" << (states.size() - 1) << std::endl; + + /// Initialize prediction trajectory with placeholders + predictedTrajectory.resize(states.size()); + errorTrajectory.resize(states.size()); + + /// Process each COMPLETE window + for (int windowIdx = 0; windowIdx < numCompleteWindows; windowIdx++) { + const size_t windowStart = windowIdx * stepsPerWindow; + const size_t windowEnd = windowStart + stepsPerWindow; // No min() needed + + /// RESET: Initialize new EKF at window start + Gal3ImuEKF ekf = initializeGal3EKF(states[windowStart].navState, params); + const imuBias::ConstantBias windowBias = states[windowStart].bias; + + /// Integrate through this window + for (size_t k = windowStart; k < windowEnd; k++) { + Vector3 omega = imuData[k].omega - windowBias.gyroscope(); + Vector3 acceleration = imuData[k].acc - windowBias.accelerometer(); + + ekf.predict(omega, acceleration, dt); + } + + /// Get end-of-window prediction + const Gal3 predictedGal3 = ekf.state(); + const NavState& groundTruthNavState = states[windowEnd].navState; + const Gal3 groundTruthGal3 = convertToGal3(groundTruthNavState, predictedGal3.time()); + const Vector9 navigationError = computeGal3Error(predictedGal3, groundTruthGal3); + const Matrix9 navigationCovariance = extractNavigationCovariance(ekf); + + /// FILL THE ENTIRE WINDOW with this prediction (piecewise constant) + for (size_t k = windowStart; k <= windowEnd; k++) { + predictedTrajectory[k] = createPredictedPointFromGal3( + predictedGal3, navigationCovariance, states[k].timestamp); + errorTrajectory[k] = navigationError; + } + + /// Compute NEES (only once per window) + auto nees = computeNEES(navigationError, navigationCovariance); + if (nees) { + neesValues.push_back(*nees); + if (windowIdx < 3) { + std::cout << " Window " << (windowIdx + 1) + << ": NEES = " << *nees << std::endl; + } + } + } + + std::string timeStr = std::to_string(static_cast(preintegrationTime * 10)) + "s"; + + std::cout << "Exporting: GT=" << groundTruthTrajectory.size() << std::endl; + + exportTrajectoryResults(groundTruthTrajectory, predictedTrajectory, errorTrajectory, + "gal3", datasetName, timeStr); + exportNEESValues("gal3", datasetName, timeStr, predictedTrajectory, neesValues); + + TrajectoryValidator::printErrorStatistics(errorTrajectory); + + return NEESEvaluator::computeStatistics(neesValues, preintegrationTime); +} + +/// Same for NavState +NEESEvaluator::NEESResults EKFNEESEvaluator::processTimeWindowWithNavStateEKF( + const std::shared_ptr& params, + double preintegrationTime, double dt, + const std::string& datasetName) const { + + vector neesValues; + vector groundTruthTrajectory; + vector predictedTrajectory; + vector errorTrajectory; + + const auto& states = dataset_.getStates(); + const auto& imuData = dataset_.getImuData(); + + /// Store complete ground truth + for (size_t i = 0; i < states.size(); i++) { + groundTruthTrajectory.push_back( + createGroundTruthPoint(states[i].navState, states[i].timestamp)); + } + + const int stepsPerWindow = static_cast(preintegrationTime / dt + 0.5); + const int numCompleteWindows = (states.size() - 1) / stepsPerWindow; + const size_t actualEndIndex = numCompleteWindows * stepsPerWindow; + + std::cout << "NavState " << datasetName << " @ " << preintegrationTime + << "s: " << stepsPerWindow << " steps/window, " + << numCompleteWindows << " complete windows, " + << "ending at index " << actualEndIndex << "/" << (states.size() - 1) << std::endl; + + /// Pre-allocate prediction arrays + predictedTrajectory.resize(states.size()); + errorTrajectory.resize(states.size()); + + for (int windowIdx = 0; windowIdx < numCompleteWindows; windowIdx++) { + const size_t windowStart = windowIdx * stepsPerWindow; + const size_t windowEnd = windowStart + stepsPerWindow; // No min() needed + + NavStateImuEKF ekf = initializeNavStateEKF(states[windowStart].navState, params); + const imuBias::ConstantBias windowBias = states[windowStart].bias; + + for (size_t k = windowStart; k < windowEnd; k++) { + Vector3 omega = imuData[k].omega - windowBias.gyroscope(); + Vector3 acceleration = imuData[k].acc - windowBias.accelerometer(); + ekf.predict(omega, acceleration, dt); + } + + const NavState predicted = ekf.state(); + const NavState& groundTruth = states[windowEnd].navState; + const Vector9 error = groundTruth.logmap(predicted); + const Matrix9 navigationCovariance = ekf.covariance(); + + /// Fill entire window with constant prediction + for (size_t k = windowStart; k <= windowEnd; k++) { + predictedTrajectory[k] = createPredictedPointFromNavState( + predicted, navigationCovariance, states[k].timestamp); + errorTrajectory[k] = error; + } + + auto nees = computeNEES(error, navigationCovariance); + if (nees) { + neesValues.push_back(*nees); + } + } + + std::string timeStr = std::to_string(static_cast(preintegrationTime * 10)) + "s"; + + exportTrajectoryResults(groundTruthTrajectory, predictedTrajectory, errorTrajectory, + "navstate", datasetName, timeStr); + exportNEESValues("navstate", datasetName, timeStr, predictedTrajectory, neesValues); + + TrajectoryValidator::printErrorStatistics(errorTrajectory); + + return NEESEvaluator::computeStatistics(neesValues, preintegrationTime); +} +} \ No newline at end of file diff --git a/eval/EKFNEESEvaluator.h b/eval/EKFNEESEvaluator.h new file mode 100644 index 0000000..b9c3f8a --- /dev/null +++ b/eval/EKFNEESEvaluator.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file EKFNEESEvaluator.h + * @brief NEES evaluator for EKF comparison + * @author Alec Kain + */ + +#pragma once + +#include "NEESEvaluator.h" +#include +#include +#include "TrajectoryValidator.h" + +namespace gtsam { + +/// Extended NEES evaluator for EKF comparison +class EKFNEESEvaluator { +public: + /// Window indices for processing + struct WindowIndices { + int startIndex; + int endIndex; + }; + + /// Constructor + explicit EKFNEESEvaluator(const Dataset& dataset); + + /// Compute timestep from dataset + double computeTimestep() const; + + /// Run Gal3ImuEKF evaluation (uses default dataset name) + NEESEvaluator::NEESResults runGal3ImuEKF(double interval, double alpha) const; + + /// Run Gal3ImuEKF evaluation with custom dataset name for file output + NEESEvaluator::NEESResults runGal3ImuEKF(double interval, double alpha, + const std::string& datasetName) const; + + /// Run NavStateImuEKF evaluation + NEESEvaluator::NEESResults runNavStateImuEKF(double interval, double alpha) const; + + /// Run NavStateImuEKF evaluation with custom dataset name for file output + NEESEvaluator::NEESResults runNavStateImuEKF(double interval, double alpha, + const std::string& datasetName) const; + +private: + const Dataset& dataset_; + + /// Compute NEES value + std::optional computeNEES(const Vector& error, + const Matrix& covarianceMatrix) const; + + /// Compute window indices + WindowIndices computeWindowIndices(int windowNumber, int windowSize) const; + + /// Check if window is valid + bool isWindowValid(const WindowIndices& window) const; + + /// Gal3 helper functions + Gal3 convertToGal3(const NavState& navState, double time) const; + Matrix initializeGal3Covariance() const; + Gal3ImuEKF initializeGal3EKF(const NavState& initialState, + const std::shared_ptr& params) const; + void propagateGal3EKF(Gal3ImuEKF& ekf, const WindowIndices& window, + const imuBias::ConstantBias& bias, double dt) const; + Vector9 computeGal3Error(const Gal3& predicted, const Gal3& groundTruth) const; + Matrix9 extractNavigationCovariance(const Gal3ImuEKF& ekf) const; + + /// Trajectory data management + TrajectoryValidator::TrajectoryPoint createGroundTruthPoint( + const NavState& navState, double timestamp) const; + TrajectoryValidator::TrajectoryPoint createPredictedPointFromGal3( + const Gal3& gal3State, const Matrix9& covariance, double timestamp) const; + TrajectoryValidator::TrajectoryPoint createPredictedPointFromNavState( + const NavState& navState, const Matrix9& covariance, double timestamp) const; + void storeTrajectoryData( + std::vector& groundTruthTrajectory, + std::vector& predictedTrajectory, + std::vector& errorTrajectory, + const Gal3& predictedGal3, const NavState& groundTruthNavState, + const Matrix9& navigationCovariance, const Vector9& navigationError, + double timestamp) const; + + /// Process single Gal3 window + std::optional processGal3Window( + const WindowIndices& window, + const std::shared_ptr& params, + double dt, + std::vector& groundTruthTrajectory, + std::vector& predictedTrajectory, + std::vector& errorTrajectory) const; + + /// Export trajectory results with dataset name and preintegration time + void exportTrajectoryResults( + const std::vector& groundTruthTrajectory, + const std::vector& predictedTrajectory, + const std::vector& errorTrajectory, + const std::string& filterName, + const std::string& datasetName, + const std::string& preintegrationTime) const; + + /// Export NEES values with dataset name and preintegration time + void exportNEESValues( + const std::string& filterName, + const std::string& datasetName, + const std::string& preintegrationTime, + const std::vector& trajectory, + const std::vector& neesValues) const; + + /// Process time window with Gal3ImuEKF (with dataset name) + NEESEvaluator::NEESResults processTimeWindowWithGal3EKF( + const std::shared_ptr& params, + double preintegrationTime, double dt, + const std::string& datasetName) const; + + /// NavState helper functions + NavStateImuEKF initializeNavStateEKF(const NavState& initialState, + const std::shared_ptr& params) const; + void propagateNavStateEKF(NavStateImuEKF& ekf, const WindowIndices& window, + const imuBias::ConstantBias& bias, double dt) const; + Vector9 computeNavStateError(const NavState& predicted, + const NavState& groundTruth) const; + + /// Process single NavState window + std::optional processNavStateWindow( + const WindowIndices& window, + const std::shared_ptr& params, + double dt, + std::vector& groundTruthTrajectory, + std::vector& predictedTrajectory, + std::vector& errorTrajectory) const; + + /// Process time window with NavStateImuEKF + NEESEvaluator::NEESResults processTimeWindowWithNavStateEKF( + const std::shared_ptr& params, + double preintegrationTime, double dt, + const std::string& datasetName) const; +}; + +} // namespace gtsam \ No newline at end of file diff --git a/eval/NEESEvaluator.cpp b/eval/NEESEvaluator.cpp index ac9d8a3..3566ef3 100644 --- a/eval/NEESEvaluator.cpp +++ b/eval/NEESEvaluator.cpp @@ -30,7 +30,7 @@ Vector NEESEvaluator::computeError(const NavState& predicted, const imuBias::ConstantBias& biasPred, const imuBias::ConstantBias& biasActual) const { Vector15 error; - error << predicted.localCoordinates(actual), + error << predicted.logmap(actual), biasActual.vector() - biasPred.vector(); return error; } diff --git a/eval/NEESEvaluator.h b/eval/NEESEvaluator.h index e86f292..c230fb6 100644 --- a/eval/NEESEvaluator.h +++ b/eval/NEESEvaluator.h @@ -23,6 +23,7 @@ namespace gtsam { +/// NEES evaluation for IMU preintegration class NEESEvaluator { public: /** @@ -36,20 +37,23 @@ class NEESEvaluator { double preintTime; }; - // Evaluates NEES values given a dataset (Dataset) + /// Evaluates NEES values given a dataset (Dataset) explicit NEESEvaluator(const Dataset& dataset) : dataset_(dataset) {} - // Method for running the evaluator with parameters for interval (double) and alpha size (double) - // Returns NEES results instead of printing + /// Method for running the evaluator with parameters for interval (double) and alpha size (double) + /// Returns NEES results instead of printing NEESResults run(double interval, double alpha = 3.0) const; - // Static method for printing NEES results + /// Static method for printing NEES results static void printNeesStatistics(const NEESResults& results); + + /// Compute statistics from NEES values (made public for EKF evaluator) + static NEESResults computeStatistics(const std::vector& neesResults, double preintTime); private: const Dataset& dataset_; - // Helper functions ordered "up" - used functions defined before calling functions + /// Helper functions ordered "up" - used functions defined before calling functions bool isValidWindow(int startIdx, int endIdx) const; @@ -57,7 +61,7 @@ class NEESEvaluator { const NavState& actual, const imuBias::ConstantBias& biasPred, const imuBias::ConstantBias& biasActual) const; - + std::optional computeNEES(const Vector& error, const Matrix& covMatrix) const; std::optional calculateWindowNEES(const std::shared_ptr& params, @@ -69,12 +73,10 @@ class NEESEvaluator { NEESResults processTimeWindow(const std::shared_ptr& params, double preintTime, double dt) const; - // Statistics computation helper functions + /// Statistics computation helper functions static double computeMean(const std::vector& values); static double computeMedian(const std::vector& values); static double computeVariance(const std::vector& values, double mean); - - static NEESResults computeStatistics(const std::vector& neesResults, double preintTime); }; } // namespace gtsam \ No newline at end of file diff --git a/eval/TrajectoryValidator.cpp b/eval/TrajectoryValidator.cpp new file mode 100644 index 0000000..7749054 --- /dev/null +++ b/eval/TrajectoryValidator.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file TrajectoryValidator.cpp + * @brief Implementation of trajectory validation utilities + * @author Alec Kain + */ + +#include "TrajectoryValidator.h" +#include +#include +#include +#include + +namespace gtsam { + +Vector9 TrajectoryValidator::extractStdDev(const Matrix9& covariance) { + Vector9 stdDev; + for (int i = 0; i < 9; ++i) { + stdDev(i) = std::sqrt(std::abs(covariance(i, i))); + } + return stdDev; +} + +void TrajectoryValidator::exportToCSV( + const std::string& filename, + const std::vector& groundTruthTrajectory, + const std::vector& predictedTrajectory, + const std::vector& errorTrajectory) { + + std::ofstream file(filename); + if (!file.is_open()) { + std::cerr << "Failed to open file: " << filename << std::endl; + return; + } + + std::cout << "Writing CSV: GT=" << groundTruthTrajectory.size() + << ", Pred=" << predictedTrajectory.size() + << ", Error=" << errorTrajectory.size() << std::endl; + + // Write header + file << "timestamp,gt_x,gt_y,gt_z,gt_vx,gt_vy,gt_vz,gt_roll,gt_pitch,gt_yaw," + << "pred_x,pred_y,pred_z,pred_vx,pred_vy,pred_vz,pred_roll,pred_pitch,pred_yaw," + << "err_rot_x,err_rot_y,err_rot_z,err_vel_x,err_vel_y,err_vel_z,err_pos_x,err_pos_y,err_pos_z"; + + // Add covariance std columns + file << ",rot_std_x,rot_std_y,rot_std_z,vel_std_x,vel_std_y,vel_std_z,pos_std_x,pos_std_y,pos_std_z"; + + // Add full covariance matrix + for (int i = 0; i < 9; i++) { + for (int j = 0; j < 9; j++) { + file << ",cov_" << i << "_" << j; + } + } + file << "\n"; + + file << std::fixed << std::setprecision(6); + + /// Write all rows - use ground truth size as reference (complete trajectory) + /// Predictions and errors will be empty/zeros where not computed + const size_t totalRows = groundTruthTrajectory.size(); + + for (size_t i = 0; i < totalRows; ++i) { + const auto& gt = groundTruthTrajectory[i]; + + /// Ground truth (always available) + file << gt.timestamp << "," + << gt.position.x() << "," << gt.position.y() << "," << gt.position.z() << "," + << gt.velocity.x() << "," << gt.velocity.y() << "," << gt.velocity.z() << "," + << gt.rpy.x() << "," << gt.rpy.y() << "," << gt.rpy.z(); + + /// Prediction (if available at this index) + if (i < predictedTrajectory.size()) { + const auto& pred = predictedTrajectory[i]; + file << "," << pred.position.x() << "," << pred.position.y() << "," << pred.position.z() << "," + << pred.velocity.x() << "," << pred.velocity.y() << "," << pred.velocity.z() << "," + << pred.rpy.x() << "," << pred.rpy.y() << "," << pred.rpy.z(); + } else { + // No prediction for this point (outside window) + file << ",,,,,,,,,"; + } + + /// Error (if available at this index) + if (i < errorTrajectory.size()) { + const Vector9& error = errorTrajectory[i]; + // Error vector: [rotation(3), position(3), velocity(3)] + file << "," << error(0) << "," << error(1) << "," << error(2) << "," + << error(6) << "," << error(7) << "," << error(8) << "," + << error(3) << "," << error(4) << "," << error(5); + } else { + // No error for this point + file << ",,,,,,,,,"; + } + + /// Standard deviations and covariance (if prediction available) + if (i < predictedTrajectory.size() && predictedTrajectory[i].covariance.size() > 0) { + const auto& pred = predictedTrajectory[i]; + file << "," << std::sqrt(pred.covariance(0,0)) << "," << std::sqrt(pred.covariance(1,1)) << "," << std::sqrt(pred.covariance(2,2)) + << "," << std::sqrt(pred.covariance(6,6)) << "," << std::sqrt(pred.covariance(7,7)) << "," << std::sqrt(pred.covariance(8,8)) + << "," << std::sqrt(pred.covariance(3,3)) << "," << std::sqrt(pred.covariance(4,4)) << "," << std::sqrt(pred.covariance(5,5)); + + // Full covariance + for (int row = 0; row < 9; row++) { + for (int col = 0; col < 9; col++) { + file << "," << pred.covariance(row, col); + } + } + } else { + // No covariance data + file << ",0,0,0,0,0,0,0,0,0"; + for (int row = 0; row < 81; row++) { + file << ",0"; + } + } + + file << "\n"; + } + + file.close(); + std::cout << "✓ Exported trajectory to " << filename << std::endl; +} + +void TrajectoryValidator::printErrorStatistics(const std::vector& errors) { + if (errors.empty()) { + std::cout << "No errors to analyze.\n"; + return; + } + + Vector3 rmsRotation = Vector3::Zero(); + Vector3 rmsVelocity = Vector3::Zero(); + Vector3 rmsPosition = Vector3::Zero(); + + for (const auto& error : errors) { + rmsRotation += error.head<3>().array().square().matrix(); + rmsVelocity += error.segment<3>(3).array().square().matrix(); + rmsPosition += error.tail<3>().array().square().matrix(); + } + + const size_t n = errors.size(); + rmsRotation = (rmsRotation / n).array().sqrt().matrix(); + rmsVelocity = (rmsVelocity / n).array().sqrt().matrix(); + rmsPosition = (rmsPosition / n).array().sqrt().matrix(); + + std::cout << "\n=== Error Statistics ===" << std::endl; + std::cout << std::fixed << std::setprecision(6); + std::cout << "RMS Rotation (rad): [" << rmsRotation.transpose() << "]" << std::endl; + std::cout << "RMS Velocity (m/s): [" << rmsVelocity.transpose() << "]" << std::endl; + std::cout << "RMS Position (m): [" << rmsPosition.transpose() << "]" << std::endl; +} + +} // namespace gtsam \ No newline at end of file diff --git a/eval/TrajectoryValidator.h b/eval/TrajectoryValidator.h new file mode 100644 index 0000000..2f69c4f --- /dev/null +++ b/eval/TrajectoryValidator.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file TrajectoryValidator.h + * @brief Trajectory validation utilities for EKF evaluation + * @author Alec Kain + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Trajectory validation and export utilities +class TrajectoryValidator { +public: + /// Data structure for trajectory point with covariance + struct TrajectoryPoint { + double timestamp; ///< Time in seconds + Point3 position; ///< Position in meters + Vector3 velocity; ///< Velocity in m/s + Vector3 rpy; ///< Roll-pitch-yaw in degrees + Matrix9 covariance; ///< 9x9 covariance matrix (rotation, position, velocity) + + /// Default constructor + TrajectoryPoint() + : timestamp(0.0), + position(Point3::Zero()), + velocity(Vector3::Zero()), + rpy(Vector3::Zero()), + covariance(Matrix9::Zero()) {} + }; + + /// Extract standard deviations from covariance matrix diagonal + static Vector9 extractStdDev(const Matrix9& covariance); + + /** + * Export trajectory data to CSV file + * @param filename Output CSV filename + * @param groundTruthTrajectory Ground truth trajectory points + * @param predictedTrajectory Predicted trajectory points with covariance + * @param errorTrajectory Error vectors (9D: rotation, position, velocity) + */ + static void exportToCSV( + const std::string& filename, + const std::vector& groundTruthTrajectory, + const std::vector& predictedTrajectory, + const std::vector& errorTrajectory); + + /** + * Print RMS error statistics to console + * @param errors Vector of 9D error vectors (rotation, velocity, position) + */ + static void printErrorStatistics(const std::vector& errors); +}; + +} // namespace gtsam \ No newline at end of file diff --git a/eval/ValidationReport.cpp b/eval/ValidationReport.cpp new file mode 100644 index 0000000..9a80105 --- /dev/null +++ b/eval/ValidationReport.cpp @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ValidationReport.cpp + * @brief Implementation of validation report utilities + * @author Alec Kain + */ + +#include "ValidationReport.h" + +namespace gtsam { + +void ValidationReport::printMethodResults( + const std::string& datasetName, + const std::string& methodName, + const NEESEvaluator::NEESResults& results_0_2s, + const NEESEvaluator::NEESResults& results_0_5s, + const NEESEvaluator::NEESResults& results_1_0s) { + + std::cout << std::left << std::setw(16) << datasetName + << std::setw(24) << methodName + << std::fixed << std::setprecision(3) + << std::setw(8) << results_0_2s.median + << std::setw(8) << results_0_5s.median + << std::setw(8) << results_1_0s.median + << std::endl; +} + +void ValidationReport::printDatasetResults(const DatasetResults& results) { + // Print Gal3ImuEKF results + printMethodResults( + results.datasetName, + "Gal3ImuEKF:", + results.gal3_0_2s, + results.gal3_0_5s, + results.gal3_1_0s + ); + + // Print NavStateImuEKF results + printMethodResults( + results.datasetName, + "NavStateImuEKF:", + results.navstate_0_2s, + results.navstate_0_5s, + results.navstate_1_0s + ); +} + +} // namespace gtsam \ No newline at end of file diff --git a/eval/ValidationReport.h b/eval/ValidationReport.h new file mode 100644 index 0000000..f4cbbb7 --- /dev/null +++ b/eval/ValidationReport.h @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file ValidationReport.h + * @brief Validation report utilities for NEES evaluation + * @author Alec Kain + */ + +#pragma once + +#include "NEESEvaluator.h" +#include +#include +#include + +namespace gtsam { + +/// Utilities for formatting and printing validation reports +class ValidationReport { +public: + /// Structure to hold results for a single dataset + struct DatasetResults { + std::string datasetName; + + /// Gal3ImuEKF results at different time intervals + NEESEvaluator::NEESResults gal3_0_2s; + NEESEvaluator::NEESResults gal3_0_5s; + NEESEvaluator::NEESResults gal3_1_0s; + + /// NavStateImuEKF results at different time intervals + NEESEvaluator::NEESResults navstate_0_2s; + NEESEvaluator::NEESResults navstate_0_5s; + NEESEvaluator::NEESResults navstate_1_0s; + }; + + /// Print results for a single dataset + static void printDatasetResults(const DatasetResults& results); + + /// Print results for a single method (Gal3 or NavState) + static void printMethodResults( + const std::string& datasetName, + const std::string& methodName, + const NEESEvaluator::NEESResults& results_0_2s, + const NEESEvaluator::NEESResults& results_0_5s, + const NEESEvaluator::NEESResults& results_1_0s); +}; + +} // namespace gtsam \ No newline at end of file diff --git a/eval/evalGal3NavStateImuEKFNEES.cpp b/eval/evalGal3NavStateImuEKFNEES.cpp new file mode 100644 index 0000000..b5bb178 --- /dev/null +++ b/eval/evalGal3NavStateImuEKFNEES.cpp @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file evalGal3NavStateImuEKFNEES.cpp + * @brief Clean NEES comparison table for EKF variants + * @author Alec Kain + */ + +#include "EKFNEESEvaluator.h" +#include "ValidationReport.h" +#include +#include + +using namespace gtsam; +using namespace std; + +/// NEES results for all intervals +struct DatasetResults { + NEESEvaluator::NEESResults gal3_0_2s; + NEESEvaluator::NEESResults gal3_0_5s; + NEESEvaluator::NEESResults gal3_1_0s; + NEESEvaluator::NEESResults navstate_0_2s; + NEESEvaluator::NEESResults navstate_0_5s; + NEESEvaluator::NEESResults navstate_1_0s; +}; + +/// Print table header +static void printTableHeader() { + cout << "\n" << string(70, '=') << "\n"; + cout << "Dataset\t\tMethod\t\t\t0.2s\t0.5s\t1.0s\n"; + cout << string(70, '-') << "\n"; +} + +/// Print NEES results row +static void printTableRow(const string& datasetName, + const string& methodName, + const NEESEvaluator::NEESResults& result_0_2s, + const NEESEvaluator::NEESResults& result_0_5s, + const NEESEvaluator::NEESResults& result_1_0s) { + cout << datasetName << "\t" << methodName << ":\t" + << fixed << setprecision(3) + << result_0_2s.median << "\t" + << result_0_5s.median << "\t" + << result_1_0s.median << "\n"; +} + +/// Evaluate single dataset +static DatasetResults evaluateDataset(const string& datasetPath, const string& datasetName) { + Dataset dataset(datasetPath); + EKFNEESEvaluator evaluator(dataset); + + DatasetResults results; + + // Evaluate Gal3ImuEKF - use public API with dataset name + results.gal3_0_2s = evaluator.runGal3ImuEKF(0.2, 3.0, datasetName); + results.gal3_0_5s = evaluator.runGal3ImuEKF(0.5, 3.0, datasetName); + results.gal3_1_0s = evaluator.runGal3ImuEKF(1.0, 3.0, datasetName); + + // Evaluate NavStateImuEKF - use public API with dataset name + results.navstate_0_2s = evaluator.runNavStateImuEKF(0.2, 3.0, datasetName); + results.navstate_0_5s = evaluator.runNavStateImuEKF(0.5, 3.0, datasetName); + results.navstate_1_0s = evaluator.runNavStateImuEKF(1.0, 3.0, datasetName); + + return results; +} + +/// Main evaluation program +int main(int argc, char* argv[]) { + try { + printTableHeader(); + + // Evaluate MH01 dataset + const string mh01Path = "../eval/data/euroc/euroc_MH01.csv"; + DatasetResults mh01Results = evaluateDataset(mh01Path, "MH01"); + + printTableRow("MH01", "Gal3ImuEKF", + mh01Results.gal3_0_2s, mh01Results.gal3_0_5s, mh01Results.gal3_1_0s); + printTableRow("MH01", "NavStateImuEKF", + mh01Results.navstate_0_2s, mh01Results.navstate_0_5s, mh01Results.navstate_1_0s); + + cout << string(70, '-') << "\n"; + + // Evaluate V202 dataset + const string v202Path = "../eval/data/euroc/euroc_V202.csv"; + DatasetResults v202Results = evaluateDataset(v202Path, "V202"); + + printTableRow("V202", "Gal3ImuEKF", + v202Results.gal3_0_2s, v202Results.gal3_0_5s, v202Results.gal3_1_0s); + printTableRow("V202", "NavStateImuEKF", + v202Results.navstate_0_2s, v202Results.navstate_0_5s, v202Results.navstate_1_0s); + + cout << string(70, '=') << "\n\n"; + cout << "Final table results for 0.2, 0.5, 1s preintegration times:" << "\n"; + printTableRow("MH01", "Gal3ImuEKF", + mh01Results.gal3_0_2s, mh01Results.gal3_0_5s, mh01Results.gal3_1_0s); + printTableRow("MH01", "NavStateImuEKF", + mh01Results.navstate_0_2s, mh01Results.navstate_0_5s, mh01Results.navstate_1_0s); + printTableRow("V202", "Gal3ImuEKF", + v202Results.gal3_0_2s, v202Results.gal3_0_5s, v202Results.gal3_1_0s); + printTableRow("V202", "NavStateImuEKF", + v202Results.navstate_0_2s, v202Results.navstate_0_5s, v202Results.navstate_1_0s); + + return 0; + + } catch (const exception& e) { + cerr << "Error: " << e.what() << "\n"; + return 1; + } +} \ No newline at end of file diff --git a/eval/visualize_trajectory.py b/eval/visualize_trajectory.py new file mode 100644 index 0000000..e5b9ef6 --- /dev/null +++ b/eval/visualize_trajectory.py @@ -0,0 +1,944 @@ +#!/usr/bin/env python3 +""" +Trajectory visualization - Multi-filter support with comprehensive plotting +Visualizes ground truth vs predicted trajectories and frequency analysis +""" + +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.mplot3d import Axes3D +import sys +import os + +def load_trajectory(filename): + """Load trajectory data from CSV""" + if not os.path.exists(filename): + print(f"⚠️ {filename} not found") + return None + df = pd.read_csv(filename) + return df + +def plot_3d_trajectory_single(filterName, datasetName, preintegrationTime, timeSuffix, figureNumber): + """ + Plot 3D trajectory comparing ground truth against a single preintegration time. + Each preintegration time gets its own dedicated plot. + """ + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{timeSuffix}.csv' + + if not os.path.exists(trajectoryFile): + print(f"⚠️ {trajectoryFile} not found") + return + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None: + return + + figure = plt.figure(figureNumber, figsize=(14, 10)) + axes = figure.add_subplot(111, projection='3d') + + # Plot complete ground truth trajectory - all points + axes.plot(dataFrame['gt_x'], dataFrame['gt_y'], dataFrame['gt_z'], + color='#2E4057', linewidth=4, linestyle=':', + label='Ground Truth', alpha=0.8, zorder=5) + + # Filter predictions to only VALID data (remove NaN/zeros at end) + predictionMask = (dataFrame['pred_x'].notna() & + (dataFrame['pred_x'] != 0) & + (dataFrame['pred_y'] != 0) & + (dataFrame['pred_z'] != 0)) + validPredictions = dataFrame[predictionMask] + + # Plot prediction for this preintegration time - solid line, only valid data + if len(validPredictions) > 0: + axes.plot(validPredictions['pred_x'], validPredictions['pred_y'], validPredictions['pred_z'], + color='#E63946', linewidth=2.5, linestyle='-', + label=f'{filterName.upper()} Prediction ({preintegrationTime}s)', alpha=0.85, zorder=10) + + # Mark start and end points using GROUND TRUTH (not predictions) + axes.scatter(dataFrame['gt_x'].iloc[0], dataFrame['gt_y'].iloc[0], dataFrame['gt_z'].iloc[0], + c='#06A77D', s=150, marker='o', edgecolors='white', linewidths=2, + label='Start', zorder=15) + axes.scatter(dataFrame['gt_x'].iloc[-1], dataFrame['gt_y'].iloc[-1], dataFrame['gt_z'].iloc[-1], + c='#1E88E5', s=150, marker='s', edgecolors='white', linewidths=2, + label='End', zorder=15) + + axes.set_xlabel('X (m)', fontsize=12, fontweight='bold') + axes.set_ylabel('Y (m)', fontsize=12, fontweight='bold') + axes.set_zlabel('Z (m)', fontsize=12, fontweight='bold') + axes.set_title(f'3D Trajectory: {filterName.upper()} - {datasetName}\n' + f'Ground Truth vs {preintegrationTime}s Preintegration', fontsize=14, fontweight='bold') + axes.legend(fontsize=10, loc='best', framealpha=0.95, edgecolor='gray') + axes.grid(True, alpha=0.25, linestyle='--') + axes.set_facecolor('#F5F5F5') + + filename = f'trajectory_3d_{filterName}_{datasetName}_{timeSuffix}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_3d_trajectory_combined(filterName, datasetName, figureNumber): + """ + Plot all three preintegration times on one figure for comparison + """ + figure = plt.figure(figureNumber, figsize=(18, 14)) + axes = figure.add_subplot(111, projection='3d') + + preintegrationConfigs = { + '0.2s': ('2s', '#E63946', '-'), # red solid + '0.5s': ('5s', '#F77F00', '--'), # orange dashed + '1.0s': ('10s', '#7209B7', ':') # purple dotted + } + + # Load and plot ground truth from first available file + firstTrajectory = None + for suffix in ['2s', '5s', '10s']: + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + if os.path.exists(trajectoryFile): + firstTrajectory = load_trajectory(trajectoryFile) + break + + if firstTrajectory is None: + print(f"⚠️ No trajectory files found for {filterName} - {datasetName}") + return + + # Plot complete ground truth trajectory + axes.plot(firstTrajectory['gt_x'], firstTrajectory['gt_y'], firstTrajectory['gt_z'], + color='#2E4057', linewidth=4.5, linestyle=':', + label='Ground Truth', alpha=0.75, zorder=5) + + # Plot predictions for each preintegration time (filtered) + for preintegrationLabel, (suffix, color, lineStyle) in preintegrationConfigs.items(): + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + + if not os.path.exists(trajectoryFile): + print(f"⚠️ {trajectoryFile} not found, skipping {preintegrationLabel}") + continue + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None: + continue + + # Filter to only VALID predictions + predictionMask = (dataFrame['pred_x'].notna() & + (dataFrame['pred_x'] != 0) & + (dataFrame['pred_y'] != 0) & + (dataFrame['pred_z'] != 0)) + validPredictions = dataFrame[predictionMask] + + if len(validPredictions) > 0: + axes.plot(validPredictions['pred_x'], validPredictions['pred_y'], validPredictions['pred_z'], + color=color, linestyle=lineStyle, linewidth=3, + label=f'{filterName.upper()} ({preintegrationLabel})', alpha=0.85, zorder=10) + + axes.set_xlabel('X (m)', fontsize=12, fontweight='bold') + axes.set_ylabel('Y (m)', fontsize=12, fontweight='bold') + axes.set_zlabel('Z (m)', fontsize=12, fontweight='bold') + axes.set_title(f'3D Trajectory Comparison: {filterName.upper()} - {datasetName}\n' + f'Ground Truth vs All Preintegration Times', fontsize=14, fontweight='bold') + axes.legend(fontsize=11, loc='best', framealpha=0.95, edgecolor='gray') + axes.grid(True, alpha=0.25, linestyle='--') + axes.set_facecolor('#F5F5F5') + + filename = f'trajectory_3d_combined_{filterName}_{datasetName}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_time_series_position(filter_name, dataset_name, fignum=2): + """ + Plot position (x, y, z) time series at 200 Hz for all preintegration times + """ + fig, axes = plt.subplots(3, 1, figsize=(18, 12)) + fig.suptitle(f'Position Time Series @ 200Hz - {filter_name.upper()} - {dataset_name}', + fontsize=16, fontweight='bold') + + preint_configs = { + '0.2s': ('2s', '#E63946', '--'), + '0.5s': ('5s', '#F77F00', ':'), + '1.0s': ('10s', '#7209B7', '-.') + } + + axes_labels = ['X', 'Y', 'Z'] + gt_cols = ['gt_x', 'gt_y', 'gt_z'] + pred_cols = ['pred_x', 'pred_y', 'pred_z'] + + # Load ground truth + first_traj = None + for suffix in ['2s', '5s', '10s']: + traj_file = f'{filter_name}_trajectory_{dataset_name}_{suffix}.csv' + if os.path.exists(traj_file): + first_traj = load_trajectory(traj_file) + break + + if first_traj is None: + print(f"⚠️ No trajectory files found") + return + + for i, (ax, label, gt_col) in enumerate(zip(axes, axes_labels, gt_cols)): + # Plot ground truth - SOLID + ax.plot(first_traj['timestamp'], first_traj[gt_col], + color='#2E4057', linewidth=3, linestyle='-', + label='Ground Truth', alpha=0.85, zorder=5) + + # Plot predictions for each preintegration time - DASHED + for preint_label, (suffix, color, linestyle) in preint_configs.items(): + traj_file = f'{filter_name}_trajectory_{dataset_name}_{suffix}.csv' + + if not os.path.exists(traj_file): + continue + + df = load_trajectory(traj_file) + if df is None: + continue + + # Filter to only VALID predictions (remove NaN/zeros at end) + prediction_mask = df[pred_cols[i]].notna() & (df[pred_cols[i]] != 0) + valid_predictions = df[prediction_mask] + + # Only plot if we have valid data + if len(valid_predictions) > 0: + ax.plot(valid_predictions['timestamp'], valid_predictions[pred_cols[i]], + color=color, linestyle=linestyle, linewidth=2.5, + label=f'{filter_name.upper()} ({preint_label})', alpha=0.85, zorder=10) + + ax.set_ylabel(f'{label} Position (m)', fontsize=12, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=9, loc='best', framealpha=0.95, edgecolor='gray') + ax.set_facecolor('#F5F5F5') + + if i == 2: + ax.set_xlabel('Time (s)', fontsize=12, fontweight='bold') + + plt.tight_layout() + filename = f'timeseries_position_{filter_name}_{dataset_name}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_time_series_velocity(filter_name, dataset_name, fignum=3): + """ + Plot velocity (vx, vy, vz) time series at 200 Hz for all preintegration times + """ + fig, axes = plt.subplots(3, 1, figsize=(18, 12)) + fig.suptitle(f' Veocity Time Series @ 200Hz - {filter_name.upper()} - {dataset_name}', + fontsize=16, fontweight='bold') + + preint_configs = { + '0.2s': ('2s', '#E63946', '--'), + '0.5s': ('5s', '#F77F00', ':'), + '1.0s': ('10s', '#7209B7', '-.') + } + + axes_labels = ['Vx', 'Vy', 'Vz'] + gt_cols = ['gt_vx', 'gt_vy', 'gt_vz'] + pred_cols = ['pred_vx', 'pred_vy', 'pred_vz'] + + # Get ground truth + first_traj = None + for suffix in ['2s', '5s', '10s']: + traj_file = f'{filter_name}_trajectory_{dataset_name}_{suffix}.csv' + if os.path.exists(traj_file): + first_traj = load_trajectory(traj_file) + break + + if first_traj is None: + return + + for i, (ax, label, gt_col) in enumerate(zip(axes, axes_labels, gt_cols)): + # Plot ground truth - SOLID + ax.plot(first_traj['timestamp'], first_traj[gt_col], + color='#2E4057', linewidth=3, linestyle='-', + label='Ground Truth', alpha=0.85, zorder=5) + + # Plot predictions - DASHED + for preint_label, (suffix, color, linestyle) in preint_configs.items(): + traj_file = f'{filter_name}_trajectory_{dataset_name}_{suffix}.csv' + + if not os.path.exists(traj_file): + continue + + df = load_trajectory(traj_file) + if df is None: + continue + + # Filter to only VALID predictions (remove NaN/zeros at end) + prediction_mask = df[pred_cols[i]].notna() & (df[pred_cols[i]] != 0) + valid_predictions = df[prediction_mask] + + # Only plot if we have valid data + if len(valid_predictions) > 0: + ax.plot(valid_predictions['timestamp'], valid_predictions[pred_cols[i]], + color=color, linestyle=linestyle, linewidth=2.5, + label=f'{filter_name.upper()} ({preint_label})', alpha=0.85, zorder=10) + + ax.set_ylabel(f'{label} (m/s)', fontsize=12, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=9, loc='best', framealpha=0.95, edgecolor='gray') + ax.set_facecolor('#F5F5F5') + + if i == 2: + ax.set_xlabel('Time (s)', fontsize=12, fontweight='bold') + + plt.tight_layout() + filename = f'timeseries_velocity_{filter_name}_{dataset_name}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_time_series_orientation(filter_name, dataset_name, fignum=4): + """ + Plot orientation (roll, pitch, yaw) time series at 200 Hz for all preintegration times + """ + fig, axes = plt.subplots(3, 1, figsize=(18, 12)) + fig.suptitle(f'Orientation Time Series @ 200Hz - {filter_name.upper()} - {dataset_name}', + fontsize=16, fontweight='bold') + + preint_configs = { + '0.2s': ('2s', '#E63946', '--'), + '0.5s': ('5s', '#F77F00', ':'), + '1.0s': ('10s', '#7209B7', '-.') + } + + axes_labels = ['Roll', 'Pitch', 'Yaw'] + gt_cols = ['gt_roll', 'gt_pitch', 'gt_yaw'] + pred_cols = ['pred_roll', 'pred_pitch', 'pred_yaw'] + + # Get ground truth + first_traj = None + for suffix in ['2s', '5s', '10s']: + traj_file = f'{filter_name}_trajectory_{dataset_name}_{suffix}.csv' + if os.path.exists(traj_file): + first_traj = load_trajectory(traj_file) + break + + if first_traj is None: + return + + for i, (ax, label, gt_col) in enumerate(zip(axes, axes_labels, gt_cols)): + # Plot ground truth - SOLID + ax.plot(first_traj['timestamp'], first_traj[gt_col], + color='#2E4057', linewidth=3, linestyle='-', + label='Ground Truth', alpha=0.85, zorder=5) + + # Plot predictions - DASHED + for preint_label, (suffix, color, linestyle) in preint_configs.items(): + traj_file = f'{filter_name}_trajectory_{dataset_name}_{suffix}.csv' + + if not os.path.exists(traj_file): + continue + + df = load_trajectory(traj_file) + if df is None: + continue + + # Filter to only VALID predictions (remove NaN/zeros at end) + prediction_mask = df[pred_cols[i]].notna() & (df[pred_cols[i]] != 0) + valid_predictions = df[prediction_mask] + + # Only plot if we have valid data + if len(valid_predictions) > 0: + ax.plot(valid_predictions['timestamp'], valid_predictions[pred_cols[i]], + color=color, linestyle=linestyle, linewidth=2.5, + label=f'{filter_name.upper()} ({preint_label})', alpha=0.85, zorder=10) + + ax.set_ylabel(f'{label} (deg)', fontsize=12, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=9, loc='best', framealpha=0.95, edgecolor='gray') + ax.set_facecolor('#F5F5F5') + + if i == 2: + ax.set_xlabel('Time (s)', fontsize=12, fontweight='bold') + + plt.tight_layout() + filename = f'timeseries_orientation_{filter_name}_{dataset_name}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_frequency_spectrum(filterName, datasetName, figureNumber=5): + """ + Plot frequency spectrum showing FULL 0-200 Hz range with mirrored spectrum + Shows both positive frequencies (0-100 Hz) and their mirror (100-200 Hz) + """ + figure, axes = plt.subplots(3, 3, figsize=(20, 14)) + figure.suptitle(f'Frequency Spectrum @ 200Hz - {filterName.upper()} - {datasetName}', + fontsize=16, fontweight='bold') + + # Load trajectory + trajectoryFile = None + for suffix in ['2s', '5s', '10s']: + testFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + if os.path.exists(testFile): + trajectoryFile = testFile + break + + if trajectoryFile is None: + print(f"⚠️ No trajectory files found") + return + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None: + return + + timestampDelta = dataFrame['timestamp'].iloc[1] - dataFrame['timestamp'].iloc[0] + samplingRate = 1.0 / timestampDelta + nyquistFrequency = samplingRate / 2.0 + + print(f"Sampling rate: {samplingRate:.2f} Hz") + print(f"Nyquist frequency: {nyquistFrequency:.2f} Hz") + + colors = ['#E63946', '#F77F00', '#06A77D'] + + # Position spectrum (0-200 Hz with mirroring) + positionColumns = ['gt_x', 'gt_y', 'gt_z'] + for i, column in enumerate(positionColumns): + ax = axes[0, i] + signal = dataFrame[column].values + + # Use rfft for real signals (0 to Nyquist) + fftValues = np.fft.rfft(signal) + fftFrequencies = np.fft.rfftfreq(len(signal), timestampDelta) + magnitudes = np.abs(fftValues) + + + frequencies_full = np.concatenate([ + fftFrequencies, # 0 to 100 Hz + samplingRate - fftFrequencies[-2:0:-1] # 100 to 200 Hz (mirrored) + ]) + magnitudes_full = np.concatenate([ + magnitudes, # 0 to 100 Hz + magnitudes[-2:0:-1] # Mirror (symmetric) + ]) + + # Plot full 0-200 Hz spectrum + ax.semilogy(frequencies_full, magnitudes_full, color=colors[i], linewidth=2, alpha=0.8) + + # Reference lines + ax.axvline(x=1.0, color='gray', linestyle='--', alpha=0.4, linewidth=1, label='1 Hz') + ax.axvline(x=10.0, color='purple', linestyle='--', alpha=0.4, linewidth=1, label='10 Hz') + ax.axvline(x=50.0, color='orange', linestyle='--', alpha=0.5, linewidth=1.5, label='50 Hz') + ax.axvline(x=nyquistFrequency, color='red', linestyle='-', linewidth=2, + label=f'Nyquist ({nyquistFrequency:.0f} Hz)') + ax.axvline(x=samplingRate, color='darkred', linestyle='-', linewidth=2.5, + label=f'Sampling ({samplingRate:.0f} Hz)') + + # Add shaded region for mirrored frequencies (100-200 Hz) + ax.axvspan(nyquistFrequency, samplingRate, alpha=0.15, color='blue', + label='Mirrored Spectrum') + + ax.set_xlabel('Frequency (Hz)', fontsize=10, fontweight='bold') + ax.set_ylabel('Magnitude (log scale)', fontsize=10, fontweight='bold') + ax.set_title(f'Position {["X", "Y", "Z"][i]} Spectrum', fontsize=11, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--', which='both') + ax.set_xlim([0, samplingRate]) # Full 0-200 Hz + ax.legend(fontsize=7, loc='upper right') + ax.set_facecolor('#F5F5F5') + + # Velocity spectrum (same mirroring logic) + velocityColumns = ['gt_vx', 'gt_vy', 'gt_vz'] + for i, column in enumerate(velocityColumns): + ax = axes[1, i] + signal = dataFrame[column].values + + fftValues = np.fft.rfft(signal) + fftFrequencies = np.fft.rfftfreq(len(signal), timestampDelta) + magnitudes = np.abs(fftValues) + + # Mirror spectrum + frequencies_full = np.concatenate([ + fftFrequencies, + samplingRate - fftFrequencies[-2:0:-1] + ]) + magnitudes_full = np.concatenate([ + magnitudes, + magnitudes[-2:0:-1] + ]) + + ax.semilogy(frequencies_full, magnitudes_full, color=colors[i], linewidth=2, alpha=0.8) + + ax.axvline(x=1.0, color='gray', linestyle='--', alpha=0.4, linewidth=1) + ax.axvline(x=10.0, color='purple', linestyle='--', alpha=0.4, linewidth=1) + ax.axvline(x=50.0, color='orange', linestyle='--', alpha=0.5, linewidth=1.5) + ax.axvline(x=nyquistFrequency, color='red', linestyle='-', linewidth=2, + label=f'Nyquist ({nyquistFrequency:.0f} Hz)') + ax.axvline(x=samplingRate, color='darkred', linestyle='-', linewidth=2.5, + label=f'Sampling ({samplingRate:.0f} Hz)') + + ax.axvspan(nyquistFrequency, samplingRate, alpha=0.15, color='blue', + label='Mirrored Spectrum') + + ax.set_xlabel('Frequency (Hz)', fontsize=10, fontweight='bold') + ax.set_ylabel('Magnitude (log scale)', fontsize=10, fontweight='bold') + ax.set_title(f' Veocity {["X", "Y", "Z"][i]} Spectrum', fontsize=11, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--', which='both') + ax.set_xlim([0, samplingRate]) + ax.legend(fontsize=7, loc='upper right') + ax.set_facecolor('#F5F5F5') + + # Orientation spectrum (same mirroring logic) + orientationColumns = ['gt_roll', 'gt_pitch', 'gt_yaw'] + for i, column in enumerate(orientationColumns): + ax = axes[2, i] + signal = dataFrame[column].values + + fftValues = np.fft.rfft(signal) + fftFrequencies = np.fft.rfftfreq(len(signal), timestampDelta) + magnitudes = np.abs(fftValues) + + # Mirror spectrum + frequencies_full = np.concatenate([ + fftFrequencies, + samplingRate - fftFrequencies[-2:0:-1] + ]) + magnitudes_full = np.concatenate([ + magnitudes, + magnitudes[-2:0:-1] + ]) + + ax.semilogy(frequencies_full, magnitudes_full, color=colors[i], linewidth=2, alpha=0.8) + + ax.axvline(x=1.0, color='gray', linestyle='--', alpha=0.4, linewidth=1) + ax.axvline(x=10.0, color='purple', linestyle='--', alpha=0.4, linewidth=1) + ax.axvline(x=50.0, color='orange', linestyle='--', alpha=0.5, linewidth=1.5) + ax.axvline(x=nyquistFrequency, color='red', linestyle='-', linewidth=2, + label=f'Nyquist ({nyquistFrequency:.0f} Hz)') + ax.axvline(x=samplingRate, color='darkred', linestyle='-', linewidth=2.5, + label=f'Sampling ({samplingRate:.0f} Hz)') + + ax.axvspan(nyquistFrequency, samplingRate, alpha=0.15, color='blue', + label='Mirrored Spectrum') + + ax.set_xlabel('Frequency (Hz)', fontsize=10, fontweight='bold') + ax.set_ylabel('Magnitude (log scale)', fontsize=10, fontweight='bold') + ax.set_title(f'{["Roll", "Pitch", "Yaw"][i]} Spectrum', fontsize=11, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--', which='both') + ax.set_xlim([0, samplingRate]) + ax.legend(fontsize=7, loc='upper right') + ax.set_facecolor('#F5F5F5') + + plt.tight_layout() + filename = f'frequency_spectrum_{filterName}_{datasetName}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_high_frequency_zoom(filterName, datasetName, figureNumber=6): + """ + Zoomed view of 40-200 Hz (NO SHADING, just data) + """ + figure, axes = plt.subplots(3, 3, figsize=(20, 14)) + figure.suptitle(f'High-Frequency Content (40-200 Hz) - {filterName.upper()} - {datasetName}', + fontsize=16, fontweight='bold') + + trajectoryFile = None + for suffix in ['2s', '5s', '10s']: + testFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + if os.path.exists(testFile): + trajectoryFile = testFile + break + + if trajectoryFile is None: + return + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None: + return + + timestampDelta = dataFrame['timestamp'].iloc[1] - dataFrame['timestamp'].iloc[0] + samplingRate = 1.0 / timestampDelta + nyquistFrequency = samplingRate / 2.0 + + colors = ['#E63946', '#F77F00', '#06A77D'] + + # Position high-frequency zoom (40-200 Hz) + positionColumns = ['gt_x', 'gt_y', 'gt_z'] + for i, column in enumerate(positionColumns): + ax = axes[0, i] + signal = dataFrame[column].values + + # Use rfft for real signals (only returns 0-Nyquist) + fftValues = np.fft.rfft(signal) + fftFrequencies = np.fft.rfftfreq(len(signal), timestampDelta) + magnitudes = np.abs(fftValues) + + # Filter to high-frequency range (40-200 Hz) + highFrequencyMask = (fftFrequencies >= 40) & (fftFrequencies <= samplingRate) + highFrequencies = fftFrequencies[highFrequencyMask] + highMagnitudes = magnitudes[highFrequencyMask] + + ax.plot(highFrequencies, highMagnitudes, color=colors[i], linewidth=2, alpha=0.8) + + # Mark key frequencies + ax.axvline(x=50.0, color='orange', linestyle='--', linewidth=2, label='50 Hz') + ax.axvline(x=80.0, color='purple', linestyle='--', linewidth=2, label='80 Hz') + ax.axvline(x=nyquistFrequency, color='red', linestyle='-', linewidth=2, + label=f'Nyquist ({nyquistFrequency:.0f} Hz)') + ax.axvline(x=samplingRate, color='darkred', linestyle='-', linewidth=3, + label=f'Sampling Rate ({samplingRate:.0f} Hz)') + + ax.set_xlabel('Frequency (Hz)', fontsize=10, fontweight='bold') + ax.set_ylabel('Magnitude', fontsize=10, fontweight='bold') + ax.set_title(f'Position {["X", "Y", "Z"][i]} (40-200 Hz)', fontsize=11, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.set_xlim([40, samplingRate]) # Show 40-200 Hz + ax.legend(fontsize=8) + ax.set_facecolor('#F5F5F5') + + # Velocity high-frequency zoom (40-200 Hz) + velocityColumns = ['gt_vx', 'gt_vy', 'gt_vz'] + for i, column in enumerate(velocityColumns): + ax = axes[1, i] + signal = dataFrame[column].values + + # Use rfft for real signals (only returns 0-Nyquist) + fftValues = np.fft.rfft(signal) + fftFrequencies = np.fft.rfftfreq(len(signal), timestampDelta) + magnitudes = np.abs(fftValues) + + highFrequencyMask = (fftFrequencies >= 40) & (fftFrequencies <= samplingRate) + highFrequencies = fftFrequencies[highFrequencyMask] + highMagnitudes = magnitudes[highFrequencyMask] + + ax.plot(highFrequencies, highMagnitudes, color=colors[i], linewidth=2, alpha=0.8) + + ax.axvline(x=50.0, color='orange', linestyle='--', linewidth=2, label='50 Hz') + ax.axvline(x=80.0, color='purple', linestyle='--', linewidth=2, label='80 Hz') + ax.axvline(x=nyquistFrequency, color='red', linestyle='-', linewidth=2, + label=f'Nyquist ({nyquistFrequency:.0f} Hz)') + ax.axvline(x=samplingRate, color='darkred', linestyle='-', linewidth=3, + label=f'Sampling Rate ({samplingRate:.0f} Hz)') + + ax.set_xlabel('Frequency (Hz)', fontsize=10, fontweight='bold') + ax.set_ylabel('Magnitude', fontsize=10, fontweight='bold') + ax.set_title(f' Veocity {["X", "Y", "Z"][i]} (40-200 Hz)', fontsize=11, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=8) + ax.set_facecolor('#F5F5F5') + + # Orientation high-frequency zoom (40-200 Hz) + orientationColumns = ['gt_roll', 'gt_pitch', 'gt_yaw'] + for i, column in enumerate(orientationColumns): + ax = axes[2, i] + signal = dataFrame[column].values + + # Use rfft for real signals (only returns 0-Nyquist) + fftValues = np.fft.rfft(signal) + fftFrequencies = np.fft.rfftfreq(len(signal), timestampDelta) + magnitudes = np.abs(fftValues) + + highFrequencyMask = (fftFrequencies >= 40) & (fftFrequencies <= samplingRate) + highFrequencies = fftFrequencies[highFrequencyMask] + highMagnitudes = magnitudes[highFrequencyMask] + + ax.plot(highFrequencies, highMagnitudes, color=colors[i], linewidth=2, alpha=0.8) + + ax.axvline(x=50.0, color='orange', linestyle='--', linewidth=2, label='50 Hz') + ax.axvline(x=80.0, color='purple', linestyle='--', linewidth=2, label='80 Hz') + ax.axvline(x=nyquistFrequency, color='red', linestyle='-', linewidth=2, + label=f'Nyquist ({nyquistFrequency:.0f} Hz)') + ax.axvline(x=samplingRate, color='darkred', linestyle='-', linewidth=3, + label=f'Sampling Rate ({samplingRate:.0f} Hz)') + + ax.set_xlabel('Frequency (Hz)', fontsize=10, fontweight='bold') + ax.set_ylabel('Magnitude', fontsize=10, fontweight='bold') + ax.set_title(f'{["Roll", "Pitch", "Yaw"][i]} (40-200 Hz)', fontsize=11, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=8) + ax.set_facecolor('#F5F5F5') + + plt.tight_layout() + filename = f'frequency_spectrum_highfreq_{filterName}_{datasetName}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_time_series_acceleration(filterName, datasetName, figureNumber=7): + """ + Plot accelerometer measurements (ax, ay, az) time series at 200 Hz + Shows ground truth acceleration vs time with distinct colors and spacing + """ + figure, axes = plt.subplots(3, 1, figsize=(18, 12)) + figure.suptitle(f'Acceleration Time Series @ 200Hz - {filterName.upper()} - {datasetName}', + fontsize=16, fontweight='bold') + + preintegrationConfigs = { + '0.2s': ('2s', '#E63946', '--'), # Red + '0.5s': ('5s', '#F77F00', '-.'), # Orange + '1.0s': ('10s', '#7209B7', ':') # Purple + } + + axesLabels = ['Ax', 'Ay', 'Az'] + gtColumns = ['gt_ax', 'gt_ay', 'gt_az'] + predColumns = ['pred_ax', 'pred_ay', 'pred_az'] + + # Load ground truth from first available file + firstTrajectory = None + for suffix in ['2s', '5s', '10s']: + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + if os.path.exists(trajectoryFile): + firstTrajectory = load_trajectory(trajectoryFile) + break + + if firstTrajectory is None: + print(f"⚠️ No trajectory files found for acceleration plot") + return + + for i, (ax, label, gtColumn) in enumerate(zip(axes, axesLabels, gtColumns)): + # Check if acceleration columns exist + if gtColumn not in firstTrajectory.columns: + ax.text(0.5, 0.5, f'No {label} data available', + ha='center', va='center', fontsize=14, color='red') + ax.set_ylabel(f'{label} (m/s²)', fontsize=12, fontweight='bold') + continue + + # Plot ground truth - THICK SOLID BLACK + ax.plot(firstTrajectory['timestamp'], firstTrajectory[gtColumn], + color='#000000', linewidth=4, linestyle='-', + label='Ground Truth', alpha=1, zorder=1) + + # Plot predictions with vertical offsets and distinct styling + offsetValues = [0.0, 0.3, -0.3] # Small vertical offsets for visual separation + + for idx, (preintegrationLabel, (suffix, color, lineStyle)) in enumerate(preintegrationConfigs.items()): + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + + if not os.path.exists(trajectoryFile): + continue + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None or predColumns[i] not in dataFrame.columns: + continue + + # Filter to only VALID predictions + predictionMask = dataFrame[predColumns[i]].notna() & (dataFrame[predColumns[i]] != 0) + validPredictions = dataFrame[predictionMask] + + # Only plot if we have valid data + if len(validPredictions) > 0: + # Apply small vertical offset for visual separation + accelerationData = validPredictions[predColumns[i]] + offsetValues[idx] + + ax.plot(validPredictions['timestamp'], accelerationData, + color=color, linestyle=lineStyle, linewidth=3.5, + label=f'{filterName.upper()} ({preintegrationLabel})', + alpha=0.4, zorder=10 + idx) + + ax.set_ylabel(f'{label} (m/s²)', fontsize=12, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=10, loc='best', framealpha=0.95, edgecolor='gray') + ax.set_facecolor('#F8F8F8') + + # Add horizontal reference line at zero + ax.axhline(y=0, color='gray', linestyle='--', linewidth=1, alpha=0.4) + + if i == 2: + ax.set_xlabel('Time (s)', fontsize=12, fontweight='bold') + + plt.tight_layout() + filename = f'timeseries_acceleration_{filterName}_{datasetName}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_time_series_displacement(filterName, datasetName, figureNumber=8): + """ + Plot displacement/delta position (dx, dy, dz) time series at 200 Hz + Shows incremental position changes between consecutive predictions + """ + figure, axes = plt.subplots(3, 1, figsize=(18, 12)) + figure.suptitle(f'Displacement (Δ Position) Time Series @ 200Hz - {filterName.upper()} - {datasetName}', + fontsize=16, fontweight='bold') + + preintegrationConfigs = { + '0.2s': ('2s', '#E63946', '--'), + '0.5s': ('5s', '#F77F00', ':'), + '1.0s': ('10s', '#7209B7', '-.') + } + + axesLabels = ['ΔX', 'ΔY', 'ΔZ'] + gtPositionColumns = ['gt_x', 'gt_y', 'gt_z'] + predPositionColumns = ['pred_x', 'pred_y', 'pred_z'] + + # Load ground truth from first available file + firstTrajectory = None + for suffix in ['2s', '5s', '10s']: + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + if os.path.exists(trajectoryFile): + firstTrajectory = load_trajectory(trajectoryFile) + break + + if firstTrajectory is None: + print(f"⚠️ No trajectory files found for displacement plot") + return + + for i, (ax, label, gtColumn) in enumerate(zip(axes, axesLabels, gtPositionColumns)): + # Compute ground truth displacement (difference between consecutive positions) + gtDisplacement = firstTrajectory[gtColumn].diff() + + # Plot ground truth displacement - SOLID + ax.plot(firstTrajectory['timestamp'], gtDisplacement, + color="#070707", linewidth=3, linestyle='-', + label='Ground Truth Δ', alpha=1, zorder=1) + + # Plot predictions for each preintegration time - DASHED + for preintegrationLabel, (suffix, color, lineStyle) in preintegrationConfigs.items(): + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + + if not os.path.exists(trajectoryFile): + continue + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None: + continue + + # Filter to only VALID predictions + predictionMask = dataFrame[predPositionColumns[i]].notna() & (dataFrame[predPositionColumns[i]] != 0) + validPredictions = dataFrame[predictionMask] + + # Compute predicted displacement + if len(validPredictions) > 0: + predDisplacement = validPredictions[predPositionColumns[i]].diff() + + ax.plot(validPredictions['timestamp'], predDisplacement, + color=color, linestyle=lineStyle, linewidth=2.5, + label=f'{filterName.upper()} ({preintegrationLabel})', alpha=0.5, zorder=10) + + ax.set_ylabel(f'{label} (m)', fontsize=12, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=9, loc='best', framealpha=0.95, edgecolor='gray') + ax.set_facecolor('#F5F5F5') + ax.axhline(y=0, color='gray', linestyle='--', linewidth=1, alpha=0.5) # Zero reference + + if i == 2: + ax.set_xlabel('Time (s)', fontsize=12, fontweight='bold') + + plt.tight_layout() + filename = f'timeseries_displacement_{filterName}_{datasetName}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def plot_time_series_integrated_displacement(filterName, datasetName, figureNumber=9): + """ + Plot cumulative displacement from t=0 (integrated velocity) + Shows total distance traveled in each axis + """ + figure, axes = plt.subplots(3, 1, figsize=(18, 12)) + figure.suptitle(f'Cumulative Displacement from t=0 @ 200Hz - {filterName.upper()} - {datasetName}', + fontsize=16, fontweight='bold') + + preintegrationConfigs = { + '0.2s': ('2s', '#E63946', '--'), + '0.5s': ('5s', '#F77F00', ':'), + '1.0s': ('10s', '#7209B7', '-.') + } + + axesLabels = ['X', 'Y', 'Z'] + gtPositionColumns = ['gt_x', 'gt_y', 'gt_z'] + predPositionColumns = ['pred_x', 'pred_y', 'pred_z'] + + # Load ground truth from first available file + firstTrajectory = None + for suffix in ['2s', '5s', '10s']: + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + if os.path.exists(trajectoryFile): + firstTrajectory = load_trajectory(trajectoryFile) + break + + if firstTrajectory is None: + print(f"⚠️ No trajectory files found for integrated displacement plot") + return + + for i, (ax, label, gtColumn) in enumerate(zip(axes, axesLabels, gtPositionColumns)): + # Compute cumulative displacement from initial position + initialPosition = firstTrajectory[gtColumn].iloc[0] + gtCumulativeDisplacement = firstTrajectory[gtColumn] - initialPosition + + # Plot ground truth cumulative displacement - SOLID + ax.plot(firstTrajectory['timestamp'], gtCumulativeDisplacement, + color='#2E4057', linewidth=3, linestyle='-', + label='Ground Truth', alpha=0.85, zorder=5) + + # Plot predictions for each preintegration time - DASHED + for preintegrationLabel, (suffix, color, lineStyle) in preintegrationConfigs.items(): + trajectoryFile = f'{filterName}_trajectory_{datasetName}_{suffix}.csv' + + if not os.path.exists(trajectoryFile): + continue + + dataFrame = load_trajectory(trajectoryFile) + if dataFrame is None: + continue + + # Filter to only VALID predictions + predictionMask = dataFrame[predPositionColumns[i]].notna() & (dataFrame[predPositionColumns[i]] != 0) + validPredictions = dataFrame[predictionMask] + + if len(validPredictions) > 0: + # Compute cumulative displacement from initial prediction + predInitialPosition = validPredictions[predPositionColumns[i]].iloc[0] + predCumulativeDisplacement = validPredictions[predPositionColumns[i]] - predInitialPosition + + ax.plot(validPredictions['timestamp'], predCumulativeDisplacement, + color=color, linestyle=lineStyle, linewidth=2.5, + label=f'{filterName.upper()} ({preintegrationLabel})', alpha=0.85, zorder=10) + + ax.set_ylabel(f'{label} Cumulative (m)', fontsize=12, fontweight='bold') + ax.grid(True, alpha=0.25, linestyle='--') + ax.legend(fontsize=9, loc='best', framealpha=0.95, edgecolor='gray') + ax.set_facecolor('#F5F5F5') + ax.axhline(y=0, color='gray', linestyle='--', linewidth=1, alpha=0.5) + + if i == 2: + ax.set_xlabel('Time (s)', fontsize=12, fontweight='bold') + + plt.tight_layout() + filename = f'timeseries_cumulative_displacement_{filterName}_{datasetName}.png' + plt.savefig(filename, dpi=300, bbox_inches='tight') + print(f"✓ Saved {filename}") + +def process_filter_dataset(filterName, datasetName): + """Process a single filter-dataset combination for all preintegration times""" + print(f"\n{'='*80}") + print(f" Processing {filterName} - {datasetName}") + print('='*80) + + # Generate figure numbers + figureNumberBase = (100 if filterName == "gal3" else 200) + \ + (0 if datasetName == "MH01" else 50) + + # Plot separate 3D trajectories for each preintegration time + preintegrationConfigs = [ + ('0.2', '2s'), + ('0.5', '5s'), + ('1.0', '10s') + ] + + for idx, (preint_time, time_suffix) in enumerate(preintegrationConfigs): + plot_3d_trajectory_single(filterName, datasetName, preint_time, + time_suffix, figureNumberBase + idx + 1) + + # Also create combined plot showing all three + plot_3d_trajectory_combined(filterName, datasetName, figureNumberBase + 10) + + # Time series plots + plot_time_series_position(filterName, datasetName, figureNumberBase + 20) + plot_time_series_velocity(filterName, datasetName, figureNumberBase + 30) + plot_time_series_orientation(filterName, datasetName, figureNumberBase + 40) + + # Frequency spectrum analysis + plot_frequency_spectrum(filterName, datasetName, figureNumberBase + 50) + plot_high_frequency_zoom(filterName, datasetName, figureNumberBase + 60) + + # NEW: Acceleration and displacement plots + plot_time_series_acceleration(filterName, datasetName, figureNumberBase + 70) + plot_time_series_displacement(filterName, datasetName, figureNumberBase + 80) + plot_time_series_integrated_displacement(filterName, datasetName, figureNumberBase + 90) + +if __name__ == '__main__': + # Enable interactive mode for displaying plots + plt.ion() + + # Process all filter-dataset combinations + for filterName in ["gal3", "navstate"]: + for datasetName in ["MH01", "V202"]: + process_filter_dataset(filterName, datasetName) + + print(f"\n{'='*80}") + print("✓ All visualizations complete!") + print('='*80) + + # Keep plots open + plt.show(block=True) \ No newline at end of file