Method SelectFeaturesForInit

src/processing/Initializer.cpp:348–430

}

std::vector<std::shared_ptr<Feature>> Initializer::SelectFeaturesForInit(
    const std::vector<std::shared_ptr<Frame>>& frames
) const {
    std::vector<std::shared_ptr<Feature>> selected_features;
    
    if (frames.empty()) {
        return selected_features;
    }
    
    // Get features from the last frame (they have all the observation history)
    auto last_frame = frames.back();
    const auto& all_features = last_frame->GetFeatures();
    
    // Filter by observation count
    std::vector<std::shared_ptr<Feature>> candidates;
    for (const auto& feat : all_features) {
        int obs_count = feat->GetObservationCount();
        if (obs_count >= m_min_observations) {
            candidates.push_back(feat);
        }
    }
    
    if (candidates.size() < static_cast<size_t>(m_min_features)) {
        return selected_features;
    }
    
    // Grid-based sampling for uniform distribution
    const int grid_cols = m_init_grid_cols;
    const int grid_rows = m_init_grid_rows;
    const int total_grids = grid_cols * grid_rows;
    
    // Get camera dimensions from config
    const int img_width = m_camera_width;
    const int img_height = m_camera_height;
    
    const float cell_width = static_cast<float>(img_width) / grid_cols;
    const float cell_height = static_cast<float>(img_height) / grid_rows;
    
    // Assign candidates to grid cells
    std::vector<std::vector<std::shared_ptr<Feature>>> grid(total_grids);
    
    for (const auto& feat : candidates) {
        cv::Point2f pt = feat->GetPixelCoord();
        int col = static_cast<int>(pt.x / cell_width);
        int row = static_cast<int>(pt.y / cell_height);
        
        // Clamp to valid range
        col = std::max(0, std::min(col, grid_cols - 1));
        row = std::max(0, std::min(row, grid_rows - 1));
        
        int grid_idx = row * grid_cols + col;
        grid[grid_idx].push_back(feat);
    }
    
    // Count non-empty cells
    int non_empty_cells = 0;
    for (const auto& cell : grid) {
        if (!cell.empty()) {