After initializing and optimizing poses, the next step is to update these poses to the previous frame. Let's examine the updateLatestStates() source code:

void Navigator::updateLatestStates()
{
    // Lock mPropagate to ensure thread-safe updates to the latest state
    mPropagate.lock();
    // Update the latest timestamp with current frame timestamp plus time delay td
    current_time = FrameTimestamps[frame_index] + td;
    // Update the latest position information
    current_position = Positions[frame_index];
    // Update the latest orientation information (quaternion)
    current_orientation = Orientations[frame_index];
    // Update the latest velocity information
    current_velocity = Velocities[frame_index];
    // Update the latest accelerometer bias
    current_acc_bias = AccBiases[frame_index];
    // Update the latest gyroscope bias
    current_gyro_bias = GyroBiases[frame_index];
    // Update the latest accelerometer sensor data
    current_acc_0 = acc_0;
    // Update the latest gyroscope sensor data
    current_gyro_0 = gyro_0;
    // Lock mBuf to ensure thread-safe access to IMU data queue
    mBuf.lock();
    // Create a temporary accelerometer data queue
    queue> tmp_acc_queue = acc_queue;
    // Create a temporary gyroscope data queue
    queue> tmp_gyro_queue = gyro_queue;
    // Unlock mBuf
    mBuf.unlock();
    // Iterate through accelerometer and gyroscope data queues for fast prediction
    while(!tmp_acc_queue.empty())
    {
        // Extract timestamp from the queue
        double timestamp = tmp_acc_queue.front().first;
        // Extract accelerometer data from the queue
        Eigen::Vector3d accel_data = tmp_acc_queue.front().second;
        // Extract gyroscope data from the queue
        Eigen::Vector3d gyro_data = tmp_gyro_queue.front().second;
        // Call fastPredictIMU function for fast IMU prediction
        fastPredictIMU(timestamp, accel_data, gyro_data);
        // Remove processed data from the queue
        tmp_acc_queue.pop();
        tmp_gyro_queue.pop();
    }
    // Unlock mPropagate
    mPropagate.unlock();
}

This function primarily updates the estimator's latest state. Through locking mechanisms, it ensures thread safety, extracts the latest pose, velocity, biases, and sensor data, and utilizes IMU data queue for fast prediction. Let's examine the fast prediction function fastPredictIMU():

// Use inertial sensor data to quickly predict and update system state (pose, position, velocity)
// Ensure timely state prediction with each new sensor data arrival
void Navigator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)
{
    // Calculate time difference between current time and latest time
    double time_delta = t - current_time;
    // Update latest time to current time
    current_time = t;
    // Calculate initial gravity-free acceleration
    //current_orientation is the latest orientation quaternion, current_acc_0 is the latest accelerometer reading,
    //current_acc_bias is the accelerometer bias, g is gravity vector. The calculated gravity_free_accel_initial 
    //is the linear acceleration under current orientation minus gravity.
    Eigen::Vector3d gravity_free_accel_initial = current_orientation * (current_acc_0 - current_acc_bias) - g;
    // Calculate uncompensated gyroscope reading
    //current_gyro_0 is the latest gyroscope reading, angular_velocity is the current gyroscope reading,
    //current_gyro_bias is the gyroscope bias. The calculated un_gyro is the uncompensated gyroscope reading.
    Eigen::Vector3d un_gyro = 0.5 * (current_gyro_0 + angular_velocity) - current_gyro_bias;
    // Update orientation quaternion
    //Utility::deltaQ is a helper function to calculate rotation quaternion increment.
    //The updated orientation quaternion current_orientation is calculated based on current uncompensated 
    //gyroscope reading and time_delta.
    current_orientation = current_orientation * Utility::deltaQ(un_gyro * time_delta);
    // Calculate current gravity-free acceleration
    //linear_acceleration is the current accelerometer reading, current_acc_bias is the accelerometer bias.
    //The calculated gravity_free_accel_current is the linear acceleration under updated orientation minus gravity.
    Eigen::Vector3d gravity_free_accel_current = current_orientation * (linear_acceleration - current_acc_bias) - g;
    // Calculate average acceleration
    //Calculate the average of gravity_free_accel_initial and gravity_free_accel_current to get 
    //the average gravity-free acceleration.
    Eigen::Vector3d avg_gravity_free_accel = 0.5 * (gravity_free_accel_initial + gravity_free_accel_current);
    // Update position through double integration (displacement, velocity, and acceleration)
    //current_position is the current position, current_velocity is the current velocity, 
    //time_delta is the time difference, avg_gravity_free_accel is the average gravity-free acceleration.
    //Update position using quadratic integration formula.
    current_position = current_position + time_delta * current_velocity + 0.5 * time_delta * time_delta * avg_gravity_free_accel;
    // Update velocity through single integration
    //Update velocity using linear integration formula.
    current_velocity = current_velocity + time_delta * avg_gravity_free_accel;
    // Update latest accelerometer sensor data
    current_acc_0 = linear_acceleration;
    // Update latest gyroscope sensor data
    current_gyro_0 = angular_velocity;
}

This prediction process is based on existing IMU data, utilizing accelerometer and gyroscope sensor data to predict the state (including position, orientation, velocity, etc.). This process helps to quickly update and predict the system's current state before new IMU data arrives, thereby improving the system's real-time performance and continuity. Now, let's examine the sliding window function slideWindow():

void Navigator::slideWindow()
{
    // Create a timing object to record the marginalization operation time
    TicToc margin_timer;
    // If marginalization flag is MARGIN_OLD, remove the oldest frame from the sliding window
    if (marginalization_flag == MARGIN_OLD)
    {
        // Record the timestamp and orientation of the oldest frame
        double oldest_time = FrameTimestamps[0];
        back_R0 = Orientations[0];
        back_P0 = Positions[0];
        // If current frame count equals window size, the sliding window is full and needs to remove the oldest frame
        if (frame_index == WINDOW_SIZE)
        {
            // Shift each frame's data forward by one position
            for (int i = 0; i < WINDOW_SIZE; i++)
            {
                FrameTimestamps[i] = FrameTimestamps[i + 1];
                Orientations[i].swap(Orientations[i + 1]);
                Positions[i].swap(Positions[i + 1]);
                if (USE_IMU)
                {
                    std::swap(pre_integrations[i], pre_integrations[i + 1]);
                    time_delta_buf[i].swap(time_delta_buf[i + 1]);
                    linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
                    angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
                    Velocities[i].swap(Velocities[i + 1]);
                    AccBiases[i].swap(AccBiases[i + 1]);
                    GyroBiases[i].swap(GyroBiases[i + 1]);
                }
            }
            // Copy the last frame's data to the second-to-last position
            FrameTimestamps[WINDOW_SIZE] = FrameTimestamps[WINDOW_SIZE - 1];
            Positions[WINDOW_SIZE] = Positions[WINDOW_SIZE - 1];
            Orientations[WINDOW_SIZE] = Orientations[WINDOW_SIZE - 1];
            
            if (USE_IMU)
            {
                Velocities[WINDOW_SIZE] = Velocities[WINDOW_SIZE - 1];
                AccBiases[WINDOW_SIZE] = AccBiases[WINDOW_SIZE - 1];
                GyroBiases[WINDOW_SIZE] = GyroBiases[WINDOW_SIZE - 1];
                // Delete the last frame's pre-integration info and create a new pre-integration object
                delete pre_integrations[WINDOW_SIZE];
                pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyro_0, AccBiases[WINDOW_SIZE], GyroBiases[WINDOW_SIZE]};
                // Clear the last frame's time delta, linear acceleration, and angular velocity buffers
                time_delta_buf[WINDOW_SIZE].clear();
                linear_acceleration_buf[WINDOW_SIZE].clear();
                angular_velocity_buf[WINDOW_SIZE].clear();
            }
            
            // If in initial phase or solver_flag is INITIAL
            if (true || solver_flag == INITIAL)
            {
                // Delete the oldest frame's pre-integration info and remove it from all image frame records
                map::iterator frame_it;
                // Find the frame with timestamp oldest_time in all_image_frame and set iterator frame_it to it
                frame_it = all_image_frame.find(oldest_time);
                // Delete pre-integration info
                delete frame_it->second.pre_integration;
                // Delete image frame record
                all_image_frame.erase(all_image_frame.begin(), frame_it);
            }
            // Call slideWindowOld to handle marginalization of the oldest frame in the sliding window
            slideWindowOld();
        }
    }
    // If marginalization flag is not MARGIN_OLD, remove the newest frame from the sliding window
    else
    {
        // If current frame count equals window size, the sliding window is full and needs to remove the newest frame
        if (frame_index == WINDOW_SIZE)
        {
            // Copy the newest frame's data to the second-to-last position
            FrameTimestamps[frame_index - 1] = FrameTimestamps[frame_index];
            Positions[frame_index - 1] = Positions[frame_index];
            Orientations[frame_index - 1] = Orientations[frame_index];
            
            if (USE_IMU)
            {
                // Add the newest frame's IMU data to the second-to-last frame's pre-integration info
                for (unsigned int i = 0; i < time_delta_buf[frame_index].size(); i++)
                {
                    double tmp_dt = time_delta_buf[frame_index][i];
                    Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_index][i];
                    Vector3d tmp_angular_velocity = angular_velocity_buf[frame_index][i];
                    
                    pre_integrations[frame_index - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
                    
                    time_delta_buf[frame_index - 1].push_back(tmp_dt);
                    linear_acceleration_buf[frame_index - 1].push_back(tmp_linear_acceleration);
                    angular_velocity_buf[frame_index - 1].push_back(tmp_angular_velocity);
                }
                
                // Copy the newest frame's velocity, accelerometer bias, and gyroscope bias to the second-to-last frame
                Velocities[frame_index - 1] = Velocities[frame_index];
                AccBiases[frame_index - 1] = AccBiases[frame_index];
                GyroBiases[frame_index - 1] = GyroBiases[frame_index];
                
                // Delete the last frame's pre-integration info and create a new pre-integration object
                delete pre_integrations[WINDOW_SIZE];
                pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyro_0, AccBiases[WINDOW_SIZE], GyroBiases[WINDOW_SIZE]};
                
                // Clear the last frame's time delta, linear acceleration, and angular velocity buffers
                time_delta_buf[WINDOW_SIZE].clear();
                linear_acceleration_buf[WINDOW_SIZE].clear();
                angular_velocity_buf[WINDOW_SIZE].clear();
            }
            // Call slideWindowNew to handle marginalization of the newest frame in the sliding window
            slideWindowNew();
        }
    }
}

This function primarily manages frames in the sliding window. When the sliding window size reaches the preset maximum, it is responsible for removing the oldest or newest frame to make space for new frames. Based on the marginalization flag, it determines whether to remove the oldest or newest frame in the window: Marginalizing the oldest frame: When the sliding window is full and needs to marginalize the oldest frame, it shifts each frame's data forward by one position, processes IMU pre-integration information, and then calls slideWindowOld() for further processing. Marginalizing the newest frame: When the sliding window is full and needs to marginalize the newest frame, it copies the newest frame's data to the second-to-last position, processes IMU pre-integration information, and then calls slideWindowNew() for further processing. This implements frame management in the sliding window, ensuring the system can process new image and IMU data while maintaining a constant number of frames in the window. The slideWindowOld() source code:

void Navigator::slideWindowOld()
{
    // Increment oldest frame counter
    oldest_frame_count++;
    
    // Determine if depth adjustment is needed
    // If the solver state is NON_LINEAR, shift_depth is true, otherwise false
    bool shift_depth = solver_flag == NON_LINEAR ? true : false;
    if (shift_depth)
    {
        // Define variables R0 and R1 for rotation matrices, P0 and P1 for position vectors
        Matrix3d R0, R1;
        Vector3d P0, P1;
        // Calculate R0 and R1, the rotation matrices for the oldest and current oldest frames in the sliding window,
        // multiplied by the camera-to-IMU extrinsic parameters
        R0 = back_R0 * camera_imu_rotation[0];
        R1 = Orientations[0] * camera_imu_rotation[0];
        // Calculate P0 and P1, the position vectors for the oldest and current oldest frames,
        // performing translation transformation
        P0 = back_P0 + back_R0 * camera_imu_translation[0];
        P1 = Positions[0] + Orientations[0] * camera_imu_translation[0];
        // Call the feature manager's removeBackShiftDepth method to remove features from the oldest frame
        // and perform depth adjustment
        feature_manager.removeBackShiftDepth(R0, P0, R1, P1);
    }
    else
        // If no depth adjustment is needed, directly remove features from the oldest frame
        feature_manager.removeBack();
}

The slideWindowNew() source code:

void Navigator::slideWindowNew()
{
    // Increment newest frame counter
    newest_frame_count++;
    // Remove features corresponding to the current frame count from the feature manager
    feature_manager.removeFront(frame_index);
}

The slideWindow() function shares similarities with the marginalization processing in the optimization function, with the main difference being that slideWindow() is responsible for actually removing features and frames, calling specific functions to remove frames and manage features (removeBackShiftDepth, removeBack, and removeFront), while the optimization function focuses more on the marginalization operation during the optimization process. For the removeBackShiftDepth() function:

// Manage feature point starting frame indices: For feature points not starting from the oldest frame,
// decrement their starting frame index by 1.
// Remove feature points from the oldest frame: For feature points starting from the oldest frame,
// remove their record in the oldest frame and check their remaining observation frames.
// If observation frames are less than 2, remove the feature point.
// Depth adjustment: For remaining feature points, calculate their estimated depth in the current frame
// and update their depth values.
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marginalization_R, Eigen::Vector3d marginalization_P, 
                                          Eigen::Matrix3d new_frame_R, Eigen::Vector3d new_frame_P)
{
    // Iterate through all feature points
    for (auto it = features.begin(), it_next = features.begin();
         it != features.end(); it = it_next)
    {
        // Store the next iterator position in advance
        it_next++;
        
        // If the feature point's starting frame is not 0, it means this feature point does not appear in the oldest frame
        if (it->start_frame != 0)
            // Decrement the starting frame index by 1 because the oldest frame has been removed
            // and all frame indices need to move forward
            it->start_frame--;
        else
        {
            // If the feature point's starting frame is 0, it means this feature point appears in the oldest frame
            // Get the normalized image plane coordinates of this feature point in the oldest frame
            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  
            // Remove the record of this feature point in the oldest frame
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            // If the number of observation frames for this feature point is less than 2 after removing the oldest frame,
            // it means this feature point doesn't have enough observations, so remove the feature point
            if (it->feature_per_frame.size() < 2)
            {
                // Remove this feature point from the feature list
                features.erase(it);
                continue;
            }
            else
            {
                // Calculate the 3D coordinates of this feature point in the oldest frame
                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
                // Transform the feature point's 3D coordinates from the oldest frame's camera coordinate system
                // to the world coordinate system
                Eigen::Vector3d world_pts_i = marginalization_R * pts_i + marginalization_P;
                // Transform the feature point's 3D coordinates from the world coordinate system
                // to the current frame's camera coordinate system
                Eigen::Vector3d pts_j = new_frame_R.transpose() * (world_pts_i - new_frame_P);
                // Get the depth value of the feature point in the current frame
                double depth_j = pts_j(2);
                // If the depth value is greater than 0, update the feature point's estimated depth
                if (depth_j > 0)
                    it->estimated_depth = depth_j;
                else
                    // If the depth value is less than or equal to 0, set the feature point's estimated depth
                    // to the initial depth
                    it->estimated_depth = INITIAL_DEPTH;
            }
        }
        // Remove tracking-lost feature after marginalization
        /*
        if (it->endFrame() < WINDOW_SIZE - 1)
        {
            features.erase(it);
        }
        */
    }
}

This function removes features from the oldest frame in the sliding window and handles feature management with depth adjustment when necessary. Specifically, it processes feature points based on the oldest and current frame pose information, performing depth adjustment on the features. For removeBack():

void FeatureManager::removeBack()
{
    // Iterate through all feature points
    for (auto it = features.begin(), it_next = features.begin();
         it != features.end(); it = it_next)
    {
        // Store the next iterator position in advance
        it_next++;
        
        // If the feature point's starting frame is not 0, it means this feature point does not appear in the oldest frame
        if (it->start_frame != 0)
            // Decrement the starting frame index by 1 because the oldest frame has been removed
            // and all frame indices need to move forward
            it->start_frame--;
        else
        {
            // If the feature point's starting frame is 0, it means this feature point appears in the oldest frame
            // Remove the record of this feature point in the oldest frame
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            // If the number of observation frames for this feature point is 0 after removing the oldest frame,
            // it means this feature point has no observation frames left, so remove the feature point
            if (it->feature_per_frame.size() == 0)
                // Remove this feature point from the feature list
                features.erase(it);
        }
    }
}

This function is straightforward - it's the removeBackShiftDepth() function without depth adjustment updates. The removeFront() function deletes the newest frame's features, similar to the previous function:

void FeatureManager::removeFront(int frame_index)
{
    // Iterate through all feature points
    for (auto it = features.begin(), it_next = features.begin(); it != features.end(); it = it_next)
    {
        // Store the next iterator position in advance
        it_next++;
        
        // If the feature point's starting frame is the current frame index, it means this feature point appears in the newest frame
        if (it->start_frame == frame_index)
        {
            // Decrement the starting frame index by 1 because the newest frame has been removed
            // and all frame indices need to move forward
            it->start_frame--;
        }
        else
        {
            // Calculate the position of this feature point in the window
            int j = WINDOW_SIZE - 1 - it->start_frame;
            // If the feature point's end frame is less than the frame before the current newest frame,
            // it means this feature point doesn't need processing, continue to the next iteration
            if (it->endFrame() < frame_index - 1)
                continue;
            // Remove the record of this feature point in the current newest frame
            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
            // If the number of observation frames for this feature point is 0 after removing the newest frame,
            // it means this feature point has no observation frames left, so remove the feature point
            if (it->feature_per_frame.size() == 0)
                // Remove this feature point from the feature list
                features.erase(it);
        }
    }
}