Method ComputeEssentialMatrix

src/processing/Initializer.cpp:455–618

}

bool Initializer::ComputeEssentialMatrix(
    const std::vector<Eigen::Vector3f>& bearings1,
    const std::vector<Eigen::Vector3f>& bearings2,
    Eigen::Matrix3f& E,
    std::vector<bool>& inlier_mask
) const {
    if (bearings1.size() != bearings2.size() || bearings1.size() < 5) {
        return false;
    }
    
    const size_t num_points = bearings1.size();
    inlier_mask.resize(num_points, false);
    
    // RANSAC parameters
    const int min_samples = 8;  // Use 8-point algorithm (more stable than 5-point)
    int best_inliers = 0;
    Eigen::Matrix3f best_E = Eigen::Matrix3f::Identity();
    std::vector<bool> best_mask(num_points, false);
    
    // Random number generator
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dis(0, num_points - 1);
    
    // RANSAC loop
    for (int iter = 0; iter < m_ransac_iterations; ++iter) {
        // 1. Randomly sample 5 points
        std::vector<int> sample_indices;
        sample_indices.reserve(min_samples);
        
        while (sample_indices.size() < static_cast<size_t>(min_samples)) {
            int idx = dis(gen);
            // Check for duplicate
            if (std::find(sample_indices.begin(), sample_indices.end(), idx) == sample_indices.end()) {
                sample_indices.push_back(idx);
            }
        }
        
        // 2. Compute Essential matrix from 8 points using 8-point algorithm
        std::vector<Eigen::Vector3f> sample_bearings1, sample_bearings2;
        for (int idx : sample_indices) {
            sample_bearings1.push_back(bearings1[idx]);
            sample_bearings2.push_back(bearings2[idx]);
        }
        
        // 8-point algorithm
        Eigen::MatrixXf A(min_samples, 9);  // Use 8 samples
        for (size_t i = 0; i < sample_bearings1.size(); ++i) {
            const auto& b1 = sample_bearings1[i];
            const auto& b2 = sample_bearings2[i];
            
            // Essential matrix constraint: b2^T * E * b1 = 0
            A(i, 0) = b2.x() * b1.x();
            A(i, 1) = b2.x() * b1.y();
            A(i, 2) = b2.x() * b1.z();
            A(i, 3) = b2.y() * b1.x();
            A(i, 4) = b2.y() * b1.y();
            A(i, 5) = b2.y() * b1.z();