Method ProcessIntermediateFrames

src/processing/Estimator.cpp:1186–1359

DEPRECATED: This function was used for interpolation-based initialization.

// DEPRECATED: This function was used for interpolation-based initialization.
void Estimator::ProcessIntermediateFrames() {
    if (m_frame_window.size() < 3) {
        return;  // No intermediate frames
    }
    
    auto first_kf = m_frame_window.front();
    auto last_kf = m_frame_window.back();
    
    Eigen::Matrix4f T_first = first_kf->GetTwb();
    Eigen::Matrix4f T_last = last_kf->GetTwb();
    
    // Extract rotation and translation
    Eigen::Matrix3f R_first = T_first.block<3, 3>(0, 0);
    Eigen::Vector3f t_first = T_first.block<3, 1>(0, 3);
    Eigen::Matrix3f R_last = T_last.block<3, 3>(0, 0);
    Eigen::Vector3f t_last = T_last.block<3, 1>(0, 3);
    
    // Compute relative rotation using Rodrigues
    Eigen::Matrix3f R_rel = R_first.transpose() * R_last;
    Eigen::AngleAxisf aa(R_rel);
    Eigen::Vector3f axis = aa.axis();
    float angle = aa.angle();
    
    int n_frames = static_cast<int>(m_frame_window.size());
    
    LOG_DEBUG("Processing {} intermediate frames to create keyframes...", n_frames - 2);
    
    // Build global feature ID to MapPoint map from first_kf and last_kf (the two frames with triangulated MapPoints)
    std::unordered_map<int, std::shared_ptr<MapPoint>> feature_to_mappoint;
    
    // Collect from first keyframe
    const auto& first_features = first_kf->GetFeatures();
    for (size_t j = 0; j < first_features.size(); ++j) {
        auto mp = first_kf->GetMapPoint(static_cast<int>(j));
        if (mp && !mp->IsBad()) {
            feature_to_mappoint[first_features[j]->GetFeatureId()] = mp;
        }
    }
    
    // Collect from last keyframe (may add new MapPoints or override)
    const auto& last_features = last_kf->GetFeatures();
    for (size_t j = 0; j < last_features.size(); ++j) {
        auto mp = last_kf->GetMapPoint(static_cast<int>(j));
        if (mp && !mp->IsBad()) {
            int feat_id = last_features[j]->GetFeatureId();
            if (feature_to_mappoint.find(feat_id) == feature_to_mappoint.end()) {
                feature_to_mappoint[feat_id] = mp;
            }
        }
    }
    
    LOG_DEBUG("Found {} unique MapPoints from first/last keyframes for observation propagation", 
             feature_to_mappoint.size());
    
    // Process intermediate frames (skip first and last which already have poses)
    for (int i = 1; i < n_frames - 1; ++i) {
        auto& frame = m_frame_window[i];