function [Rs, values] = h3d_guess_rotations(p, ht1, ht2) % [Rs, values] = h3d_guess_rotations(p, ht1, ht2) % % Rs is a list of rotation matrices hyp_rot = {}; hyp_values = []; nhyp = 1; fprintf('Upsampling HT1...\n'); ht1u = h3d_upsample(ht1, p.upsample); fprintf('Upsampling HT2...\n'); ht2u = h3d_upsample(ht2, p.upsample); fprintf('Looking for peaks in HT1...\n'); p1u = h3d_find_hs_peak(ht1u); fprintf('Looking for peaks in HT2...\n'); p2u = h3d_find_hs_peak(ht2u); if p.debug_compensation_show f = figure end for peak1=1:min(p.guess_rot_max_peaks1, size(p1u,2)) for peak2=1:min(p.guess_rot_max_peaks2, size(p2u,2)) m1 = p1u(2:4,peak1); m2 = p2u(2:4,peak2); fprintf('------ matching peak %d -> peak %d -- \n ', peak1, peak2); % take care of the special cases % if they are coincident if m1'*m2 > 0.9999 % identity r1 = eye(3); else % if they are opposite if m1'*m2 < -0.9999 % 180 deg rotation a = h3d_get_one_perpendicular(m1); r1 = rot_axis_angle(a, pi); else % general case r1 = rot_axis_angle(cross(m1,m2), angle_between_vectors(m1,m2)); end end if det(p.debug_true_R) > 0 fprintf('Error in correspondence between m1 and m2: %f deg \n ', rad2deg(angle_between_vectors(p.debug_true_R*m1,m2))); fprintf('After aligning m1 to m2, max error is %f deg.\n', h3d_rotation_error(p.debug_true_R, r1)); end % Now we do the first 360 deg global correlation max_correction = pi; [thetas, global_values] = h3d_compensate_one_rotation(p, ht1u, ht2u, m1, r1, max_correction, 0); % We look for the first p.guess_rot_first_corr_num_hyp hypotheses for a=1:numel(thetas) if a > p.guess_rot_first_corr_num_hyp break end fprintf('Global search for r2: axis: %s hyp #%d: theta = %fdeg\n', display_v3(m1), a, rad2deg(thetas(a))) r2 = rot_axis_angle(r1*m1, thetas(a)); Rest = r2 * r1; if det(p.debug_true_R) > 0 fprintf('After first compensation, max error is %f deg.\n', h3d_rotation_error(p.debug_true_R, Rest)); end % Direction along which to do the other compensations axes = {}; axes{1} = h3d_get_one_perpendicular(m1); axes{2} = cross(m1,axes{1}); axes{3} = m1; for i=(numel(axes)+1):p.guess_rot_num_compensations axes{i} = rot_axis_angle(axes{i-2},deg2rad(30)) * normalize(cross(axes{i-1},axes{i-2})); end % do the other compensations for i=1:p.guess_rot_num_compensations axis = axes{i}; max_correction = deg2rad(p.guess_rot_max_correction_deg); spice = i; [lthetas, lvalues] = h3d_compensate_one_rotation(p, ht1u, ht2u, axis, Rest, max_correction, spice); if numel(lthetas) == 0 fprintf('No maximum found: unstable?\n'); else r_i = rot_axis_angle(Rest*axis, lthetas(1)); Rest = r_i * Rest; fprintf(' Axis: %s quality = %1.3f comp = %3.1f deg \n', display_v3(axis), lvalues(1), rad2deg(lthetas(1))); if det(p.debug_true_R) > 0 fprintf(' after compensating %f deg, max error is %f deg.\n', rad2deg(lthetas(1)),... h3d_rotation_error(p.debug_true_R, Rest)); end end end hyp_rot{nhyp} = Rest; hyp_values(nhyp) = global_values(a); nhyp = nhyp + 1; end if false && p.debug_compensation_show subplot(3,1,1); imagesc(cyl1); subplot(3,1,2); imagesc(cyl2); subplot(3,1,3); imagesc(cyl1-cyl2); title('final value'); figure pause(0.01) end % pause end end % Merge the hypotheses which are closer than p.guess_rot_merge_hyp_threshold_deg Rs = {}; values = []; [Y,I] = sort(hyp_values, 'descend'); for i=1:numel(I) R = hyp_rot{I(i)}; if not(is_it_closer_than(Rs,R, p.guess_rot_merge_hyp_threshold_deg)) values(numel(values)+1) = hyp_values(I(i)); Rs{numel(Rs)+1} = R; else fprintf('Merging one hypothesis.\n'); end end %%% if det(p.debug_true_R) > 0 fprintf('Summary:\n') for i=1:numel(values) fprintf(' value: %f error = %f\n', values(i), h3d_rotation_error(p.debug_true_R, Rs{i})); end end function res = is_it_closer_than(Rs, R, threshold_deg) res = false; for i=1:numel(Rs) [v,theta] = rot_matrix_to_axis_angle(Rs{i}*R'); if rad2deg(abs(theta)) < threshold_deg res = true; return; end end