function [solutions, values, results_stats] = h3d_evaluate_solutions_hausdorff(p, scan1, scan2, add_R,add_T, solutions, npoints, plausible_threshold) % function [solutions, values] = h3d_evaluate_solutions_hausdorff(p, scan1, scan2, add_R,add_T, solutions, npoints) % % Evaluates the goodness of a list of solutions using Hausdorff distance. % % - solutions is a list of tuples {rotation, translation} % - npoints is how many random points to use from the second scan. % % - add_R and add_T are added to the points of the second scan % Sample npoints from the second scan scan2_important = h3d_scan_sample_points(scan2, npoints); sampled_points = {}; for i=1:size(scan2_important.sampled_points,2) sampled_points{end+1} = scan2_important.sampled_points(1:3,i); end h3d_display_clouds(scan2_important.sampled_points,eye(3),[0;0;0],scan2_important.sampled_points,eye(3),[0;0;0],1) max_obs = -1000; % For each solution for s=1:numel(solutions) R = solutions{s}{1}; T = solutions{s}{2}; % Each point can be one of these: num_out_of_field = 0; num_behind_something = 0; num_inconsistent = 0; num_plausible = 0; % For each sampled point for a=1:numel(sampled_points) p = sampled_points{a}; % Coordinates in the first scan frame of reference % given the estimated R,T pw = R' * ( p - T); pw = add_R * pw + add_T; [visible, rho] = h3d_scan_whats_your_view(scan1, pw); weight = norm(pw); if not(visible) num_out_of_field = num_out_of_field + weight; continue; end if abs(rho - norm(pw)) < plausible_threshold num_plausible = num_plausible + weight; else if rho < norm(pw) num_behind_something = num_behind_something + weight; else num_inconsistent = num_inconsistent + weight; end end end values(s) = num_plausible; results_stats{s} = {num_out_of_field, num_behind_something, num_inconsistent, num_plausible}; if values(s) > 0 max_obs = values(s); fprintf('%5d/%d: Out %5.2f Behind %5.2f Incons %5.2f Plaus %5.2f val %5.2f\n', s, numel(solutions),num_out_of_field, num_behind_something, num_inconsistent, num_plausible, values(s)); end end % sort the hypotheses [Y,I] = sort(values, 'descend'); solutions = {solutions{I}}; values = values(I); results_stats = {results_stats{I}};