Method TriangulateSinglePoint

src/processing/Initializer.cpp:725–780

}

bool Initializer::TriangulateSinglePoint(
    const Eigen::Vector3f& bearing1,
    const Eigen::Vector3f& bearing2,
    const Eigen::Matrix3f& R,
    const Eigen::Vector3f& t,
    Eigen::Vector3f& point3d
) const {
    // R: rotation from frame1 to frame2 (rot_ref_to_cur)
    // t: translation from frame1 to frame2 (trans_ref_to_cur)
    
    // Transform translation to go from frame2 to frame1
    const Eigen::Vector3f trans_12 = -R.transpose() * t;
    // Transform bearing2 to frame1 coordinates
    const Eigen::Vector3f bearing2_in_frame1 = R.transpose() * bearing2;
    
   
    
    // Build the linear system: A * lambda = b
    // Ray1: p1 = λ1 * bearing1
    // Ray2: p2 = λ2 * bearing2_in_frame1 + trans_12
    
    Eigen::Matrix2f A;
    A(0, 0) = bearing1.dot(bearing1);
    A(1, 0) = bearing1.dot(bearing2_in_frame1);
    A(0, 1) = -A(1, 0);
    A(1, 1) = -bearing2_in_frame1.dot(bearing2_in_frame1);
    
    Eigen::Vector2f b;
    b(0) = bearing1.dot(trans_12);
    b(1) = bearing2_in_frame1.dot(trans_12);
    
    // Check for degenerate matrix
    float det = A(0,0) * A(1,1) - A(0,1) * A(1,0);
    if (std::abs(det) < 1e-10f) {
        return false;
    }
    
    // Solve for lambda
    const Eigen::Vector2f lambda = A.inverse() * b;
    
    // For equirectangular cameras, skip depth positive check
    // (depth_is_positive = false for bearing_vector initializer)
    // Only check for finite values
    if (!std::isfinite(lambda(0)) || !std::isfinite(lambda(1))) {
        return false;
    }
    
    // Compute 3D points on both rays
    const Eigen::Vector3f pt_1 = lambda(0) * bearing1;
    const Eigen::Vector3f pt_2 = lambda(1) * bearing2_in_frame1 + trans_12;
    
    // Return the mid-point (in frame1 coordinates)
    point3d = (pt_1 + pt_2) / 2.0f;
    
    return true;
}

int Initializer::TestPoseCandidate(