From 27daba075473c4c22cf55bec7fb82088c608f17f Mon Sep 17 00:00:00 2001 From: avnishnp Date: Fri, 25 Jul 2025 18:31:14 -0400 Subject: [PATCH 1/4] Relative pose graph ImuPreintegration --- super_odometry/CMakeLists.txt | 2 +- .../imuPreintegration_relative.h | 191 +++++ .../imuPreintegration_relative.cpp | 746 ++++++++++++++++++ 3 files changed, 938 insertions(+), 1 deletion(-) create mode 100644 super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h create mode 100644 super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp diff --git a/super_odometry/CMakeLists.txt b/super_odometry/CMakeLists.txt index 46be14c..b70364a 100755 --- a/super_odometry/CMakeLists.txt +++ b/super_odometry/CMakeLists.txt @@ -79,7 +79,7 @@ set(GTSAM_LIBRARIES gtsam) # SuperOdom Library add_library(SuperOdomLib SHARED src/FeatureExtraction/featureExtraction.cpp - src/ImuPreintegration/imuPreintegration.cpp + src/ImuPreintegration/imuPreintegration_relative.cpp src/LaserMapping/laserMapping.cpp src/LaserMapping/lidarOptimization.cpp src/LaserMapping/LocalMap.cpp diff --git a/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h b/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h new file mode 100644 index 0000000..79bf6cc --- /dev/null +++ b/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h @@ -0,0 +1,191 @@ +// +// Header for Pure Relative Pose Graph Implementation +// +#pragma once +#ifndef IMUPREINTEGRATION_H +#define IMUPREINTEGRATION_H + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include + +#include "utility.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "super_odometry/utils/Twist.h" +#include "super_odometry/container/MapRingBuffer.h" +#include "super_odometry/config/parameter.h" +#include "super_odometry/tic_toc.h" +#include +#include "super_odometry/sensor_data/imu/imu_data.h" + +namespace super_odometry { + + using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) + using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) + using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) + using FrameId = std::uint64_t; + + struct imuPreintegration_config{ + float imuAccNoise; + float imuAccBiasN; + float imuGyrNoise; + float imuGyrBiasN; + float imuGravity; + float lidar_correction_noise; + float smooth_factor; + bool use_imu_roll_pitch; + SensorType sensor; + + double imu_acc_x_limit; + double imu_acc_y_limit; + double imu_acc_z_limit; + }; + + class imuPreintegration : public rclcpp::Node { + public: + imuPreintegration(const rclcpp::NodeOptions & options); + + static constexpr double delta_t = 0; + static constexpr double imu_laser_timedelay= 0.8; + + public: + void initInterface(); + bool readParameters(); + void laserodometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg); + void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imu_raw); + void initial_system(double currentCorrectionTime, gtsam::Pose3 lidarPose); + void process_imu_odometry(double currentCorrectionTime, gtsam::Pose3 lidarPose); + bool build_graph(gtsam::Pose3 lidarPose, double curLaserodomtimestamp); + void repropagate_imuodometry(double currentCorrectionTime); + bool failureDetection(const gtsam::Vector3 &velCur, + const gtsam::imuBias::ConstantBias &biasCur); + void integrate_imumeasurement(double currentCorrectionTime); + void reset_graph(); + void resetOptimization(); + void resetParams(); + bool handleIMUInitialization(const sensor_msgs::msg::Imu::SharedPtr&imu_raw, + sensor_msgs::msg::Imu& thisImu); + void updateAndPublishPath(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); + void publishTransform(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); + + // Modified for relative pose graph - includes world pose parameter + void prepareOdometryMessage(nav_msgs::msg::Odometry &odometry, + const sensor_msgs::msg::Imu& thisImu, + const gtsam::NavState ¤tStateLocal, + const gtsam::Pose3& currentWorldPose); + + void publishTransformsAndPath(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); + void processTiming(const sensor_msgs::msg::Imu& thisImu); + void initializeImu(const sensor_msgs::msg::Imu::SharedPtr& imu_raw); + void correctLivoxGravity(sensor_msgs::msg::Imu& thisImu); + sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu &imu_in); + + template + double secs(T msg) { + return msg->header.stamp.sec + msg->header.stamp.nanosec*1e-9; + } + + private: + rclcpp::Subscription::SharedPtr subImu; + rclcpp::Subscription::SharedPtr subLaserOdometry; + + rclcpp::Publisher::SharedPtr pubImuOdometry; + rclcpp::Publisher::SharedPtr pubHealthStatus; + rclcpp::Publisher::SharedPtr pubImuPath; + + rclcpp::CallbackGroup::SharedPtr cb_group_; + + public: + gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; + gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; + gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; + gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; + gtsam::Vector noiseModelBetweenBias; + std::shared_ptr imuIntegratorOpt_; + std::shared_ptr imuIntegratorImu_; + + // Graph nodes store relative poses + gtsam::Pose3 prevPose_; // Previous pose in local frame + gtsam::Vector3 prevVel_; + gtsam::NavState prevState_; + gtsam::imuBias::ConstantBias prevBias_; + gtsam::NavState prevStateOdom; + gtsam::imuBias::ConstantBias prevBiasOdom; + + gtsam::ISAM2 optimizer; + gtsam::NonlinearFactorGraph graphFactors; + gtsam::Values graphValues; + + // Lidar odometry poses + gtsam::Pose3 lidarodom_w_pre; + gtsam::Pose3 lidarodom_w_cur; + + // World pose tracking (outside of graph) + gtsam::Pose3 worldPose; // Current pose in world frame + gtsam::Pose3 lastLidarPose; // Last lidar pose for relative computation + + public: + // Extrinsic calibration + gtsam::Pose3 imu2cam; + gtsam::Pose3 cam2Lidar; + gtsam::Pose3 imu2Lidar; + gtsam::Pose3 lidar2Imu; + + public: + MapRingBuffer imuBuf; + std::deque imuQueOpt; + std::deque imuQueImu; + MapRingBuffer lidarOdomBuf; + std::mutex mBuf; + Imu::Ptr imu_Init = std::make_shared(); + + public: + bool systemInitialized = false; + bool doneFirstOpt = false; + bool health_status = true; + bool imu_init_success = false; + + Eigen::Quaterniond firstImu; + Eigen::Vector3d gyr_pre; + + double first_imu_time_stamp; + double last_processed_lidar_time = -1; + double lastImuT_imu = -1; + double lastImuT_opt = -1; + int key = 1; + int imuPreintegrationResetId = 0; + int frame_count = 0; + + enum IMU_STATE : uint8_t { + FAIL=0, + SUCCESS=1, + UNKNOW=2 + }; + + IMU_STATE RESULT; + nav_msgs::msg::Odometry::SharedPtr cur_frame = nullptr; + nav_msgs::msg::Odometry::SharedPtr last_frame = nullptr; + imuPreintegration_config config_; + }; + +} + +#endif // IMUPREINTEGRATION_H \ No newline at end of file diff --git a/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp b/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp new file mode 100644 index 0000000..77e1ed6 --- /dev/null +++ b/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp @@ -0,0 +1,746 @@ +// +// Relative Pose Graph Implementation +// All poses are relative in the graph +// +#include "super_odometry/ImuPreintegration/imuPreintegration_relative.h" + +namespace super_odometry { + + imuPreintegration::imuPreintegration(const rclcpp::NodeOptions & options) + : Node("imu_preintegration_node", options) { + } + + void imuPreintegration::initInterface() { + //! Callback Groups + cb_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + rclcpp::QoS imu_qos(10); + imu_qos.best_effort(); // Use BEST_EFFORT reliability + imu_qos.keep_last(10); // Keep last 10 messages + + if (!readGlobalparam(shared_from_this())) { + RCLCPP_ERROR(this->get_logger(), "[SuperOdometry::imuPreintegration] Could not read global parameters. Exiting..."); + rclcpp::shutdown(); + } + + if (!readParameters()) { + RCLCPP_ERROR(this->get_logger(), "[SuperOdometry::imuPreintegration] Could not read local parameters. Exiting..."); + rclcpp::shutdown(); + } + + if (!readCalibration(shared_from_this())) + { + RCLCPP_ERROR(this->get_logger(), "[SuperOdometry::imuPreintegration] Could not read calibration parameters. Exiting..."); + rclcpp::shutdown(); + } + + RCLCPP_INFO(this->get_logger(), "[SuperOdometry::imuPreintegration] use_imu_rol_pitch: %d", config_.use_imu_roll_pitch); + + //subscribe and publish relevant topics + subImu = this->create_subscription( + IMU_TOPIC, imu_qos, + std::bind(&imuPreintegration::imuHandler, this, + std::placeholders::_1), sub_options); + subLaserOdometry = this->create_subscription( + ProjectName+"/laser_odometry", 5, + std::bind(&imuPreintegration::laserodometryHandler, this, + std::placeholders::_1), sub_options); + + pubImuOdometry = this->create_publisher( + ProjectName+"/state_estimation", 10); + pubHealthStatus = this->create_publisher( + ProjectName+"/state_estimation_health", 1); + pubImuPath = this->create_publisher( + ProjectName+"/imuodom_path", 1); + + // set relevant parameter + std::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(config_.imuGravity); + + p->accelerometerCovariance = + gtsam::Matrix33::Identity(3, 3) * pow(config_.imuAccNoise, 2); // acc white noise in continuous + p->gyroscopeCovariance = + gtsam::Matrix33::Identity(3, 3) * pow(config_.imuGyrNoise, 2); // gyro white noise in continuous + p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * + pow(1e-4, 2); // error committed in integrating position from velocities + + gtsam::imuBias::ConstantBias prior_imu_bias( + (gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias + + priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m + + priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e-2); // m/s + priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-1); // 1e-2 ~ 1e-3 seems to be good + + // Noise for relative pose measurements + correctionNoise = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << config_.lidar_correction_noise, config_.lidar_correction_noise, + config_.lidar_correction_noise, config_.lidar_correction_noise, + config_.lidar_correction_noise, config_.lidar_correction_noise).finished()); + + noiseModelBetweenBias = (gtsam::Vector(6) + << config_.imuAccBiasN, + config_.imuAccBiasN, config_.imuAccBiasN, config_.imuGyrBiasN, config_.imuGyrBiasN, config_.imuGyrBiasN) + .finished(); + imuIntegratorImu_ = std::make_shared(p, prior_imu_bias); // setting up the IMU integration for IMU message + imuIntegratorOpt_ = std::make_shared(p, prior_imu_bias); // setting up the IMU integration for optimization + + //set extrinsic matrix for laser and imu + if (PROVIDE_IMU_LASER_EXTRINSIC) { + lidar2Imu = gtsam::Pose3(gtsam::Rot3(imu_laser_R), gtsam::Point3(imu_laser_T)); + } else { + imu2cam = gtsam::Pose3(gtsam::Rot3(imu_camera_R), gtsam::Point3(imu_camera_T)); + cam2Lidar = gtsam::Pose3(gtsam::Rot3(cam_laser_R), gtsam::Point3(cam_laser_T)); + imu2Lidar = imu2cam.compose(cam2Lidar); + lidar2Imu = imu2Lidar.inverse(); + } + + // Initialize world pose tracking (outside of graph) + worldPose = gtsam::Pose3(); + lastLidarPose = gtsam::Pose3(); + } + + + bool imuPreintegration::readParameters() + { + this->declare_parameter("imu_preintegration_node.acc_n", 1e-3); + this->declare_parameter("imu_preintegration_node.acc_w", 1e-3); + this->declare_parameter("imu_preintegration_node.gyr_n", 1e-6); + this->declare_parameter("imu_preintegration_node.gyr_w", 1e-6); + this->declare_parameter("imu_preintegration_node.g_norm", 9.80511); + this->declare_parameter("imu_preintegration_node.lidar_correction_noise",0.01); + this->declare_parameter("imu_preintegration_node.smooth_factor",0.9); + this->declare_parameter("imu_preintegration_node.use_imu_roll_pitch", false); + this->declare_parameter("imu_preintegration_node.imu_acc_x_limit", 1.0); + this->declare_parameter("imu_preintegration_node.imu_acc_y_limit", 1.0); + this->declare_parameter("imu_preintegration_node.imu_acc_z_limit", 1.0); + + config_.imuAccNoise = this->get_parameter("imu_preintegration_node.acc_n").as_double(); + config_.imuAccBiasN = this->get_parameter("imu_preintegration_node.acc_w").as_double(); + config_.imuGyrNoise = this->get_parameter("imu_preintegration_node.gyr_n").as_double(); + config_.imuGyrBiasN = this->get_parameter("imu_preintegration_node.gyr_w").as_double(); + config_.imuGravity = this->get_parameter("imu_preintegration_node.g_norm").as_double(); + config_.lidar_correction_noise = this->get_parameter("imu_preintegration_node.lidar_correction_noise").as_double(); + config_.smooth_factor = this->get_parameter("imu_preintegration_node.smooth_factor").as_double(); + config_.use_imu_roll_pitch = USE_IMU_ROLL_PITCH; + config_.imu_acc_x_limit = IMU_ACC_X_LIMIT; + config_.imu_acc_y_limit = IMU_ACC_Y_LIMIT; + config_.imu_acc_z_limit = IMU_ACC_Z_LIMIT; + + if (SENSOR == "livox") { + config_.sensor = SensorType::LIVOX; + } else if (SENSOR == "velodyne") { + config_.sensor = SensorType::VELODYNE; + } else if (SENSOR == "ouster") { + config_.sensor = SensorType::OUSTER; + } + + return true; + } + + void imuPreintegration::resetOptimization() { + gtsam::ISAM2Params optParameters; + optParameters.relinearizeThreshold = 0.1; + optParameters.relinearizeSkip = 1; + optimizer = gtsam::ISAM2(optParameters); + + gtsam::NonlinearFactorGraph newGraphFactors; + graphFactors = newGraphFactors; + + gtsam::Values NewGraphValues; + graphValues = NewGraphValues; + } + + void imuPreintegration::resetParams() { + lastImuT_imu = -1; + doneFirstOpt = false; + systemInitialized = false; + } + + void imuPreintegration::reset_graph() { + // Get current estimates + gtsam::Values currentEstimate = optimizer.calculateEstimate(); + + // Get last state + gtsam::Vector3 currentVel = currentEstimate.at(V(key - 1)); + gtsam::imuBias::ConstantBias currentBias = currentEstimate.at(B(key - 1)); + + // Reset graph + resetOptimization(); + + // Add anchor at identity (new local origin) + gtsam::PriorFactor priorPose(X(0), gtsam::Pose3(), priorPoseNoise); + graphFactors.add(priorPose); + + gtsam::PriorFactor priorVel(V(0), currentVel, priorVelNoise); + graphFactors.add(priorVel); + + gtsam::PriorFactor priorBias(B(0), currentBias, priorBiasNoise); + graphFactors.add(priorBias); + + // Insert values + graphValues.insert(X(0), gtsam::Pose3()); + graphValues.insert(V(0), currentVel); + graphValues.insert(B(0), currentBias); + + // Update states + prevPose_ = gtsam::Pose3(); + prevVel_ = currentVel; + prevBias_ = currentBias; + prevState_ = gtsam::NavState(prevPose_, prevVel_); + + // Optimize + optimizer.update(graphFactors, graphValues); + graphFactors.resize(0); + graphValues.clear(); + + key = 1; + } + + void imuPreintegration::initial_system(double currentCorrectionTime, gtsam::Pose3 lidarPose) { + resetOptimization(); + + // Clear old IMU data + while (!imuQueOpt.empty()) { + if (secs(&imuQueOpt.front()) < currentCorrectionTime - delta_t) { + lastImuT_opt = secs(&imuQueOpt.front()); + imuQueOpt.pop_front(); + } + else + break; + } + + // Initialize world pose + worldPose = lidarPose.compose(lidar2Imu); + lastLidarPose = lidarPose; + + // Graph starts at identity + prevPose_ = gtsam::Pose3(); + + // Add anchor prior + gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise); + graphFactors.add(priorPose); + + prevVel_ = gtsam::Vector3(0, 0, 0); + gtsam::PriorFactor priorVel(V(0), prevVel_, + priorVelNoise); + graphFactors.add(priorVel); + + prevBias_ = gtsam::imuBias::ConstantBias(); + gtsam::PriorFactor priorBias( + B(0), prevBias_, priorBiasNoise); + graphFactors.add(priorBias); + + graphValues.insert(X(0), prevPose_); + graphValues.insert(V(0), prevVel_); + graphValues.insert(B(0), prevBias_); + + optimizer.update(graphFactors, graphValues); + graphFactors.resize(0); + graphValues.clear(); + + imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); + imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); + + key = 1; + systemInitialized = true; + } + + void imuPreintegration::integrate_imumeasurement(double currentCorrectionTime) { + // 1. integrate imu data and optimize + + while (!imuQueOpt.empty()) + { + // pop and integrate imu data that is between two optimizations + sensor_msgs::msg::Imu *thisImu = &imuQueOpt.front(); + double imuTime = secs(thisImu); + if (imuTime < currentCorrectionTime - delta_t) + { + double dt = (lastImuT_opt < 0) ? (1.0 / 200.0) : (imuTime - lastImuT_opt); + lastImuT_opt = imuTime; + + if(dt < 0.001 || dt > 0.5) + dt = 0.005; + + imuIntegratorOpt_->integrateMeasurement( + gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), + gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt); + + imuQueOpt.pop_front(); + } + else + break; + } + + } + + + bool imuPreintegration::build_graph(gtsam::Pose3 lidarPose, double curLaserodomtimestamp) { + // Calculate relative transformation from lidar + gtsam::Pose3 relativeLidarPose = lastLidarPose.inverse().compose(lidarPose); + gtsam::Pose3 relativeImuPose = lidar2Imu.inverse().compose(relativeLidarPose).compose(lidar2Imu); + + // Get IMU prediction + gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); + + // Add IMU factor + const gtsam::PreintegratedImuMeasurements &preint_imu = + dynamic_cast( + *imuIntegratorOpt_); + gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), + B(key - 1), preint_imu); + graphFactors.add(imu_factor); + + // Add relative pose constraint from lidar + gtsam::BetweenFactor lidar_factor(X(key - 1), X(key), relativeImuPose, correctionNoise); + graphFactors.add(lidar_factor); + + // Add bias evolution + graphFactors.add(gtsam::BetweenFactor( + B(key - 1), B(key), gtsam::imuBias::ConstantBias(), + gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias))); + + // Insert initial values + graphValues.insert(X(key), prevPose_.compose(relativeImuPose)); + graphValues.insert(V(key), propState_.v()); + graphValues.insert(B(key), prevBias_); + + + // optimize + bool systemSolvedSuccessfully = false; + try { + optimizer.update(graphFactors, graphValues); + optimizer.update(); + systemSolvedSuccessfully = true; + } + catch (const gtsam::IndeterminantLinearSystemException &) { + systemSolvedSuccessfully = false; + RCLCPP_WARN(this->get_logger(), "Update failed due to underconstrained call to isam2 in imuPreintegration"); + } + + graphFactors.resize(0); + graphValues.clear(); + + if (systemSolvedSuccessfully) { + + gtsam::Values result = optimizer.calculateEstimate(); + prevPose_ = result.at(X(key)); + prevVel_ = result.at(V(key)); + prevState_ = gtsam::NavState(prevPose_, prevVel_); + prevBias_ = result.at(B(key)); + imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); + + // Update world pose tracking - just use the lidar pose directly + worldPose = lidarPose.compose(lidar2Imu); + lastLidarPose = lidarPose; + } + + return systemSolvedSuccessfully; + } + + void imuPreintegration::repropagate_imuodometry(double currentCorrectionTime) { + prevStateOdom = prevState_; + prevBiasOdom = prevBias_; + + double lastImuQT = -1; + while (!imuQueImu.empty() && secs(&imuQueImu.front()) < currentCorrectionTime - delta_t) { + lastImuQT = secs(&imuQueImu.front()); + imuQueImu.pop_front(); + } + + if (!imuQueImu.empty()) { + imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom); + for (int i = 0; i < (int)imuQueImu.size(); ++i) { + sensor_msgs::msg::Imu *thisImu = &imuQueImu[i]; + double imuTime = secs(thisImu); + double dt = (lastImuQT < 0) ? (1.0 / 200.0) : (imuTime - lastImuQT); + lastImuQT = imuTime; + + if(dt < 0.001 || dt > 0.5) + dt = 0.005; + + imuIntegratorImu_->integrateMeasurement( + gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), + gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), + dt); + lastImuQT = imuTime; + } + } + } + + void imuPreintegration::process_imu_odometry(double currentCorrectionTime, gtsam::Pose3 lidarPose) { + // Reset graph periodically + if (key > 100) { + reset_graph(); + } + + // 1. integrate_imumeasurement + integrate_imumeasurement(currentCorrectionTime); + + // Build and optimize graph + bool successOptimization = build_graph(lidarPose, currentCorrectionTime); + + // Check for failures + if (failureDetection(prevVel_, prevBias_) || !successOptimization) { + RCLCPP_WARN(this->get_logger(), "failureDetected"); + resetParams(); + return; + } + + // 4. reprogate_imuodometry + repropagate_imuodometry(currentCorrectionTime); + ++key; + + doneFirstOpt = true; + } + + bool imuPreintegration::failureDetection(const gtsam::Vector3 &velCur, + const gtsam::imuBias::ConstantBias &biasCur) { + Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z()); + if (vel.norm() > 30) { + RCLCPP_WARN(this->get_logger(), "Large velocity, reset IMU-preintegration!"); + return true; + } + + Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), + biasCur.accelerometer().z()); + Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), + biasCur.gyroscope().z()); + + if (ba.norm() > 2.0 || bg.norm() > 1.0) { + RCLCPP_WARN(this->get_logger(), "Large bias, reset IMU-preintegration!"); + return true; + } + + return false; + } + + void imuPreintegration::laserodometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg) { + std::lock_guard lock(mBuf); + + cur_frame = odomMsg; + double lidarOdomTime = secs(odomMsg); + + if (imuQueOpt.empty()) + return; + + float p_x = odomMsg->pose.pose.position.x; + float p_y = odomMsg->pose.pose.position.y; + float p_z = odomMsg->pose.pose.position.z; + float r_x = odomMsg->pose.pose.orientation.x; + float r_y = odomMsg->pose.pose.orientation.y; + float r_z = odomMsg->pose.pose.orientation.z; + float r_w = odomMsg->pose.pose.orientation.w; + gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), + gtsam::Point3(p_x, p_y, p_z)); + + // 0. initialize system + if (systemInitialized == false) { + initial_system(lidarOdomTime, lidarPose); + return; + } + + TicToc Optimization_time; + //1. process imu odometry + process_imu_odometry(lidarOdomTime, lidarPose); + + // 2. safe landing process + double latest_imu_time = secs(&imuQueImu.back()); + + if (lidarOdomTime - latest_imu_time < imu_laser_timedelay) { + RESULT = IMU_STATE::SUCCESS; + health_status = true; + + if((int)odomMsg->pose.covariance[0] == 1) { + RESULT = IMU_STATE::FAIL; + } + + } else { + health_status = false; + RCLCPP_INFO(this->get_logger(), "LOOSE CONNECTION WITH IMU DRIVER, PLEASE CHECK HARDWARE!!"); + RESULT = IMU_STATE::FAIL; + + std_msgs::msg::Bool health_status_msg; + health_status_msg.data = health_status; + pubHealthStatus->publish(health_status_msg); + } + + last_frame = cur_frame; + last_processed_lidar_time = lidarOdomTime; + } + //TODO: need to consider the extrinsic matrix of imu and lidar + sensor_msgs::msg::Imu imuPreintegration::imuConverter(const sensor_msgs::msg::Imu &imu_in) { + sensor_msgs::msg::Imu imu_out = imu_in; + + Eigen::Matrix3d imu_laser_R_Gravity; + imu_laser_R_Gravity = imu_Init->imu_laser_R_Gravity; + + // Rotate gyroscope + Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, + imu_in.angular_velocity.z); + gyr = imu_laser_R_Gravity * gyr; + imu_out.angular_velocity.x = gyr.x(); + imu_out.angular_velocity.y = gyr.y(); + imu_out.angular_velocity.z = gyr.z(); + + // Rotate acceleration + Eigen::Vector3d acc(imu_in.linear_acceleration.x, + imu_in.linear_acceleration.y, + imu_in.linear_acceleration.z); + + acc = imu_laser_R_Gravity * acc; + acc = acc + ((gyr - gyr_pre) * 200).cross(-imu_laser_T) + gyr.cross(gyr.cross(-imu_laser_T)); + imu_out.linear_acceleration.x = acc.x(); + imu_out.linear_acceleration.y = acc.y(); + imu_out.linear_acceleration.z = acc.z(); + + + // rotate roll pitch yaw + Eigen::Quaterniond q(imu_in.orientation.w, imu_in.orientation.x, + imu_in.orientation.y, imu_in.orientation.z); + + q.normalize(); + + + Eigen::Quaterniond q_extrinsic; + q_extrinsic=Eigen::Quaterniond(imu_laser_R_Gravity); + + Eigen::Quaterniond q_new; + + q_new=q*q_extrinsic; + + q_new.normalize(); + + imu_out.orientation.x = q_new.x(); + imu_out.orientation.y = q_new.y(); + imu_out.orientation.z = q_new.z(); + imu_out.orientation.w = q_new.w(); + + gyr_pre = gyr; + + return imu_out; + } + + + void imuPreintegration::imuHandler(const sensor_msgs::msg::Imu::SharedPtr imu_raw) { + std::lock_guard lock(mBuf); + + // 1. Pre-process IMU data + sensor_msgs::msg::Imu thisImu = imuConverter(*imu_raw); + assert(imu_raw->linear_acceleration.x != thisImu.linear_acceleration.x); + + // 2. Handle IMU initialization for LIVOX sensor + if (!handleIMUInitialization(imu_raw, thisImu)) { + return; + } + + // 3. Process timing and queue management + processTiming(thisImu); + + // 4. Early return if first optimization not done + if (!doneFirstOpt) { + return; + } + + // Predict current state in local frame + gtsam::NavState currentStateLocal = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); + + // Transform to world frame for output + gtsam::Pose3 relativePose = prevStateOdom.pose().inverse().compose(currentStateLocal.pose()); + gtsam::Pose3 currentWorldPose = worldPose.compose(relativePose); + + // Create odometry message + nav_msgs::msg::Odometry odometry; + prepareOdometryMessage(odometry, thisImu, currentStateLocal, currentWorldPose); + + if (frame_count++ % 4 == 0) { + pubImuOdometry->publish(odometry); + } + + publishTransformsAndPath(odometry, thisImu); + + // Publish health status + std_msgs::msg::Bool health_status_msg; + health_status_msg.data = health_status; + pubHealthStatus->publish(health_status_msg); + } + + bool imuPreintegration::handleIMUInitialization(const sensor_msgs::msg::Imu::SharedPtr&imu_raw, + sensor_msgs::msg::Imu& thisImu) { + if (!imu_init_success) { + initializeImu(imu_raw); + } + + if (config_.sensor == SensorType::LIVOX) { + correctLivoxGravity(thisImu); + } + + return imu_init_success; + } + + void imuPreintegration::initializeImu(const sensor_msgs::msg::Imu::SharedPtr& imu_raw) { + Imu::Ptr imudata = std::make_shared(); + imudata->time = imu_raw->header.stamp.sec + imu_raw->header.stamp.nanosec * 1e-9; + imudata->acc = Eigen::Vector3d(imu_raw->linear_acceleration.x, + imu_raw->linear_acceleration.y, + imu_raw->linear_acceleration.z); + imudata->gyr = Eigen::Vector3d(imu_raw->angular_velocity.x, + imu_raw->angular_velocity.y, + imu_raw->angular_velocity.z); + imudata->q_w_i = Eigen::Quaterniond(imu_raw->orientation.w, + imu_raw->orientation.x, + imu_raw->orientation.y, + imu_raw->orientation.z); + + imuBuf.addMeas(imudata, imudata->time); + + double first_imu_time = 0.0; + imuBuf.getFirstTime(first_imu_time); + + if (imudata->time - first_imu_time > 1.0) { + imu_Init->imuInit(imuBuf); + imu_init_success = true; + imuBuf.clean(imudata->time); + std::cout << "IMU Initialization Process Finish! " << std::endl; + } + } + + void imuPreintegration::correctLivoxGravity(sensor_msgs::msg::Imu& thisImu) { + const double gravity = 9.8105; + Eigen::Vector3d acc(thisImu.linear_acceleration.x, + thisImu.linear_acceleration.y, + thisImu.linear_acceleration.z); + acc = acc * gravity / imu_Init->acc_mean.norm(); + thisImu.linear_acceleration.x = acc.x(); + thisImu.linear_acceleration.y = acc.y(); + thisImu.linear_acceleration.z = acc.z(); + } + + void imuPreintegration::processTiming(const sensor_msgs::msg::Imu& thisImu) { + double imuTime = secs(&thisImu); + double dt = (lastImuT_imu < 0) ? (1.0 / 200.0) : (imuTime - lastImuT_imu); + lastImuT_imu = imuTime; + + if (dt < 0.001 || dt > 0.5) { + dt = 0.005; + } + + imuQueOpt.push_back(thisImu); + imuQueImu.push_back(thisImu); + } + + void imuPreintegration::publishTransformsAndPath(nav_msgs::msg::Odometry &odometry, + const sensor_msgs::msg::Imu& thisImu) { + publishTransform(odometry, thisImu); + updateAndPublishPath(odometry, thisImu); + } + + void imuPreintegration::publishTransform(nav_msgs::msg::Odometry &odometry, + const sensor_msgs::msg::Imu& thisImu) { + tf2_ros::TransformBroadcaster br(this); + geometry_msgs::msg::TransformStamped transform_stamped_; + tf2::Transform transform; + transform_stamped_.header.stamp = thisImu.header.stamp; + transform_stamped_.header.frame_id = WORLD_FRAME; + transform_stamped_.child_frame_id = SENSOR_FRAME; + + tf2::Quaternion q; + transform.setOrigin(tf2::Vector3(odometry.pose.pose.position.x, + odometry.pose.pose.position.y, + odometry.pose.pose.position.z)); + + q.setW(odometry.pose.pose.orientation.w); + q.setX(odometry.pose.pose.orientation.x); + q.setY(odometry.pose.pose.orientation.y); + q.setZ(odometry.pose.pose.orientation.z); + transform.setRotation(q); + transform_stamped_.transform = tf2::toMsg(transform); + + if(frame_count % 4 == 0) + br.sendTransform(transform_stamped_); + } + + void imuPreintegration::updateAndPublishPath(nav_msgs::msg::Odometry &odometry, + const sensor_msgs::msg::Imu& thisImu) { + static nav_msgs::msg::Path imuPath; + static double last_path_time = -1; + double curimuTime = secs(&thisImu); + + if (curimuTime - last_path_time > 0.1) { + last_path_time = curimuTime; + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = thisImu.header.stamp; + pose_stamped.header.frame_id = WORLD_FRAME; + pose_stamped.pose = odometry.pose.pose; + imuPath.poses.push_back(pose_stamped); + + while (!imuPath.poses.empty() && + abs(secs(&imuPath.poses.front()) - + secs(&imuPath.poses.back())) > 3.0) + imuPath.poses.erase(imuPath.poses.begin()); + + if (pubImuPath->get_subscription_count() != 0) { + imuPath.header.stamp = thisImu.header.stamp; + imuPath.header.frame_id = WORLD_FRAME; + pubImuPath->publish(imuPath); + } + } + } + + void imuPreintegration::prepareOdometryMessage(nav_msgs::msg::Odometry &odometry, + const sensor_msgs::msg::Imu &thisImu, + const gtsam::NavState ¤tStateLocal, + const gtsam::Pose3& currentWorldPose) { + // Transform to lidar frame + gtsam::Pose3 lidarPoseWorld = currentWorldPose.compose(imu2Lidar); + + Eigen::Quaterniond q_w_curr; + if (config_.use_imu_roll_pitch) { + q_w_curr = Eigen::Quaterniond(thisImu.orientation.w, thisImu.orientation.x, + thisImu.orientation.y, thisImu.orientation.z); + } else { + q_w_curr = Eigen::Quaterniond(lidarPoseWorld.rotation().toQuaternion().w(), + lidarPoseWorld.rotation().toQuaternion().x(), + lidarPoseWorld.rotation().toQuaternion().y(), + lidarPoseWorld.rotation().toQuaternion().z()); + } + + // Transform velocity to world then body frame + Eigen::Vector3d velocity_world = currentWorldPose.rotation().matrix() * currentStateLocal.velocity(); + Eigen::Vector3d velocity_body = lidarPoseWorld.rotation().inverse().matrix() * velocity_world; + + odometry.header.stamp = thisImu.header.stamp; + odometry.header.frame_id = WORLD_FRAME; + odometry.child_frame_id = SENSOR_FRAME; + + q_w_curr.normalized(); + + odometry.pose.pose.position.x = lidarPoseWorld.translation().x(); + odometry.pose.pose.position.y = lidarPoseWorld.translation().y(); + odometry.pose.pose.position.z = lidarPoseWorld.translation().z(); + odometry.pose.pose.orientation.x = q_w_curr.x(); + odometry.pose.pose.orientation.y = q_w_curr.y(); + odometry.pose.pose.orientation.z = q_w_curr.z(); + odometry.pose.pose.orientation.w = q_w_curr.w(); + + odometry.twist.twist.linear.x = velocity_body.x(); + odometry.twist.twist.linear.y = velocity_body.y(); + odometry.twist.twist.linear.z = velocity_body.z(); + odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x(); + odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y(); + odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z(); + + odometry.pose.covariance[0] = double(RESULT); + odometry.pose.covariance[1] = prevBiasOdom.accelerometer().x(); + odometry.pose.covariance[2] = prevBiasOdom.accelerometer().y(); + odometry.pose.covariance[3] = prevBiasOdom.accelerometer().z(); + odometry.pose.covariance[4] = prevBiasOdom.gyroscope().x(); + odometry.pose.covariance[5] = prevBiasOdom.gyroscope().y(); + odometry.pose.covariance[6] = prevBiasOdom.gyroscope().z(); + odometry.pose.covariance[7] = config_.imuGravity; + } + + +} // end namespace super_odometry \ No newline at end of file From 3ff50cf67910f0881c693b2894eb546a8cab91d5 Mon Sep 17 00:00:00 2001 From: avnishnp Date: Wed, 30 Jul 2025 15:56:32 -0400 Subject: [PATCH 2/4] added saving csv files directly for GT comparison --- script/save_benchmark_result.py | 33 +++++++++++++++++++ super_odometry/src/imuPreintegration_node.cpp | 2 +- 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/script/save_benchmark_result.py b/script/save_benchmark_result.py index 815e48f..6ee614f 100644 --- a/script/save_benchmark_result.py +++ b/script/save_benchmark_result.py @@ -15,6 +15,7 @@ # Rerun imports import rerun as rr import numpy as np +import csv class PathSubscriber(Node): def __init__(self): @@ -62,6 +63,37 @@ def laser_path_callback(self, msg): self.laser_last_message_time = time.time() self.get_logger().info(f'Received laser path message with {len(msg.poses)} poses') + def append_pose_to_csv(self, pose_stamped, source="imu"): + output_file = f"{source}_poses.csv" + file_exists = os.path.isfile(output_file) + + with open(output_file, mode='a', newline='') as f: + writer = csv.writer(f) + + # Write header if file is new + if not file_exists: + writer.writerow([ + "timestamp", + "p_w_b_x", "p_w_b_y", "p_w_b_z", + "q_w_b_x", "q_w_b_y", "q_w_b_z", "q_w_b_w" + ]) + + # Get timestamp from pose_stamped.header.stamp + stamp = pose_stamped.header.stamp + timestamp = stamp.sec + stamp.nanosec * 1e-9 + + # Write pose data + writer.writerow([ + timestamp, + pose_stamped.pose.position.x, + pose_stamped.pose.position.y, + pose_stamped.pose.position.z, + pose_stamped.pose.orientation.x, + pose_stamped.pose.orientation.y, + pose_stamped.pose.orientation.z, + pose_stamped.pose.orientation.w + ]) + def imu_path_callback(self, msg): """Callback for receiving IMU Path messages""" self.imu_path_messages.append(msg) @@ -70,6 +102,7 @@ def imu_path_callback(self, msg): # Add all poses from this message to the global list for pose in msg.poses: self.all_imu_poses.append(pose) + self.append_pose_to_csv(pose, source="imu") self.get_logger().info(f'Received IMU path message with {len(msg.poses)} poses, total: {len(self.all_imu_poses)}') diff --git a/super_odometry/src/imuPreintegration_node.cpp b/super_odometry/src/imuPreintegration_node.cpp index 72e1ea0..61e6434 100755 --- a/super_odometry/src/imuPreintegration_node.cpp +++ b/super_odometry/src/imuPreintegration_node.cpp @@ -2,7 +2,7 @@ // Created by shibo zhao on 2020-09-27. // #include "rclcpp/rclcpp.hpp" -#include "super_odometry/ImuPreintegration/imuPreintegration.h" +#include "super_odometry/ImuPreintegration/imuPreintegration_relative.h" int main(int argc, char **argv) { From fc4a272923a438e62d20ae190227eab9e42f92ff Mon Sep 17 00:00:00 2001 From: avnishnp Date: Tue, 5 Aug 2025 16:21:19 -0400 Subject: [PATCH 3/4] logic and parameter fix --- .../include/super_odometry/LidarProcess/LidarSlam.h | 2 +- super_odometry/src/LidarProcess/LidarSlam.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/super_odometry/include/super_odometry/LidarProcess/LidarSlam.h b/super_odometry/include/super_odometry/LidarProcess/LidarSlam.h index e735f94..4791dc6 100755 --- a/super_odometry/include/super_odometry/LidarProcess/LidarSlam.h +++ b/super_odometry/include/super_odometry/LidarProcess/LidarSlam.h @@ -274,7 +274,7 @@ namespace super_odometry { size_t LocalizationLineDistanceNbrNeighbors = 10; size_t LocalizationMinmumLineNeighborRejection = 4; - size_t LocalizationPlaneDistanceNbrNeighbors = 5; + size_t LocalizationPlaneDistanceNbrNeighbors = 9; double LocalizationLineDistancefactor = 5.0; diff --git a/super_odometry/src/LidarProcess/LidarSlam.cpp b/super_odometry/src/LidarProcess/LidarSlam.cpp index f026720..922002b 100755 --- a/super_odometry/src/LidarProcess/LidarSlam.cpp +++ b/super_odometry/src/LidarProcess/LidarSlam.cpp @@ -190,7 +190,7 @@ namespace super_odometry { RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "very small motion, not accumulating. %f", stats.translation_from_last); } - acceptResult = true; + // acceptResult = true; return acceptResult; } @@ -229,7 +229,7 @@ namespace super_odometry { ceres::Solver::Summary LidarSLAM::solveOptimizationProblem(ceres::Problem&problem){ ceres::Solver::Options options; - options.max_num_iterations=4; + options.max_num_iterations=8; options.linear_solver_type=ceres::DENSE_QR; options.minimizer_progress_to_stdout=false; options.check_gradients=false; From 118aa663b6771d8d77018c4998114759e47489b7 Mon Sep 17 00:00:00 2001 From: avnishnp Date: Thu, 7 Aug 2025 12:01:35 -0400 Subject: [PATCH 4/4] added fixed lag, batch lag, smart reset for traditional iSAM2 with analysis --- super_odometry/CMakeLists.txt | 96 +- .../imuPreintegration_relative.h | 173 +++- .../imuPreintegration_relative.cpp | 959 +++++++++++++++--- super_odometry/src/analysis.py | 334 ++++++ 4 files changed, 1380 insertions(+), 182 deletions(-) create mode 100644 super_odometry/src/analysis.py diff --git a/super_odometry/CMakeLists.txt b/super_odometry/CMakeLists.txt index b70364a..cea2049 100755 --- a/super_odometry/CMakeLists.txt +++ b/super_odometry/CMakeLists.txt @@ -48,10 +48,28 @@ find_package(Sophus REQUIRED) find_package(PCL REQUIRED) find_package(pcl_conversions REQUIRED) find_package(Ceres REQUIRED) -find_package(GTSAM REQUIRED QUIET) find_package(TBB REQUIRED) find_package(livox_ros_driver2 REQUIRED) +# GTSAM and gtsam_unstable configuration +find_package(GTSAM REQUIRED) + +# Check if gtsam_unstable is available +find_library(GTSAM_UNSTABLE_LIBRARY + NAMES gtsam_unstable + HINTS ${GTSAM_LIBRARY_DIRS} /usr/local/lib /usr/lib +) + +if(GTSAM_UNSTABLE_LIBRARY) + message(STATUS "Found gtsam_unstable: ${GTSAM_UNSTABLE_LIBRARY}") + set(GTSAM_LIBRARIES gtsam gtsam_unstable) + add_definitions(-DGTSAM_UNSTABLE_AVAILABLE) +else() + message(WARNING "gtsam_unstable not found. Fixed-Lag Smoother will not be available.") + message(WARNING "To enable Fixed-Lag Smoother, rebuild GTSAM with -DGTSAM_BUILD_UNSTABLE=ON") + set(GTSAM_LIBRARIES gtsam) +endif() + # Include directories include_directories( include @@ -67,15 +85,11 @@ include_directories( # Link directories link_directories( - include ${PCL_LIBRARY_DIRS} ${OpenCV_LIBRARY_DIRS} ${GTSAM_LIBRARY_DIRS} ) -set(GTSAM_LIBRARIES gtsam) - - # SuperOdom Library add_library(SuperOdomLib SHARED src/FeatureExtraction/featureExtraction.cpp @@ -92,10 +106,15 @@ add_library(SuperOdomLib SHARED # Set up library include directory target_include_directories(SuperOdomLib PUBLIC - $ - $ # /include/mylib + $ + $ ) +# Add compile definitions for configuration +if(GTSAM_UNSTABLE_LIBRARY) + target_compile_definitions(SuperOdomLib PUBLIC GTSAM_UNSTABLE_AVAILABLE) +endif() + ament_target_dependencies(SuperOdomLib rclcpp std_msgs @@ -115,38 +134,58 @@ ament_target_dependencies(SuperOdomLib target_link_libraries(SuperOdomLib ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} - gtsam - ${CERES_LIBRARIES} + ${GTSAM_LIBRARIES} + ${CERES_LIBRARIES} + ${TBB_LIBRARIES} + pthread ) # IMU PreIntegration node add_executable(imu_preintegration_node src/imuPreintegration_node.cpp) -ament_target_dependencies(imu_preintegration_node rclcpp nav_msgs sensor_msgs tf2_ros) +ament_target_dependencies(imu_preintegration_node + rclcpp + nav_msgs + sensor_msgs + tf2_ros + std_msgs +) target_link_libraries(imu_preintegration_node - gtsam - SuperOdomLib + ${GTSAM_LIBRARIES} + SuperOdomLib + pthread ) - - # Feature Extraction node add_executable(feature_extraction_node src/featureExtraction_node.cpp) -ament_target_dependencies(feature_extraction_node rclcpp nav_msgs sensor_msgs super_odometry_msgs livox_ros_driver2) +ament_target_dependencies(feature_extraction_node + rclcpp + nav_msgs + sensor_msgs + super_odometry_msgs + livox_ros_driver2 +) target_link_libraries(feature_extraction_node ${PCL_LIBRARIES} ${TBB_LIBRARIES} - $ - $ SuperOdomLib ) # LiDAR Mapping node add_executable(laser_mapping_node src/laserMapping_node.cpp) -ament_target_dependencies(laser_mapping_node rclcpp std_msgs nav_msgs sensor_msgs geometry_msgs tf2 tf2_ros) +ament_target_dependencies(laser_mapping_node + rclcpp + std_msgs + nav_msgs + sensor_msgs + geometry_msgs + tf2 + tf2_ros + visualization_msgs +) target_link_libraries(laser_mapping_node - ${PCL_LIBRARIES} - ${CERES_LIBRARIES} - SuperOdomLib + ${PCL_LIBRARIES} + ${CERES_LIBRARIES} + SuperOdomLib ) # Install Exported Libraries @@ -168,8 +207,21 @@ install( ament_export_include_directories(include) ament_export_libraries(SuperOdomLib) +ament_export_dependencies( + rclcpp + std_msgs + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + super_odometry_msgs + visualization_msgs + pcl_conversions +) -#Install executables +# Install executables install(TARGETS imu_preintegration_node feature_extraction_node diff --git a/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h b/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h index 79bf6cc..7c08cd2 100644 --- a/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h +++ b/super_odometry/include/super_odometry/ImuPreintegration/imuPreintegration_relative.h @@ -1,5 +1,6 @@ // -// Header for Pure Relative Pose Graph Implementation +// Unified Header for IMU Preintegration with Optional GTSAM Fixed-Lag Smoother +// Supports both traditional ISAM2 and Fixed-Lag Smoother backends // #pragma once #ifndef IMUPREINTEGRATION_H @@ -28,7 +29,12 @@ #include #include #include + +// Fixed-Lag Smoother includes from gtsam_unstable +#include #include +#include + #include "super_odometry/utils/Twist.h" #include "super_odometry/container/MapRingBuffer.h" #include "super_odometry/config/parameter.h" @@ -36,6 +42,13 @@ #include #include "super_odometry/sensor_data/imu/imu_data.h" +// Performance monitoring includes +#include +#include +#include +#include +#include + namespace super_odometry { using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) @@ -57,53 +70,153 @@ namespace super_odometry { double imu_acc_x_limit; double imu_acc_y_limit; double imu_acc_z_limit; + + // Fixed-Lag Smoother parameters + bool use_fixed_lag; // Enable fixed-lag smoother (primary mode selection) + double fixed_lag_size; // Lag window size in seconds + bool use_batch_fixed_lag; // true = BatchFixedLagSmoother, false = IncrementalFixedLagSmoother + + // Traditional ISAM2 marginalization parameters (when fixed-lag is disabled) + bool enable_marginalization; // Enable smart reset for traditional ISAM2 + double marginalization_window_size; // Time window in seconds for smart reset + int marginalization_skip; // Skip factor for marginalization + }; + + // Structure to store pose with timestamp for trajectory output + struct TimestampedPose { + double timestamp; + gtsam::Pose3 worldPose; + gtsam::Pose3 localPose; + int graphKey; + + TimestampedPose() : timestamp(0.0), graphKey(-1) {} + TimestampedPose(double t, const gtsam::Pose3& w, const gtsam::Pose3& l, int k) + : timestamp(t), worldPose(w), localPose(l), graphKey(k) {} }; class imuPreintegration : public rclcpp::Node { public: imuPreintegration(const rclcpp::NodeOptions & options); + virtual ~imuPreintegration(); static constexpr double delta_t = 0; static constexpr double imu_laser_timedelay= 0.8; + + // Unified performance monitoring structure + struct PerformanceMetrics { + // Optimization timing + double last_optimization_time_ms = 0.0; + double max_optimization_time_ms = 0.0; + double avg_optimization_time_ms = 0.0; + int optimization_count = 0; + + // Graph update timing + double last_graph_update_time_ms = 0.0; + double max_graph_update_time_ms = 0.0; + double avg_graph_update_time_ms = 0.0; + + // Graph statistics + size_t current_graph_size = 0; + size_t max_graph_size = 0; + + // Processing statistics + double total_processing_time_ms = 0.0; + int total_frames_processed = 0; + + // Memory usage + size_t current_memory_usage_mb = 0; + size_t peak_memory_usage_mb = 0; + + // Fixed-Lag specific metrics + int num_marginalizations = 0; // Number of marginalization events (Fixed-Lag) + size_t total_marginalized_keys = 0; // Total keys marginalized (Fixed-Lag) + double avg_keys_per_marginalization = 0.0; + + // Traditional ISAM2 specific metrics + int num_resets_performed = 0; // Number of smart resets performed (Traditional) + double time_since_last_reset = 0.0; // Time since last reset (Traditional) + + void reset() { + *this = PerformanceMetrics(); + } + }; public: + // Core interface methods void initInterface(); bool readParameters(); void laserodometryHandler(const nav_msgs::msg::Odometry::SharedPtr odomMsg); void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imu_raw); void initial_system(double currentCorrectionTime, gtsam::Pose3 lidarPose); + + // Initialization methods for different backends + void initializeFixedLagSmoother(); + void initializeTraditionalOptimizer(); + + // Graph building methods - unified interface void process_imu_odometry(double currentCorrectionTime, gtsam::Pose3 lidarPose); bool build_graph(gtsam::Pose3 lidarPose, double curLaserodomtimestamp); + + // Backend-specific graph building methods + bool build_graph_with_fixed_lag(gtsam::Pose3 lidarPose, double curLaserodomtimestamp); + bool build_graph_traditional(gtsam::Pose3 lidarPose, double curLaserodomtimestamp); + + // Traditional ISAM2 methods (smart reset functionality) + void reset_graph(); + void resetOptimization(); + void performSmartReset(int keepFromKey); + void handleTraditionalMarginalization(double currentCorrectionTime); + bool shouldPerformReset() const; + void updateKeyTimestamps(double timestamp); + + // IMU processing methods void repropagate_imuodometry(double currentCorrectionTime); bool failureDetection(const gtsam::Vector3 &velCur, const gtsam::imuBias::ConstantBias &biasCur); void integrate_imumeasurement(double currentCorrectionTime); - void reset_graph(); - void resetOptimization(); void resetParams(); + + // IMU initialization and preprocessing bool handleIMUInitialization(const sensor_msgs::msg::Imu::SharedPtr&imu_raw, sensor_msgs::msg::Imu& thisImu); + void initializeImu(const sensor_msgs::msg::Imu::SharedPtr& imu_raw); + void correctLivoxGravity(sensor_msgs::msg::Imu& thisImu); + sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu &imu_in); + void processTiming(const sensor_msgs::msg::Imu& thisImu); + + // Publishing methods void updateAndPublishPath(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); void publishTransform(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); - - // Modified for relative pose graph - includes world pose parameter void prepareOdometryMessage(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu, const gtsam::NavState ¤tStateLocal, const gtsam::Pose3& currentWorldPose); - void publishTransformsAndPath(nav_msgs::msg::Odometry &odometry, const sensor_msgs::msg::Imu& thisImu); - void processTiming(const sensor_msgs::msg::Imu& thisImu); - void initializeImu(const sensor_msgs::msg::Imu::SharedPtr& imu_raw); - void correctLivoxGravity(sensor_msgs::msg::Imu& thisImu); - sensor_msgs::msg::Imu imuConverter(const sensor_msgs::msg::Imu &imu_in); + + // Pose history management + void addToPoseHistory(int key, double timestamp, const gtsam::Pose3& worldPose, const gtsam::Pose3& localPose); + void cleanOldPoseHistory(double currentTime, double keepDuration = 60.0); + TimestampedPose interpolatePose(double queryTime) const; + + // Performance monitoring methods + void updatePerformanceMetrics(double opt_time_ms); + void logPerformanceTiming(std::chrono::high_resolution_clock::time_point start_time, + double opt_time_ms, double timestamp); + void logCurrentPerformance(double opt_time_ms); + void reportDetailedPerformance(); + void reportPerformanceMetrics(); + void comparePerformanceWithBaseline(); + size_t getCurrentMemoryUsageMB(); + void logPerformanceToCSV(double timestamp, double opt_time_ms, double total_time_ms); + // Utility template template double secs(T msg) { return msg->header.stamp.sec + msg->header.stamp.nanosec*1e-9; } private: + // ROS2 interface rclcpp::Subscription::SharedPtr subImu; rclcpp::Subscription::SharedPtr subLaserOdometry; @@ -114,15 +227,18 @@ namespace super_odometry { rclcpp::CallbackGroup::SharedPtr cb_group_; public: + // GTSAM noise models gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; gtsam::Vector noiseModelBetweenBias; + + // IMU preintegration std::shared_ptr imuIntegratorOpt_; std::shared_ptr imuIntegratorImu_; - // Graph nodes store relative poses + // Current state estimates gtsam::Pose3 prevPose_; // Previous pose in local frame gtsam::Vector3 prevVel_; gtsam::NavState prevState_; @@ -130,17 +246,38 @@ namespace super_odometry { gtsam::NavState prevStateOdom; gtsam::imuBias::ConstantBias prevBiasOdom; + // Fixed-Lag Smoother backends (when use_fixed_lag = true) + std::unique_ptr batchSmoother_; + std::unique_ptr incrementalSmoother_; + + // Traditional ISAM2 backend (when use_fixed_lag = false) gtsam::ISAM2 optimizer; gtsam::NonlinearFactorGraph graphFactors; gtsam::Values graphValues; - // Lidar odometry poses - gtsam::Pose3 lidarodom_w_pre; - gtsam::Pose3 lidarodom_w_cur; + // Traditional ISAM2 tracking for smart reset + std::map keyTimestamps; + double oldestTimestamp = 0.0; + double newestTimestamp = 0.0; + int resetCounter = 0; - // World pose tracking (outside of graph) + // World pose tracking (outside of graph for relative pose formulation) gtsam::Pose3 worldPose; // Current pose in world frame gtsam::Pose3 lastLidarPose; // Last lidar pose for relative computation + + // Pose history for continuous trajectory output and debugging + std::map poseHistory; + + // Key tracking across resets/marginalizations + int key = 1; + int totalKeysProcessed = 0; // Total keys processed across all resets + gtsam::Pose3 accumulatedWorldTransform; // Accumulated transform from resets + + // Performance monitoring members + PerformanceMetrics perf_metrics_; + bool perf_logging_enabled_ = true; + double last_reset_timestamp_ = 0.0; + std::chrono::high_resolution_clock::time_point start_time_; public: // Extrinsic calibration @@ -150,6 +287,7 @@ namespace super_odometry { gtsam::Pose3 lidar2Imu; public: + // Data buffers MapRingBuffer imuBuf; std::deque imuQueOpt; std::deque imuQueImu; @@ -158,22 +296,25 @@ namespace super_odometry { Imu::Ptr imu_Init = std::make_shared(); public: + // System state flags bool systemInitialized = false; bool doneFirstOpt = false; bool health_status = true; bool imu_init_success = false; + // IMU data Eigen::Quaterniond firstImu; Eigen::Vector3d gyr_pre; + // Timing double first_imu_time_stamp; double last_processed_lidar_time = -1; double lastImuT_imu = -1; double lastImuT_opt = -1; - int key = 1; int imuPreintegrationResetId = 0; int frame_count = 0; + // System health status enum IMU_STATE : uint8_t { FAIL=0, SUCCESS=1, diff --git a/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp b/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp index 77e1ed6..9d94011 100644 --- a/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp +++ b/super_odometry/src/ImuPreintegration/imuPreintegration_relative.cpp @@ -1,6 +1,6 @@ // -// Relative Pose Graph Implementation -// All poses are relative in the graph +// IMU Preintegration with Optional GTSAM Fixed-Lag Smoother +// Can switch between traditional ISAM2 and Fixed-Lag Smoother // #include "super_odometry/ImuPreintegration/imuPreintegration_relative.h" @@ -10,6 +10,15 @@ namespace super_odometry { : Node("imu_preintegration_node", options) { } + imuPreintegration::~imuPreintegration() { + if (perf_logging_enabled_) { + RCLCPP_INFO(this->get_logger(), + "\n===== FINAL PERFORMANCE REPORT ====="); + reportPerformanceMetrics(); + comparePerformanceWithBaseline(); + } + } + void imuPreintegration::initInterface() { //! Callback Groups cb_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); @@ -17,8 +26,8 @@ namespace super_odometry { sub_options.callback_group = cb_group_; rclcpp::QoS imu_qos(10); - imu_qos.best_effort(); // Use BEST_EFFORT reliability - imu_qos.keep_last(10); // Keep last 10 messages + imu_qos.best_effort(); + imu_qos.keep_last(10); if (!readGlobalparam(shared_from_this())) { RCLCPP_ERROR(this->get_logger(), "[SuperOdometry::imuPreintegration] Could not read global parameters. Exiting..."); @@ -30,13 +39,12 @@ namespace super_odometry { rclcpp::shutdown(); } - if (!readCalibration(shared_from_this())) - { + if (!readCalibration(shared_from_this())) { RCLCPP_ERROR(this->get_logger(), "[SuperOdometry::imuPreintegration] Could not read calibration parameters. Exiting..."); rclcpp::shutdown(); } - RCLCPP_INFO(this->get_logger(), "[SuperOdometry::imuPreintegration] use_imu_rol_pitch: %d", config_.use_imu_roll_pitch); + RCLCPP_INFO(this->get_logger(), "[SuperOdometry::imuPreintegration] use_imu_rol_pitch: %d", config_.use_imu_roll_pitch); //subscribe and publish relevant topics subImu = this->create_subscription( @@ -59,35 +67,30 @@ namespace super_odometry { std::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(config_.imuGravity); p->accelerometerCovariance = - gtsam::Matrix33::Identity(3, 3) * pow(config_.imuAccNoise, 2); // acc white noise in continuous + gtsam::Matrix33::Identity(3, 3) * pow(config_.imuAccNoise, 2); p->gyroscopeCovariance = - gtsam::Matrix33::Identity(3, 3) * pow(config_.imuGyrNoise, 2); // gyro white noise in continuous - p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * - pow(1e-4, 2); // error committed in integrating position from velocities + gtsam::Matrix33::Identity(3, 3) * pow(config_.imuGyrNoise, 2); + p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); - gtsam::imuBias::ConstantBias prior_imu_bias( - (gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias + gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas( - (gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m - - priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e-2); // m/s - priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-1); // 1e-2 ~ 1e-3 seems to be good + (gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); + priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e-2); + priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-1); - // Noise for relative pose measurements correctionNoise = gtsam::noiseModel::Diagonal::Sigmas( (gtsam::Vector(6) << config_.lidar_correction_noise, config_.lidar_correction_noise, config_.lidar_correction_noise, config_.lidar_correction_noise, config_.lidar_correction_noise, config_.lidar_correction_noise).finished()); noiseModelBetweenBias = (gtsam::Vector(6) - << config_.imuAccBiasN, - config_.imuAccBiasN, config_.imuAccBiasN, config_.imuGyrBiasN, config_.imuGyrBiasN, config_.imuGyrBiasN) - .finished(); - imuIntegratorImu_ = std::make_shared(p, prior_imu_bias); // setting up the IMU integration for IMU message - imuIntegratorOpt_ = std::make_shared(p, prior_imu_bias); // setting up the IMU integration for optimization + << config_.imuAccBiasN, config_.imuAccBiasN, config_.imuAccBiasN, + config_.imuGyrBiasN, config_.imuGyrBiasN, config_.imuGyrBiasN).finished(); + + imuIntegratorImu_ = std::make_shared(p, prior_imu_bias); + imuIntegratorOpt_ = std::make_shared(p, prior_imu_bias); - //set extrinsic matrix for laser and imu if (PROVIDE_IMU_LASER_EXTRINSIC) { lidar2Imu = gtsam::Pose3(gtsam::Rot3(imu_laser_R), gtsam::Point3(imu_laser_T)); } else { @@ -97,25 +100,96 @@ namespace super_odometry { lidar2Imu = imu2Lidar.inverse(); } - // Initialize world pose tracking (outside of graph) worldPose = gtsam::Pose3(); lastLidarPose = gtsam::Pose3(); + + totalKeysProcessed = 0; + + perf_metrics_.reset(); + perf_logging_enabled_ = true; + start_time_ = std::chrono::high_resolution_clock::now(); + + // Initialize the appropriate optimizer based on configuration + if (config_.use_fixed_lag) { + initializeFixedLagSmoother(); + RCLCPP_INFO(this->get_logger(), + "Fixed-Lag Smoother enabled with lag: %.1f seconds", + config_.fixed_lag_size); + } else { + initializeTraditionalOptimizer(); + RCLCPP_INFO(this->get_logger(), + "Traditional ISAM2 optimizer enabled with marginalization: %s", + config_.enable_marginalization ? "ON" : "OFF"); + } } - - bool imuPreintegration::readParameters() - { + void imuPreintegration::initializeFixedLagSmoother() { + if (config_.use_batch_fixed_lag) { + batchSmoother_ = std::make_unique( + config_.fixed_lag_size); + } else { + gtsam::ISAM2Params isam2_params; + isam2_params.relinearizeThreshold = 0.1; + isam2_params.relinearizeSkip = 1; + isam2_params.findUnusedFactorSlots = true; + + incrementalSmoother_ = std::make_unique( + config_.fixed_lag_size, isam2_params); + } + + RCLCPP_INFO(this->get_logger(), + "Using %s Fixed-Lag Smoother", + config_.use_batch_fixed_lag ? "Batch" : "Incremental"); + } + + void imuPreintegration::initializeTraditionalOptimizer() { + gtsam::ISAM2Params optParameters; + optParameters.relinearizeThreshold = 0.1; + optParameters.relinearizeSkip = 1; + + // Enable these for better marginalization performance if enabled + if (config_.enable_marginalization) { + optParameters.enablePartialRelinearizationCheck = true; + optParameters.findUnusedFactorSlots = true; + optParameters.cacheLinearizedFactors = false; + } + + optimizer = gtsam::ISAM2(optParameters); + + if (config_.enable_marginalization) { + keyTimestamps.clear(); + oldestTimestamp = 0.0; + newestTimestamp = 0.0; + } + + gtsam::NonlinearFactorGraph newGraphFactors; + graphFactors = newGraphFactors; + + gtsam::Values NewGraphValues; + graphValues = NewGraphValues; + } + + bool imuPreintegration::readParameters() { this->declare_parameter("imu_preintegration_node.acc_n", 1e-3); this->declare_parameter("imu_preintegration_node.acc_w", 1e-3); this->declare_parameter("imu_preintegration_node.gyr_n", 1e-6); this->declare_parameter("imu_preintegration_node.gyr_w", 1e-6); this->declare_parameter("imu_preintegration_node.g_norm", 9.80511); - this->declare_parameter("imu_preintegration_node.lidar_correction_noise",0.01); - this->declare_parameter("imu_preintegration_node.smooth_factor",0.9); + this->declare_parameter("imu_preintegration_node.lidar_correction_noise", 0.01); + this->declare_parameter("imu_preintegration_node.smooth_factor", 0.9); this->declare_parameter("imu_preintegration_node.use_imu_roll_pitch", false); this->declare_parameter("imu_preintegration_node.imu_acc_x_limit", 1.0); this->declare_parameter("imu_preintegration_node.imu_acc_y_limit", 1.0); this->declare_parameter("imu_preintegration_node.imu_acc_z_limit", 1.0); + + // Fixed-lag smoother parameters + this->declare_parameter("imu_preintegration_node.use_fixed_lag", true); + this->declare_parameter("imu_preintegration_node.fixed_lag_size", 10.0); + this->declare_parameter("imu_preintegration_node.use_batch_fixed_lag", false); + + // Traditional marginalization parameters (for when fixed-lag is disabled) + this->declare_parameter("imu_preintegration_node.enable_marginalization", false); + this->declare_parameter("imu_preintegration_node.marginalization_window_size", 10.0); config_.imuAccNoise = this->get_parameter("imu_preintegration_node.acc_n").as_double(); config_.imuAccBiasN = this->get_parameter("imu_preintegration_node.acc_w").as_double(); @@ -128,6 +202,13 @@ namespace super_odometry { config_.imu_acc_x_limit = IMU_ACC_X_LIMIT; config_.imu_acc_y_limit = IMU_ACC_Y_LIMIT; config_.imu_acc_z_limit = IMU_ACC_Z_LIMIT; + + config_.use_fixed_lag = this->get_parameter("imu_preintegration_node.use_fixed_lag").as_bool(); + config_.fixed_lag_size = this->get_parameter("imu_preintegration_node.fixed_lag_size").as_double(); + config_.use_batch_fixed_lag = this->get_parameter("imu_preintegration_node.use_batch_fixed_lag").as_bool(); + + config_.enable_marginalization = this->get_parameter("imu_preintegration_node.enable_marginalization").as_bool(); + config_.marginalization_window_size = this->get_parameter("imu_preintegration_node.marginalization_window_size").as_double(); if (SENSOR == "livox") { config_.sensor = SensorType::LIVOX; @@ -135,22 +216,15 @@ namespace super_odometry { config_.sensor = SensorType::VELODYNE; } else if (SENSOR == "ouster") { config_.sensor = SensorType::OUSTER; - } - - return true; - } - - void imuPreintegration::resetOptimization() { - gtsam::ISAM2Params optParameters; - optParameters.relinearizeThreshold = 0.1; - optParameters.relinearizeSkip = 1; - optimizer = gtsam::ISAM2(optParameters); + } - gtsam::NonlinearFactorGraph newGraphFactors; - graphFactors = newGraphFactors; + // Validate configuration + if (config_.use_fixed_lag && config_.enable_marginalization) { + RCLCPP_WARN(this->get_logger(), + "Both fixed-lag and traditional marginalization enabled. Fixed-lag takes precedence."); + } - gtsam::Values NewGraphValues; - graphValues = NewGraphValues; + return true; } void imuPreintegration::resetParams() { @@ -159,50 +233,7 @@ namespace super_odometry { systemInitialized = false; } - void imuPreintegration::reset_graph() { - // Get current estimates - gtsam::Values currentEstimate = optimizer.calculateEstimate(); - - // Get last state - gtsam::Vector3 currentVel = currentEstimate.at(V(key - 1)); - gtsam::imuBias::ConstantBias currentBias = currentEstimate.at(B(key - 1)); - - // Reset graph - resetOptimization(); - - // Add anchor at identity (new local origin) - gtsam::PriorFactor priorPose(X(0), gtsam::Pose3(), priorPoseNoise); - graphFactors.add(priorPose); - - gtsam::PriorFactor priorVel(V(0), currentVel, priorVelNoise); - graphFactors.add(priorVel); - - gtsam::PriorFactor priorBias(B(0), currentBias, priorBiasNoise); - graphFactors.add(priorBias); - - // Insert values - graphValues.insert(X(0), gtsam::Pose3()); - graphValues.insert(V(0), currentVel); - graphValues.insert(B(0), currentBias); - - // Update states - prevPose_ = gtsam::Pose3(); - prevVel_ = currentVel; - prevBias_ = currentBias; - prevState_ = gtsam::NavState(prevPose_, prevVel_); - - // Optimize - optimizer.update(graphFactors, graphValues); - graphFactors.resize(0); - graphValues.clear(); - - key = 1; - } - void imuPreintegration::initial_system(double currentCorrectionTime, gtsam::Pose3 lidarPose) { - resetOptimization(); - - // Clear old IMU data while (!imuQueOpt.empty()) { if (secs(&imuQueOpt.front()) < currentCorrectionTime - delta_t) { lastImuT_opt = secs(&imuQueOpt.front()); @@ -211,53 +242,81 @@ namespace super_odometry { else break; } - - // Initialize world pose + worldPose = lidarPose.compose(lidar2Imu); lastLidarPose = lidarPose; // Graph starts at identity prevPose_ = gtsam::Pose3(); - - // Add anchor prior - gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise); - graphFactors.add(priorPose); - prevVel_ = gtsam::Vector3(0, 0, 0); - gtsam::PriorFactor priorVel(V(0), prevVel_, - priorVelNoise); - graphFactors.add(priorVel); - prevBias_ = gtsam::imuBias::ConstantBias(); - gtsam::PriorFactor priorBias( - B(0), prevBias_, priorBiasNoise); - graphFactors.add(priorBias); - - graphValues.insert(X(0), prevPose_); - graphValues.insert(V(0), prevVel_); - graphValues.insert(B(0), prevBias_); - - optimizer.update(graphFactors, graphValues); - graphFactors.resize(0); - graphValues.clear(); - + prevState_ = gtsam::NavState(prevPose_, prevVel_); + + if (config_.use_fixed_lag) { + // Initialize fixed-lag smoother + gtsam::NonlinearFactorGraph newFactors; + gtsam::Values newValues; + gtsam::FixedLagSmoother::KeyTimestampMap newTimestamps; + + newFactors.add(gtsam::PriorFactor(X(0), prevPose_, priorPoseNoise)); + newFactors.add(gtsam::PriorFactor(V(0), prevVel_, priorVelNoise)); + newFactors.add(gtsam::PriorFactor(B(0), prevBias_, priorBiasNoise)); + + newValues.insert(X(0), prevPose_); + newValues.insert(V(0), prevVel_); + newValues.insert(B(0), prevBias_); + + newTimestamps[X(0)] = currentCorrectionTime; + newTimestamps[V(0)] = currentCorrectionTime; + newTimestamps[B(0)] = currentCorrectionTime; + + if (config_.use_batch_fixed_lag) { + batchSmoother_->update(newFactors, newValues, newTimestamps); + } else { + incrementalSmoother_->update(newFactors, newValues, newTimestamps); + } + } else { + // Initialize traditional ISAM2 optimizer + initializeTraditionalOptimizer(); + + gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise); + graphFactors.add(priorPose); + + gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise); + graphFactors.add(priorVel); + + gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise); + graphFactors.add(priorBias); + + graphValues.insert(X(0), prevPose_); + graphValues.insert(V(0), prevVel_); + graphValues.insert(B(0), prevBias_); + + if (config_.enable_marginalization) { + keyTimestamps[X(0)] = currentCorrectionTime; + keyTimestamps[V(0)] = currentCorrectionTime; + keyTimestamps[B(0)] = currentCorrectionTime; + oldestTimestamp = currentCorrectionTime; + newestTimestamp = currentCorrectionTime; + } + + optimizer.update(graphFactors, graphValues); + graphFactors.resize(0); + graphValues.clear(); + } + imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); - + key = 1; systemInitialized = true; } void imuPreintegration::integrate_imumeasurement(double currentCorrectionTime) { - // 1. integrate imu data and optimize - - while (!imuQueOpt.empty()) - { - // pop and integrate imu data that is between two optimizations + while (!imuQueOpt.empty()) { sensor_msgs::msg::Imu *thisImu = &imuQueOpt.front(); double imuTime = secs(thisImu); - if (imuTime < currentCorrectionTime - delta_t) - { + if (imuTime < currentCorrectionTime - delta_t) { double dt = (lastImuT_opt < 0) ? (1.0 / 200.0) : (imuTime - lastImuT_opt); lastImuT_opt = imuTime; @@ -266,31 +325,138 @@ namespace super_odometry { imuIntegratorOpt_->integrateMeasurement( gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z), - gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt); + gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt); imuQueOpt.pop_front(); } else break; } - } - bool imuPreintegration::build_graph(gtsam::Pose3 lidarPose, double curLaserodomtimestamp) { - // Calculate relative transformation from lidar + if (config_.use_fixed_lag) { + return build_graph_with_fixed_lag(lidarPose, curLaserodomtimestamp); + } else { + return build_graph_traditional(lidarPose, curLaserodomtimestamp); + } + } + + bool imuPreintegration::build_graph_with_fixed_lag(gtsam::Pose3 lidarPose, double curLaserodomtimestamp) { + auto start_time = std::chrono::high_resolution_clock::now(); + gtsam::Pose3 relativeLidarPose = lastLidarPose.inverse().compose(lidarPose); gtsam::Pose3 relativeImuPose = lidar2Imu.inverse().compose(relativeLidarPose).compose(lidar2Imu); // Get IMU prediction gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); - // Add IMU factor + gtsam::NonlinearFactorGraph newFactors; + gtsam::Values newValues; + gtsam::FixedLagSmoother::KeyTimestampMap newTimestamps; + + const gtsam::PreintegratedImuMeasurements &preint_imu = + dynamic_cast(*imuIntegratorOpt_); + newFactors.add(gtsam::ImuFactor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu)); + + newFactors.add(gtsam::BetweenFactor(X(key - 1), X(key), relativeImuPose, correctionNoise)); + + newFactors.add(gtsam::BetweenFactor( + B(key - 1), B(key), gtsam::imuBias::ConstantBias(), + gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias))); + + newValues.insert(X(key), prevPose_.compose(relativeImuPose)); + newValues.insert(V(key), propState_.v()); + newValues.insert(B(key), prevBias_); + + newTimestamps[X(key)] = curLaserodomtimestamp; + newTimestamps[V(key)] = curLaserodomtimestamp; + newTimestamps[B(key)] = curLaserodomtimestamp; + + auto opt_start = std::chrono::high_resolution_clock::now(); + + bool systemSolvedSuccessfully = false; + try { + if (config_.use_batch_fixed_lag) { + auto result = batchSmoother_->update(newFactors, newValues, newTimestamps); + size_t currentSize = batchSmoother_->getFactors().size(); + if (key % 10 == 0 && currentSize > 0) { + perf_metrics_.num_marginalizations++; + } + } else { + auto result = incrementalSmoother_->update(newFactors, newValues, newTimestamps); + auto timestamps = incrementalSmoother_->timestamps(); + + size_t keysInWindow = 0; + double currentTime = curLaserodomtimestamp; + for (const auto& [key_ts, time] : timestamps) { + if (currentTime - time <= config_.fixed_lag_size) { + keysInWindow++; + } + } + + size_t marginalizedCount = totalKeysProcessed * 3 - keysInWindow; + if (marginalizedCount > perf_metrics_.total_marginalized_keys) { + size_t newlyMarginalized = marginalizedCount - perf_metrics_.total_marginalized_keys; + perf_metrics_.total_marginalized_keys = marginalizedCount; + + if (newlyMarginalized > 0) { + perf_metrics_.num_marginalizations++; + } + } + } + + systemSolvedSuccessfully = true; + } + catch (const std::exception& e) { + systemSolvedSuccessfully = false; + RCLCPP_WARN(this->get_logger(), + "Fixed-lag smoother update failed: %s", e.what()); + } + + auto opt_end = std::chrono::high_resolution_clock::now(); + double opt_time_ms = std::chrono::duration(opt_end - opt_start).count(); + + updatePerformanceMetrics(opt_time_ms); + + if (systemSolvedSuccessfully) { + gtsam::Values result; + if (config_.use_batch_fixed_lag) { + result = batchSmoother_->calculateEstimate(); + } else { + result = incrementalSmoother_->calculateEstimate(); + } + + prevPose_ = result.at(X(key)); + prevVel_ = result.at(V(key)); + prevState_ = gtsam::NavState(prevPose_, prevVel_); + prevBias_ = result.at(B(key)); + imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); + + worldPose = lidarPose.compose(lidar2Imu); + lastLidarPose = lidarPose; + + // Always update pose history here in fixed-lag mode + TimestampedPose tsPose(curLaserodomtimestamp, worldPose, prevPose_, key); + poseHistory[curLaserodomtimestamp] = tsPose; + } + + logPerformanceTiming(start_time, opt_time_ms, curLaserodomtimestamp); + + return systemSolvedSuccessfully; + } + + bool imuPreintegration::build_graph_traditional(gtsam::Pose3 lidarPose, double curLaserodomtimestamp) { + auto start_time = std::chrono::high_resolution_clock::now(); + + gtsam::Pose3 relativeLidarPose = lastLidarPose.inverse().compose(lidarPose); + gtsam::Pose3 relativeImuPose = lidar2Imu.inverse().compose(relativeLidarPose).compose(lidar2Imu); + + gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); + const gtsam::PreintegratedImuMeasurements &preint_imu = - dynamic_cast( - *imuIntegratorOpt_); - gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), - B(key - 1), preint_imu); + dynamic_cast(*imuIntegratorOpt_); + gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu); graphFactors.add(imu_factor); // Add relative pose constraint from lidar @@ -307,8 +473,15 @@ namespace super_odometry { graphValues.insert(V(key), propState_.v()); graphValues.insert(B(key), prevBias_); - - // optimize + if (config_.enable_marginalization) { + keyTimestamps[X(key)] = curLaserodomtimestamp; + keyTimestamps[V(key)] = curLaserodomtimestamp; + keyTimestamps[B(key)] = curLaserodomtimestamp; + newestTimestamp = curLaserodomtimestamp; + } + + auto opt_start = std::chrono::high_resolution_clock::now(); + bool systemSolvedSuccessfully = false; try { optimizer.update(graphFactors, graphValues); @@ -319,26 +492,82 @@ namespace super_odometry { systemSolvedSuccessfully = false; RCLCPP_WARN(this->get_logger(), "Update failed due to underconstrained call to isam2 in imuPreintegration"); } - + + auto opt_end = std::chrono::high_resolution_clock::now(); + double opt_time_ms = std::chrono::duration(opt_end - opt_start).count(); + + updatePerformanceMetrics(opt_time_ms); + graphFactors.resize(0); graphValues.clear(); - + if (systemSolvedSuccessfully) { - gtsam::Values result = optimizer.calculateEstimate(); + prevPose_ = result.at(X(key)); prevVel_ = result.at(V(key)); prevState_ = gtsam::NavState(prevPose_, prevVel_); prevBias_ = result.at(B(key)); imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); - // Update world pose tracking - just use the lidar pose directly worldPose = lidarPose.compose(lidar2Imu); lastLidarPose = lidarPose; } + logPerformanceTiming(start_time, opt_time_ms, curLaserodomtimestamp); + return systemSolvedSuccessfully; } + + void imuPreintegration::updatePerformanceMetrics(double opt_time_ms) { + perf_metrics_.last_optimization_time_ms = opt_time_ms; + perf_metrics_.max_optimization_time_ms = std::max(perf_metrics_.max_optimization_time_ms, opt_time_ms); + perf_metrics_.avg_optimization_time_ms = + (perf_metrics_.avg_optimization_time_ms * perf_metrics_.optimization_count + opt_time_ms) / + (perf_metrics_.optimization_count + 1); + perf_metrics_.optimization_count++; + + // Get current graph size based on optimizer type + if (config_.use_fixed_lag) { + if (config_.use_batch_fixed_lag) { + perf_metrics_.current_graph_size = batchSmoother_->getFactors().size(); + } else { + perf_metrics_.current_graph_size = incrementalSmoother_->getFactors().size(); + } + } else { + perf_metrics_.current_graph_size = optimizer.size(); + } + + perf_metrics_.max_graph_size = std::max(perf_metrics_.max_graph_size, perf_metrics_.current_graph_size); + } + + void imuPreintegration::logPerformanceTiming(std::chrono::high_resolution_clock::time_point start_time, + double opt_time_ms, double timestamp) { + auto end_time = std::chrono::high_resolution_clock::now(); + double total_time_ms = std::chrono::duration(end_time - start_time).count(); + + perf_metrics_.last_graph_update_time_ms = total_time_ms; + perf_metrics_.max_graph_update_time_ms = std::max(perf_metrics_.max_graph_update_time_ms, total_time_ms); + perf_metrics_.avg_graph_update_time_ms = + (perf_metrics_.avg_graph_update_time_ms * perf_metrics_.total_frames_processed + total_time_ms) / + (perf_metrics_.total_frames_processed + 1); + + if (key % 10 == 0) { + const char* optimizer_type = config_.use_fixed_lag ? + (config_.use_batch_fixed_lag ? "Fixed-Lag Batch" : "Fixed-Lag Incremental") : + "Traditional ISAM2"; + + RCLCPP_INFO(this->get_logger(), + "%s Update [Key %d]: Opt time: %.2f ms (avg: %.2f ms), " + "Graph size: %zu nodes, Total time: %.2f ms", + optimizer_type, key, opt_time_ms, perf_metrics_.avg_optimization_time_ms, + perf_metrics_.current_graph_size, total_time_ms); + } + + if (perf_logging_enabled_) { + logPerformanceToCSV(timestamp, opt_time_ms, total_time_ms); + } + } void imuPreintegration::repropagate_imuodometry(double currentCorrectionTime) { prevStateOdom = prevState_; @@ -371,29 +600,471 @@ namespace super_odometry { } void imuPreintegration::process_imu_odometry(double currentCorrectionTime, gtsam::Pose3 lidarPose) { - // Reset graph periodically - if (key > 100) { - reset_graph(); + auto proc_start = std::chrono::high_resolution_clock::now(); + + // Handle marginalization/reset logic based on optimizer type + if (!config_.use_fixed_lag && config_.enable_marginalization) { + handleTraditionalMarginalization(currentCorrectionTime); } - - // 1. integrate_imumeasurement + integrate_imumeasurement(currentCorrectionTime); - - // Build and optimize graph + bool successOptimization = build_graph(lidarPose, currentCorrectionTime); // Check for failures if (failureDetection(prevVel_, prevBias_) || !successOptimization) { - RCLCPP_WARN(this->get_logger(), "failureDetected"); + RCLCPP_WARN(this->get_logger(), "Failure detected, resetting"); resetParams(); return; } - - // 4. reprogate_imuodometry + repropagate_imuodometry(currentCorrectionTime); + + // Only handle traditional marginalization pose history here + // Fixed-lag pose history is already handled in build_graph_with_fixed_lag() + if (successOptimization && !config_.use_fixed_lag && config_.enable_marginalization) { + gtsam::Values currentEstimate = optimizer.calculateEstimate(); + gtsam::Pose3 localPose = currentEstimate.at(X(key)); + TimestampedPose tsPose(currentCorrectionTime, worldPose, localPose, key); + poseHistory[currentCorrectionTime] = tsPose; + } + + cleanOldPoseHistory(currentCorrectionTime); + ++key; - + ++totalKeysProcessed; + perf_metrics_.total_frames_processed++; + + auto proc_end = std::chrono::high_resolution_clock::now(); + double proc_time_ms = std::chrono::duration(proc_end - proc_start).count(); + perf_metrics_.total_processing_time_ms += proc_time_ms; + doneFirstOpt = true; + + // Periodic performance reports + if (key % 50 == 0) { + reportDetailedPerformance(); + } + + if (totalKeysProcessed % 500 == 0) { + reportPerformanceMetrics(); + } + } + + void imuPreintegration::handleTraditionalMarginalization(double currentCorrectionTime) { + // Store pose history for proper tracking + if (key > 0 && doneFirstOpt) { + gtsam::Values currentEstimate = optimizer.calculateEstimate(); + gtsam::Pose3 localPose = currentEstimate.at(X(key - 1)); + + TimestampedPose tsPose(currentCorrectionTime, worldPose, localPose, key - 1); + poseHistory[currentCorrectionTime] = tsPose; + } + + // Check if we need to perform smart reset + if (key > 20) { + double timeWindow = newestTimestamp - oldestTimestamp; + + if (timeWindow > config_.marginalization_window_size) { + double cutoffTime = newestTimestamp - config_.marginalization_window_size; + int oldestKeyToKeep = -1; + + for (int i = 0; i < key; i++) { + if (keyTimestamps.count(X(i)) > 0) { + if (keyTimestamps[X(i)] >= cutoffTime) { + oldestKeyToKeep = i; + break; + } + } + } + + if (oldestKeyToKeep > 0 && oldestKeyToKeep < key - 3) { + performSmartReset(oldestKeyToKeep); + } + } + } + + // Hard reset if graph gets too large + if (key > 300) { + RCLCPP_INFO(this->get_logger(), + "Graph size exceeded maximum, performing smart reset at key %d", key); + performSmartReset(key - 10); + } + } + + void imuPreintegration::performSmartReset(int keepFromKey) { + auto reset_start = std::chrono::high_resolution_clock::now(); + + gtsam::Values currentEstimate = optimizer.calculateEstimate(); + + gtsam::Pose3 resetPose = currentEstimate.at(X(keepFromKey)); + gtsam::Vector3 resetVel = currentEstimate.at(V(keepFromKey)); + gtsam::imuBias::ConstantBias resetBias = + currentEstimate.at(B(keepFromKey)); + + gtsam::Pose3 currentLocalPose = currentEstimate.at(X(key - 1)); + gtsam::Pose3 relativeFromReset = resetPose.inverse().compose(currentLocalPose); + + initializeTraditionalOptimizer(); + + gtsam::PriorFactor priorPose(X(0), gtsam::Pose3(), priorPoseNoise); + graphFactors.add(priorPose); + + gtsam::PriorFactor priorVel(V(0), resetVel, priorVelNoise); + graphFactors.add(priorVel); + + gtsam::PriorFactor priorBias(B(0), resetBias, priorBiasNoise); + graphFactors.add(priorBias); + + graphValues.insert(X(0), gtsam::Pose3()); + graphValues.insert(V(0), resetVel); + graphValues.insert(B(0), resetBias); + + double resetTime = keyTimestamps[X(keepFromKey)]; + keyTimestamps.clear(); + keyTimestamps[X(0)] = resetTime; + keyTimestamps[V(0)] = resetTime; + keyTimestamps[B(0)] = resetTime; + oldestTimestamp = resetTime; + newestTimestamp = resetTime; + + prevPose_ = relativeFromReset; + prevVel_ = currentEstimate.at(V(key - 1)); + prevBias_ = currentEstimate.at(B(key - 1)); + prevState_ = gtsam::NavState(prevPose_, prevVel_); + + imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); + imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); + + optimizer.update(graphFactors, graphValues); + graphFactors.resize(0); + graphValues.clear(); + + auto reset_end = std::chrono::high_resolution_clock::now(); + double reset_time_ms = std::chrono::duration(reset_end - reset_start).count(); + + perf_metrics_.num_resets_performed++; + + int oldKey = key; + key = 1; + + RCLCPP_INFO(this->get_logger(), + "Smart reset completed in %.2f ms: old key %d -> new key %d, reset #%d", + reset_time_ms, oldKey, key, perf_metrics_.num_resets_performed); + } + + void imuPreintegration::reportDetailedPerformance() { + size_t current_memory_mb = getCurrentMemoryUsageMB(); + perf_metrics_.current_memory_usage_mb = current_memory_mb; + perf_metrics_.peak_memory_usage_mb = std::max(perf_metrics_.peak_memory_usage_mb, current_memory_mb); + + const char* optimizer_type = config_.use_fixed_lag ? + (config_.use_batch_fixed_lag ? "FIXED-LAG BATCH" : "FIXED-LAG INCREMENTAL") : + "TRADITIONAL ISAM2"; + + if (config_.use_fixed_lag) { + RCLCPP_INFO(this->get_logger(), + "\n╔════════════════════════════════════════════════════════╗\n" + "║ %s PERFORMANCE (Key %d) ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Graph Statistics: ║\n" + "║ Current Size: %zu factors ║\n" + "║ Total Keys Processed: %d ║\n" + "║ Total Marginalized: %zu keys ║\n" + "║ Marginalization Events: %d ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Timing Performance: ║\n" + "║ Last Optimization: %.2f ms ║\n" + "║ Avg Optimization: %.2f ms ║\n" + "║ Max Optimization: %.2f ms ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Memory Usage: ║\n" + "║ Current: %zu MB ║\n" + "║ Peak: %zu MB ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Configuration: ║\n" + "║ Fixed Lag Size: %.1f seconds ║\n" + "╚════════════════════════════════════════════════════════╝", + optimizer_type, key, + perf_metrics_.current_graph_size, + totalKeysProcessed, + perf_metrics_.total_marginalized_keys, + perf_metrics_.num_marginalizations, + perf_metrics_.last_optimization_time_ms, + perf_metrics_.avg_optimization_time_ms, + perf_metrics_.max_optimization_time_ms, + perf_metrics_.current_memory_usage_mb, + perf_metrics_.peak_memory_usage_mb, + config_.fixed_lag_size + ); + } else { + RCLCPP_INFO(this->get_logger(), + "\n╔════════════════════════════════════════════════════════╗\n" + "║ %s PERFORMANCE (Key %d) ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Graph Statistics: ║\n" + "║ Current Size: %zu nodes (Max: %zu) ║\n" + "║ Keys Since Reset: %d ║\n" + "║ Total Keys Processed: %d ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Timing Performance: ║\n" + "║ Last Optimization: %.2f ms ║\n" + "║ Avg Optimization: %.2f ms ║\n" + "║ Max Optimization: %.2f ms ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Memory Usage: ║\n" + "║ Current: %zu MB ║\n" + "║ Peak: %zu MB ║\n" + "╠════════════════════════════════════════════════════════╣\n" + "║ Configuration: ║\n" + "║ Smart Reset: %s ║\n" + "║ Window Size: %.1f seconds ║\n" + "╚════════════════════════════════════════════════════════╝", + optimizer_type, key, + perf_metrics_.current_graph_size, + perf_metrics_.max_graph_size, + key, + totalKeysProcessed, + perf_metrics_.last_optimization_time_ms, + perf_metrics_.avg_optimization_time_ms, + perf_metrics_.max_optimization_time_ms, + perf_metrics_.current_memory_usage_mb, + perf_metrics_.peak_memory_usage_mb, + config_.enable_marginalization ? "ENABLED" : "DISABLED", + config_.marginalization_window_size + ); + } + + if (perf_metrics_.last_optimization_time_ms > 50.0) { + RCLCPP_WARN(this->get_logger(), + "⚠️ Optimization time exceeding 50ms! Consider tuning parameters."); + } + } + + void imuPreintegration::reportPerformanceMetrics() { + auto current_time = std::chrono::high_resolution_clock::now(); + double runtime_seconds = std::chrono::duration(current_time - start_time_).count(); + + if (config_.use_fixed_lag) { + RCLCPP_INFO(this->get_logger(), + "\n========== FIXED-LAG SMOOTHER SUMMARY ==========\n" + "Runtime: %.1f seconds\n" + "Smoother Type: %s\n" + "Fixed Lag: %.1f seconds\n" + "Marginalization:\n" + " - Events: %d\n" + " - Total keys marginalized: %zu\n" + "Graph Performance:\n" + " - Current size: %zu factors\n" + " - Max size: %zu factors\n" + "Optimization Performance:\n" + " - Average: %.2f ms\n" + " - Max: %.2f ms\n" + "Memory Usage:\n" + " - Current: %zu MB\n" + " - Peak: %zu MB\n" + "================================================", + runtime_seconds, + config_.use_batch_fixed_lag ? "Batch" : "Incremental", + config_.fixed_lag_size, + perf_metrics_.num_marginalizations, + perf_metrics_.total_marginalized_keys, + perf_metrics_.current_graph_size, + perf_metrics_.max_graph_size, + perf_metrics_.avg_optimization_time_ms, + perf_metrics_.max_optimization_time_ms, + perf_metrics_.current_memory_usage_mb, + perf_metrics_.peak_memory_usage_mb + ); + } else { + RCLCPP_INFO(this->get_logger(), + "\n========== TRADITIONAL ISAM2 SUMMARY ==========\n" + "Runtime: %.1f seconds\n" + "Smart Reset: %s\n" + " - Resets performed: %d\n" + " - Graph size: %zu nodes\n" + "Optimization Performance:\n" + " - Average: %.2f ms\n" + " - Max: %.2f ms\n" + "Memory Usage:\n" + " - Current: %zu MB\n" + " - Peak: %zu MB\n" + "Processing:\n" + " - Total frames: %d\n" + "==============================================", + runtime_seconds, + config_.enable_marginalization ? "ENABLED" : "DISABLED", + perf_metrics_.num_resets_performed, + perf_metrics_.current_graph_size, + perf_metrics_.avg_optimization_time_ms, + perf_metrics_.max_optimization_time_ms, + perf_metrics_.current_memory_usage_mb, + perf_metrics_.peak_memory_usage_mb, + perf_metrics_.total_frames_processed + ); + } + } + + void imuPreintegration::comparePerformanceWithBaseline() { + if (config_.use_fixed_lag) { + RCLCPP_INFO(this->get_logger(), + "\n===== FIXED-LAG SMOOTHER ADVANTAGES =====\n" + "✓ True marginalization preserves information\n" + "✓ Maintains full covariance estimates\n" + "✓ Bounded computational complexity: O(1)\n" + "✓ Graph size automatically managed\n" + "✓ No manual resets needed\n" + "✓ Smooth continuous estimation\n" + "==========================================\n" + "Graph bounded to lag of %.1f seconds\n" + "Optimization time stable at: %.2f ms\n" + "Memory usage stable at: %zu MB\n" + "Can run indefinitely with consistent performance", + config_.fixed_lag_size, + perf_metrics_.avg_optimization_time_ms, + perf_metrics_.current_memory_usage_mb + ); + } else if (config_.enable_marginalization) { + RCLCPP_INFO(this->get_logger(), + "\n===== SMART RESET PERFORMANCE =====\n" + "Graph bounded to: %d nodes\n" + "Optimization time stable at: %.2f ms\n" + "Memory usage stable at: %zu MB\n" + "Resets performed: %d\n" + "Can run indefinitely without degradation\n" + "====================================", + key, + perf_metrics_.avg_optimization_time_ms, + perf_metrics_.current_memory_usage_mb, + perf_metrics_.num_resets_performed + ); + } else { + RCLCPP_WARN(this->get_logger(), + "\n===== UNBOUNDED GROWTH WARNING =====\n" + "No marginalization or fixed-lag enabled!\n" + "Graph will grow unbounded: %zu nodes\n" + "Performance will degrade over time\n" + "Consider enabling fixed-lag or marginalization\n" + "===================================", + perf_metrics_.current_graph_size + ); + } + } + + size_t imuPreintegration::getCurrentMemoryUsageMB() { + std::ifstream file("/proc/self/status"); + std::string line; + size_t memory_kb = 0; + + while (std::getline(file, line)) { + if (line.substr(0, 6) == "VmRSS:") { + std::istringstream iss(line); + std::string label; + iss >> label >> memory_kb; + break; + } + } + + return memory_kb / 1024; + } + + void imuPreintegration::logPerformanceToCSV(double timestamp, double opt_time_ms, double total_time_ms) { + static std::ofstream perf_log; + static bool first_write = true; + + if (first_write) { + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream ss; + + if (config_.use_fixed_lag) { + ss << "imu_performance_fixed_lag_" + << (config_.use_batch_fixed_lag ? "batch_" : "incremental_"); + } else { + ss << "imu_performance_traditional_" + << (config_.enable_marginalization ? "with_reset_" : "without_reset_"); + } + + ss << std::put_time(std::localtime(&time_t), "%Y%m%d_%H%M%S") << ".csv"; + + perf_log.open(ss.str()); + perf_log << "timestamp,key,graph_size,opt_time_ms,total_time_ms,memory_mb," + << "pose_history_size,total_keys_processed,marginalized_keys,events\n"; + first_write = false; + + RCLCPP_INFO(this->get_logger(), "Performance logging to: %s", ss.str().c_str()); + } + + perf_log << std::fixed << std::setprecision(6) + << timestamp << "," + << key << "," + << perf_metrics_.current_graph_size << "," + << std::setprecision(3) + << opt_time_ms << "," + << total_time_ms << "," + << perf_metrics_.current_memory_usage_mb << "," + << poseHistory.size() << "," + << totalKeysProcessed << "," + << perf_metrics_.total_marginalized_keys << "," + << (config_.use_fixed_lag ? perf_metrics_.num_marginalizations : perf_metrics_.num_resets_performed) << "\n"; + perf_log.flush(); + } + + void imuPreintegration::cleanOldPoseHistory(double currentTime, double keepDuration) { + double cutoffTime = currentTime - keepDuration; + + // Add debug logging + size_t sizeBefore = poseHistory.size(); + + auto it = poseHistory.begin(); + while (it != poseHistory.end()) { + if (it->first < cutoffTime) { + it = poseHistory.erase(it); + } else { + break; + } + } + + while (poseHistory.size() > 1000) { + poseHistory.erase(poseHistory.begin()); + } + + // Debug logging every 100 frames + if (key % 100 == 0) { + RCLCPP_INFO(this->get_logger(), + "Pose history: %zu entries (was %zu), current time: %.3f", + poseHistory.size(), sizeBefore, currentTime); + } + } + + TimestampedPose imuPreintegration::interpolatePose(double queryTime) const { + if (poseHistory.empty()) { + return TimestampedPose(); + } + + auto upper = poseHistory.lower_bound(queryTime); + + if (upper == poseHistory.end()) { + return poseHistory.rbegin()->second; + } + + if (upper == poseHistory.begin()) { + return upper->second; + } + + auto lower = std::prev(upper); + + double t1 = lower->first; + double t2 = upper->first; + double alpha = (queryTime - t1) / (t2 - t1); + + gtsam::Pose3 interpolatedWorld = lower->second.worldPose.interpolateRt( + upper->second.worldPose, alpha); + + gtsam::Pose3 interpolatedLocal = lower->second.localPose.interpolateRt( + upper->second.localPose, alpha); + + return TimestampedPose(queryTime, interpolatedWorld, interpolatedLocal, -1); } bool imuPreintegration::failureDetection(const gtsam::Vector3 &velCur, diff --git a/super_odometry/src/analysis.py b/super_odometry/src/analysis.py new file mode 100644 index 0000000..832ba1b --- /dev/null +++ b/super_odometry/src/analysis.py @@ -0,0 +1,334 @@ +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np +import os + +def load_and_analyze(csv_path, label): + """Load CSV and return analysis results""" + df = pd.read_csv(csv_path) + + # Calculate statistics + stats = { + 'label': label, + 'runtime': df['timestamp'].max() - df['timestamp'].min(), + 'total_frames': df['total_keys_processed'].max(), + 'num_events': df['events'].max(), # Updated: events instead of num_resets + 'avg_opt_time': df['opt_time_ms'].mean(), + 'max_opt_time': df['opt_time_ms'].max(), + 'avg_total_time': df['total_time_ms'].mean(), + 'max_total_time': df['total_time_ms'].max(), + 'avg_memory': df['memory_mb'].mean(), + 'peak_memory': df['memory_mb'].max(), + 'final_graph_size': df['graph_size'].iloc[-1] if len(df) > 0 else 0, + 'max_graph_size': df['graph_size'].max(), + 'total_marginalized': df['marginalized_keys'].max(), # New: marginalized keys + 'avg_pose_history': df['pose_history_size'].mean(), # New: pose history + 'max_pose_history': df['pose_history_size'].max(), + 'final_key': df['key'].max() # New: final key value + } + + # Identify event points (resets or marginalizations) + df['event_occurred'] = df['events'].diff() > 0 + event_timestamps = df[df['event_occurred']]['timestamp'].values + + # Performance over time (compare early vs late performance) + runtime = stats['runtime'] + if runtime > 20: # If runtime > 20 seconds, compare first/last 10 seconds + late_window = 10 + early_window = 10 + else: # For shorter runs, use 25% of runtime + late_window = runtime * 0.25 + early_window = runtime * 0.25 + + late_perf = df[df['timestamp'] > df['timestamp'].max() - late_window]['opt_time_ms'].mean() + early_perf = df[df['timestamp'] < df['timestamp'].min() + early_window]['opt_time_ms'].mean() + stats['degradation'] = late_perf - early_perf + + # Calculate efficiency metrics + if stats['total_frames'] > 0: + stats['avg_frames_per_event'] = stats['total_frames'] / max(stats['num_events'], 1) + stats['marginalization_ratio'] = stats['total_marginalized'] / stats['total_frames'] + else: + stats['avg_frames_per_event'] = 0 + stats['marginalization_ratio'] = 0 + + return df, stats, event_timestamps + +def print_comparison(stats1, stats2): + """Print comparison between two runs""" + print("\n" + "="*80) + print("PERFORMANCE COMPARISON") + print("="*80) + print(f"Dataset 1: {stats1['label']}") + print(f"Dataset 2: {stats2['label']}") + print("="*80) + + print(f"\n{'Metric':<35} {'Dataset 1':<15} {'Dataset 2':<15} {'Difference':<15}") + print("-"*85) + + # Runtime + print(f"{'Runtime (s)':<35} {stats1['runtime']:<15.2f} {stats2['runtime']:<15.2f} " + f"{(stats2['runtime']-stats1['runtime']):<15.2f}") + + # Frames processed + print(f"{'Total Frames Processed':<35} {stats1['total_frames']:<15} {stats2['total_frames']:<15} " + f"{stats2['total_frames']-stats1['total_frames']:<15}") + + # Events (resets or marginalizations) + print(f"{'Total Events':<35} {stats1['num_events']:<15} {stats2['num_events']:<15} " + f"{stats2['num_events']-stats1['num_events']:<15}") + + # Keys marginalized + print(f"{'Keys Marginalized':<35} {stats1['total_marginalized']:<15} {stats2['total_marginalized']:<15} " + f"{stats2['total_marginalized']-stats1['total_marginalized']:<15}") + + # Graph size + print(f"{'Final Graph Size':<35} {stats1['final_graph_size']:<15} {stats2['final_graph_size']:<15} " + f"{stats2['final_graph_size']-stats1['final_graph_size']:<15}") + + print(f"{'Max Graph Size':<35} {stats1['max_graph_size']:<15} {stats2['max_graph_size']:<15} " + f"{stats2['max_graph_size']-stats1['max_graph_size']:<15}") + + # Final key + print(f"{'Final Key Value':<35} {stats1['final_key']:<15} {stats2['final_key']:<15} " + f"{stats2['final_key']-stats1['final_key']:<15}") + + # Optimization time + print(f"{'Avg Optimization (ms)':<35} {stats1['avg_opt_time']:<15.2f} {stats2['avg_opt_time']:<15.2f} " + f"{(stats2['avg_opt_time']-stats1['avg_opt_time']):<15.2f}") + + print(f"{'Max Optimization (ms)':<35} {stats1['max_opt_time']:<15.2f} {stats2['max_opt_time']:<15.2f} " + f"{(stats2['max_opt_time']-stats1['max_opt_time']):<15.2f}") + + # Total processing time + print(f"{'Avg Total Time (ms)':<35} {stats1['avg_total_time']:<15.2f} {stats2['avg_total_time']:<15.2f} " + f"{(stats2['avg_total_time']-stats1['avg_total_time']):<15.2f}") + + # Memory + print(f"{'Avg Memory (MB)':<35} {stats1['avg_memory']:<15.2f} {stats2['avg_memory']:<15.2f} " + f"{(stats2['avg_memory']-stats1['avg_memory']):<15.2f}") + + print(f"{'Peak Memory (MB)':<35} {stats1['peak_memory']:<15.2f} {stats2['peak_memory']:<15.2f} " + f"{(stats2['peak_memory']-stats1['peak_memory']):<15.2f}") + + # Pose history + print(f"{'Avg Pose History Size':<35} {stats1['avg_pose_history']:<15.2f} {stats2['avg_pose_history']:<15.2f} " + f"{(stats2['avg_pose_history']-stats1['avg_pose_history']):<15.2f}") + + print(f"{'Max Pose History Size':<35} {stats1['max_pose_history']:<15.2f} {stats2['max_pose_history']:<15.2f} " + f"{(stats2['max_pose_history']-stats1['max_pose_history']):<15.2f}") + + # Performance degradation + print(f"{'Performance Degradation (ms)':<35} {stats1['degradation']:<15.2f} {stats2['degradation']:<15.2f} " + f"{(stats2['degradation']-stats1['degradation']):<15.2f}") + + print("\n" + "="*80) + print("EFFICIENCY ANALYSIS") + print("="*80) + + # Graph size efficiency + if stats2['final_graph_size'] > 0: + graph_reduction = (1 - stats1['final_graph_size']/stats2['final_graph_size']) * 100 + print(f"Graph Size Reduction: {graph_reduction:.1f}%") + + # Optimization speedup + if stats2['avg_opt_time'] > 0: + opt_speedup = stats2['avg_opt_time'] / stats1['avg_opt_time'] + print(f"Optimization Speedup: {opt_speedup:.2f}x") + + # Memory efficiency + if stats2['peak_memory'] > 0: + memory_savings = stats2['peak_memory'] - stats1['peak_memory'] + memory_savings_pct = (memory_savings / stats2['peak_memory']) * 100 + print(f"Memory Savings: {memory_savings:.0f} MB ({memory_savings_pct:.1f}%)") + + # Marginalization efficiency + print(f"Marginalization Ratio Dataset 1: {stats1['marginalization_ratio']:.2%}") + print(f"Marginalization Ratio Dataset 2: {stats2['marginalization_ratio']:.2%}") + + # Events per frame + print(f"Events per 100 frames Dataset 1: {(stats1['num_events']/max(stats1['total_frames'], 1)*100):.1f}") + print(f"Events per 100 frames Dataset 2: {(stats2['num_events']/max(stats2['total_frames'], 1)*100):.1f}") + +def plot_comparison(df1, df2, stats1, stats2, event_timestamps1, event_timestamps2): + """Create comparison plots""" + fig = plt.figure(figsize=(18, 12)) + + # Create 2x2 subplot layout for focused comparison + gs = fig.add_gridspec(2, 2, hspace=0.4, wspace=0.3) + + # Convert to numpy arrays and normalize timestamps to start from 0 + t1 = df1['timestamp'].values - df1['timestamp'].min() + t2 = df2['timestamp'].values - df2['timestamp'].min() + event_timestamps1_norm = event_timestamps1 - df1['timestamp'].min() + event_timestamps2_norm = event_timestamps2 - df2['timestamp'].min() + + label1 = stats1['label'] + label2 = stats2['label'] + + # 1. Graph Size Comparison + ax1 = fig.add_subplot(gs[0, :]) + ax1.plot(t1, df1['graph_size'].values, 'b-', label=f'{label1}', linewidth=2) + ax1.plot(t2, df2['graph_size'].values, 'r--', label=f'{label2}', linewidth=2) + + # Mark events + for event_time in event_timestamps1_norm: + ax1.axvline(x=event_time, color='b', linestyle=':', alpha=0.6, linewidth=1) + for event_time in event_timestamps2_norm: + ax1.axvline(x=event_time, color='r', linestyle=':', alpha=0.6, linewidth=1) + + ax1.set_xlabel('Time (s)', fontsize=12) + ax1.set_ylabel('Graph Size (nodes)', fontsize=12) + ax1.set_title('Graph Size Comparison Over Time', fontsize=14, fontweight='bold') + ax1.legend(fontsize=11) + ax1.grid(True, alpha=0.3) + + # 2. Optimization Time Comparison + ax2 = fig.add_subplot(gs[1, 0]) + ax2.plot(t1, df1['opt_time_ms'].values, 'b-', label=f'{label1}', linewidth=2) + ax2.plot(t2, df2['opt_time_ms'].values, 'r--', label=f'{label2}', linewidth=2) + ax2.axhline(y=50, color='orange', linestyle='--', label='50ms threshold', linewidth=1.5) + ax2.set_xlabel('Time (s)', fontsize=12) + ax2.set_ylabel('Optimization Time (ms)', fontsize=12) + ax2.set_title('Optimization Performance', fontsize=13, fontweight='bold') + ax2.legend(fontsize=10) + ax2.grid(True, alpha=0.3) + + # 3. Memory Usage Comparison + ax3 = fig.add_subplot(gs[1, 1]) + ax3.plot(t1, df1['memory_mb'].values, 'b-', label=f'{label1}', linewidth=2) + ax3.plot(t2, df2['memory_mb'].values, 'r--', label=f'{label2}', linewidth=2) + ax3.set_xlabel('Time (s)', fontsize=12) + ax3.set_ylabel('Memory (MB)', fontsize=12) + ax3.set_title('Memory Usage', fontsize=13, fontweight='bold') + ax3.legend(fontsize=10) + ax3.grid(True, alpha=0.3) + + plt.suptitle(f'IMU Preintegration Performance: {label1} vs {label2}', + fontsize=16, fontweight='bold', y=0.98) + + plt.tight_layout() + + # Create efficiency analysis as a separate figure + create_efficiency_plot(stats1, stats2) + + return fig + +def create_efficiency_plot(stats1, stats2): + """Create efficiency analysis bar chart""" + fig, ax = plt.subplots(figsize=(12, 8)) + + label1 = stats1['label'] + label2 = stats2['label'] + + # Calculate efficiency improvements (negative means dataset1 is better) + improvements = [] + improvement_labels = [] + + # Graph Size Reduction + if stats2['final_graph_size'] > 0: + graph_improvement = (1 - stats1['final_graph_size']/stats2['final_graph_size']) * 100 + improvements.append(graph_improvement) + improvement_labels.append('Graph Size\nReduction') + + # Optimization Time Improvement + if stats1['avg_opt_time'] > 0: + opt_improvement = (1 - stats1['avg_opt_time']/stats2['avg_opt_time']) * 100 + improvements.append(opt_improvement) + improvement_labels.append('Optimization\nSpeed Gain') + + # Memory Efficiency + if stats2['peak_memory'] > 0: + memory_improvement = (1 - stats1['peak_memory']/stats2['peak_memory']) * 100 + improvements.append(memory_improvement) + improvement_labels.append('Memory\nReduction') + + # Runtime Efficiency + if stats2['runtime'] > 0: + runtime_improvement = (1 - stats1['runtime']/stats2['runtime']) * 100 + improvements.append(runtime_improvement) + improvement_labels.append('Runtime\nEfficiency') + + # Max Optimization Time Improvement + if stats2['max_opt_time'] > 0: + max_opt_improvement = (1 - stats1['max_opt_time']/stats2['max_opt_time']) * 100 + improvements.append(max_opt_improvement) + improvement_labels.append('Peak Opt Time\nReduction') + + # Color bars based on improvement (green = positive, red = negative) + colors = ['green' if imp > 0 else 'red' for imp in improvements] + + # Create bar chart + bars = ax.bar(improvement_labels, improvements, color=colors, alpha=0.7, + edgecolor='black', linewidth=1.2) + + # Customize the plot + ax.set_ylabel('Improvement (%)', fontsize=14, fontweight='bold') + ax.set_title(f'Efficiency Analysis: {label1} vs {label2}\n(Positive = {label1} Better, Negative = {label2} Better)', + fontsize=16, fontweight='bold', pad=20) + ax.axhline(y=0, color='black', linestyle='-', linewidth=1) + ax.grid(True, alpha=0.3, axis='y') + + # Add value labels on bars + for bar, improvement in zip(bars, improvements): + height = bar.get_height() + label_y = height + (2 if height >= 0 else -5) + va = 'bottom' if height >= 0 else 'top' + + ax.text(bar.get_x() + bar.get_width()/2., label_y, + f'{improvement:.1f}%', ha='center', va=va, + fontsize=12, fontweight='bold') + + + + # Rotate x-axis labels for better readability + plt.xticks(rotation=45, ha='right') + + plt.tight_layout() + plt.savefig('efficiency_analysis.png', dpi=150, bbox_inches='tight') + plt.show() + + return fig + +def main(): + + csv_file1 = '/home/avnish/Desktop/final_report/no_smart_no_fixed/imu_performance_traditional_without_reset_20250807_005336.csv' + csv_file2 = '/home/avnish/Desktop/final_report/test_fixed/imu_performance_fixed_lag_incremental_20250807_110334.csv' + + + # Check if files exist + if not os.path.exists(csv_file1): + print(f"Error: File {csv_file1} not found!") + print(f"Please update the path in the main() function") + return + if not os.path.exists(csv_file2): + print(f"Error: File {csv_file2} not found!") + print(f"Please update the path in the main() function") + return + + print(f"\nAnalyzing performance comparison:") + print(f" File 1: {os.path.basename(csv_file1)}") + print(f" File 2: {os.path.basename(csv_file2)}") + + # Load and analyze both files + df1, stats1, events1 = load_and_analyze(csv_file1, "Traditional ISAM2") + df2, stats2, events2 = load_and_analyze(csv_file2, "Fixed-Lag Smoother") + + # Print comparison + print_comparison(stats1, stats2) + + # Create comparison plots + fig = plot_comparison(df1, df2, stats1, stats2, events1, events2) + + # Generate output filename + output_file = 'imu_performance_comparison_modified.png' + + # Save the main figure + fig.savefig(output_file, dpi=150, bbox_inches='tight') + print(f"\nComparison plot saved to: {output_file}") + print(f"Efficiency analysis saved to: efficiency_analysis.png") + + plt.show() + +if __name__ == "__main__": + main() \ No newline at end of file