Method ProcessFrame

src/processing/Estimator.cpp:209–366

}

Estimator::EstimationResult Estimator::ProcessFrame(const cv::Mat& image, double timestamp) {
    auto frame_start = std::chrono::high_resolution_clock::now();
    EstimationResult result;
    
    // Create new frame
    m_current_frame = CreateFrame(image, timestamp);
    
    // =========================================================================
    // State Machine: NOT_INITIALIZED -> VISUAL_ONLY -> VIO (without IMU data)
    // =========================================================================
    
    switch (m_tracking_state) {
    case TrackingState::NOT_INITIALIZED: {
        if (m_previous_frame) {
            result.num_tracked = TrackFeatures();
        } else {
            DetectFeatures();
        }
        
        m_frame_window.push_back(m_current_frame);
        
        if (static_cast<int>(m_frame_window.size()) > m_window_size) {
            m_frame_window.erase(m_frame_window.begin());
        }
        
        LOG_DEBUG("  Window size: {}/{}, trying init...", m_frame_window.size(), m_window_size);
        
        if (static_cast<int>(m_frame_window.size()) == m_window_size) {
            bool init_result = TryInitialize();
            
            if (init_result) {
                result.init_success = true;
                m_tracking_state = TrackingState::VISUAL_ONLY;
                
                // Switch to tracking window size (smaller than init window)
                m_window_size = m_tracking_window_size;
                LOG_INFO("Initialization SUCCESS! Switching to tracking window size: {}", m_window_size);
                
                auto first_frame = m_frame_window.front();
                auto last_frame = m_frame_window.back();
                
                first_frame->SetKeyframe(true);
                last_frame->SetKeyframe(true);
                
                m_first_keyframe_time = first_frame->GetTimestamp();
                m_last_keyframe_time = last_frame->GetTimestamp();
                
                m_keyframes.push_back(first_frame);
                m_keyframes.push_back(last_frame);
                m_last_keyframe = last_frame;
                
                m_current_pose = last_frame->GetTwb();
            } else {
                if (m_frame_window.size() >= 2) {
                    auto first_frame = m_frame_window.front();
                    auto last_frame = m_frame_window.back();
                    float parallax = m_initializer->ComputeParallax(first_frame, last_frame);