Method ValidateInitialization

src/processing/Initializer.cpp:886–992

}

bool Initializer::ValidateInitialization(
    const std::vector<Eigen::Vector3f>& bearings1,
    const std::vector<Eigen::Vector3f>& bearings2,
    const std::vector<Eigen::Vector3f>& points3d,
    const Eigen::Matrix3f& R,
    const Eigen::Vector3f& t,
    const std::vector<bool>& inlier_mask,
    float& mean_error
) const {
    // Validation:
    // Check reprojection error in BOTH frames (ref and cur)
    
    int valid_count = 0;
    int total_inliers = 0;
    int skip_zero = 0;
    int skip_ref = 0;
    int skip_cur = 0;
    std::vector<float> errors_pixel;
    float error_sum = 0.0f;
    
    const float reproj_err_thr_sq = m_max_reprojection_error * m_max_reprojection_error;
    
    for (size_t i = 0; i < points3d.size(); ++i) {
        if (!inlier_mask[i]) continue;
        total_inliers++;
        
        // Skip invalid points (zero vector - triangulation failed)
        if (points3d[i].norm() < 1e-6) {
            skip_zero++;
            continue;
        }
        
        const Eigen::Vector3f& pt = points3d[i];
        
        // Check reprojection error in BOTH frames
        
        // 1. Reprojection error in frame1 (reference)
        float error_ref = ComputeReprojectionErrorInFrame(pt, bearings1[i]);
        
        // DEBUG: Show first few points
        if (total_inliers <= 5) {
            Eigen::Vector3f b1 = bearings1[i];
            Eigen::Vector3f pt_dir = pt.normalized();
            float dot = pt_dir.dot(b1);
            LOG_INFO("  [DBG] pt[{}]: pt=({:.3f},{:.3f},{:.3f}), b1=({:.3f},{:.3f},{:.3f}), dot={:.3f}, err_ref={:.2f}px",
                     i, pt.x(), pt.y(), pt.z(), b1.x(), b1.y(), b1.z(), dot, error_ref);
        }
        
        if (error_ref > m_max_reprojection_error) {
            skip_ref++;
            continue;
        }
        
        // 2. Reprojection error in frame2 (current)
        // Transform point to frame2: P_cur = R * P_ref + t
        Eigen::Vector3f point_in_frame2 = R * pt + t;
        float error_cur = ComputeReprojectionErrorInFrame(point_in_frame2, bearings2[i]);