IMU Integration

class IMU::Point
{
public:
    Point(const float &acc_x, const float &acc_y, const float &acc_z,
          const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
          const double &timestamp);
    
    Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp);
    
    Eigen::Vector3f a;  // Accelerometer (m/s²)
    Eigen::Vector3f w;  // Gyroscope (rad/s)
    double t;           // Timestamp (seconds)
};

class IMU::Bias
{
public:
    Bias();
    Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
         const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z);
    
    float bax, bay, baz;  // Accelerometer bias (m/s²)
    float bwx, bwy, bwz;  // Gyroscope bias (rad/s)
};

class IMU::Calib
{
public:
    Calib(const Sophus::SE3<float> &Tbc, 
          const float &ng,   // Gyroscope noise
          const float &na,   // Accelerometer noise  
          const float &ngw,  // Gyroscope random walk
          const float &naw); // Accelerometer random walk
    
    Sophus::SE3<float> mTcb;  // Transform: Camera to Body (IMU)
    Sophus::SE3<float> mTbc;  // Transform: Body (IMU) to Camera
    Eigen::DiagonalMatrix<float,6> Cov;      // Measurement covariance
    Eigen::DiagonalMatrix<float,6> CovWalk;  // Random walk covariance
};

class IMU::Preintegrated
{
public:
    Preintegrated(const Bias &b_, const Calib &calib);
    
    // Integrate new measurement
    void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, 
                                  const Eigen::Vector3f &angVel, 
                                  const float &dt);
    
    // Recompute integration with updated bias
    void Reintegrate();
    
    // Get preintegrated values
    Eigen::Matrix3f GetDeltaRotation(const Bias &b_);
    Eigen::Vector3f GetDeltaVelocity(const Bias &b_);
    Eigen::Vector3f GetDeltaPosition(const Bias &b_);
    
    float dT;  // Total integration time
    
    // Preintegrated measurements (relative to initial bias)
    Eigen::Matrix3f dR;   // Rotation delta
    Eigen::Vector3f dV;   // Velocity delta  
    Eigen::Vector3f dP;   // Position delta
    
    // Jacobians for bias correction
    Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
    
    // Covariance and information matrix
    Eigen::Matrix<float,15,15> C;     // Covariance
    Eigen::Matrix<float,15,15> Info;  // Information matrix
};

// Check if IMU is initialized
if(mpAtlas->isImuInitialized()) {
    // Scale, gravity, and velocity are available
    // Full visual-inertial tracking active
} else {
    // Still initializing
    // System may be running visual-only tracking
}

const float GRAVITY_VALUE = 9.81;  // m/s²

// Predict camera pose using IMU integration
Sophus::SE3f Tcw_predicted = PredictNavStateFromIMU();

// Use prediction to guide feature search
SearchFeaturesInPredictedRegion(Tcw_predicted);

E_total = E_visual + E_inertial

E_visual = Σ ρ(|| z_i - π(T_cw * X_i) ||²)
E_inertial = Σ || r_imu(T_i, T_j, v_i, v_j, b) ||²_Σ

class IMU::IntegratedRotation
{
public:
    IntegratedRotation(const Eigen::Vector3f &angVel, 
                       const Bias &imuBias, 
                       const float &time);
    
    float deltaT;               // Integration time
    Eigen::Matrix3f deltaR;     // Rotation delta
    Eigen::Matrix3f rightJ;     // Right Jacobian
};

// Right Jacobian of SO(3)
Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v);
Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z);

// Inverse Right Jacobian
Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v);
Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y, const float &z);

// Normalize rotation matrix
Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);