2. matlab程序
(An Efficient Solution to the Five-Point Relative Pose Problem)
[R_true, ~] = qr(randn(3));
t_true = randn(3,1);
t_true = t_true / norm(t_true);
tx = [0 -t_true(3) t_true(2);
t_true(3) 0 -t_true(1);
-t_true(2) t_true(1) 0];
K = eye(3);
E_true = K' * tx * R_true * K;
disp('ground truth t:');
disp(t_true');
disp('ground truth R:');
disp(R_true);
P = randn(5, 3);
P1 = (eye(3) * P')';
P2 = (R_true * P' + t_true)';
q = [P1(:,1).*P2(:,1) P1(:,2).*P2(:,1) P1(:,3).*P2(:,1) ...
P1(:,1).*P2(:,2) P1(:,2).*P2(:,2) P1(:,3).*P2(:,2) ...
P1(:,1).*P2(:,3) P1(:,2).*P2(:,3) P1(:,3).*P2(:,3)];
[~,~,V] = svd(q);
% in x, y, z, 1 order
E_poly = V(:,6:9);
% 11, 12, 13, 21, 22, 23, 31, 32, 33
detE = o2(o1(E_poly(2,:), E_poly(6,:)) - o1(E_poly(3,:), E_poly(5,:)), E_poly(7,:)) + ...
o2(o1(E_poly(3,:), E_poly(4,:)) - o1(E_poly(1,:), E_poly(6,:)), E_poly(8,:)) + ...
o2(o1(E_poly(1,:), E_poly(5,:)) - o1(E_poly(2,:), E_poly(4,