function triangulatedPoints = triangulatePoints(P1, P2, imagePoints1, imagePoints2)
% Perform triangulation
homogeneousPoints1 = [imagePoints1, ones(size(imagePoints1, 1), 1)];
homogeneousPoints2 = [imagePoints2, ones(size(imagePoints2, 1), 1)];
% Triangulation
points3D = zeros(size(homogeneousPoints1, 1), 3);
for i = 1:size(homogeneousPoints1, 1)
A = [
homogeneousPoints1(i, 1) * P1(3, :) - P1(1, :);
homogeneousPoints1(i, 2) * P1(3, :) - P1(2, :);
homogeneousPoints2(i, 1) * P2(3, :) - P2(1, :);
homogeneousPoints2(i, 2) * P2(3, :) - P2(2, :)
];
[~, ~, V] = svd(A);
triangulatedPoint = V(:, end)';
triangulatedPoint = triangulatedPoint / triangulatedPoint(4);
points3D(i, :) = triangulatedPoint(1:3);
end
% Return the triangulated 3D points
triangulatedPoints = points3D;
end