TP-probleme-inverse-3D/TP2/estimation_E_robuste.m
2023-06-25 16:38:01 +02:00

27 lines
649 B
Mathematica

function [E, w1_new, w2_new] = estimation_E_robuste(w1, w2)
nb_min = 8;
nb_paires = size(w1, 1);
mediane = +inf;
S = 5e-6;
while mediane > S
selection = randperm(nb_paires, nb_min);
w1_new = w1(selection, :);
w2_new = w2(selection, :);
E = estimation_E(w1_new, w2_new);
d1 = w2 * E;
d2 = w1 * E';
dist1 = ( d1(:,1) .* w1(:, 1) + d1(:,2) .* w1(:, 2) + d1(:,3) ).^2 ./ ( d1(:,1).^2 + d1(:,2).^2 );
dist2 = ( d2(:,1) .* w2(:, 1) + d2(:,2) .* w2(:, 2) + d2(:,3) ).^2 ./ ( d2(:,1).^2 + d2(:,2).^2 );
mediane = mean([dist1 ; dist2]);
end
end