Method ProcessKeyframeInBackend

src/processing/Estimator.cpp:157–207

}

void Estimator::ProcessKeyframeInBackend(std::shared_ptr<Frame> keyframe) {
    LOG_DEBUG("Backend processing keyframe {}", keyframe->GetFrameId());
    
    // Lock entire BA process
    std::unique_lock<std::shared_mutex> lock(m_map_mutex);
    
    // Find previous keyframe
    std::shared_ptr<Frame> prev_keyframe = nullptr;
    if (m_keyframes.size() >= 2) {
        for (int i = static_cast<int>(m_keyframes.size()) - 2; i >= 0; --i) {
            if (m_keyframes[i]->GetFrameId() < keyframe->GetFrameId()) {
                prev_keyframe = m_keyframes[i];
                break;
            }
        }
    }
    
    // Triangulate new MapPoints
    int new_points = 0;
    if (prev_keyframe) {
        new_points = TriangulateNewMapPoints(prev_keyframe, keyframe);
    }
    
    // Run BA
    if (new_points > 0 && m_keyframes.size() >= 2) {
        Optimizer optimizer;
        optimizer.SetCamera(m_camera, m_boundary_margin);
        
        BAResult ba_result;
        if (m_tracking_state == TrackingState::VIO) {
            ba_result = optimizer.RunLocalBAwithInertial(m_keyframes, m_gravity);
            
            // Update bias
            auto last_kf = m_keyframes.back();
            Eigen::Vector3f new_gyro_bias = last_kf->GetGyroBias();
            Eigen::Vector3f new_accel_bias = last_kf->GetAccelBias();
            UpdatePreintegrationsWithNewBias(new_gyro_bias, new_accel_bias);
        } else {
            ba_result = optimizer.RunLocalBA(m_keyframes);
        }
        
        LOG_DEBUG("Backend BA: {} keyframes, {} inliers, {} outliers, cost {:.2f}->{:.2f}",
                 m_keyframes.size(), ba_result.num_inliers, ba_result.num_outliers,
                 ba_result.initial_cost, ba_result.final_cost);
        
        // Update current pose
        if (!m_keyframes.empty()) {
            m_current_pose = m_keyframes.back()->GetTwb();
        }
    }
}

Estimator::EstimationResult Estimator::ProcessFrame(const cv::Mat& image, double timestamp) {
    auto frame_start = std::chrono::high_resolution_clock::now();