rrs_select_three_points

% Select 3 points for spatial resection and possibly plot

 p_selected = rrs_select_three_points(X,y,Tol)
 
 X    N-struct given scene points, possibly at infinity
 y    N-struct given image directions
 Tol  Tolerance for points to be not at infinity (eg 100)

 X3   3x3 matrix with rows being scene points (X,Y,Z)
 y3   3x3 matrix with rows being image directions

0001 %% Select 3 points for spatial resection and possibly plot
0002 %
0003 % p_selected = rrs_select_three_points(X,y,Tol)
0004 %
0005 % X    N-struct given scene points, possibly at infinity
0006 % y    N-struct given image directions
0007 % Tol  Tolerance for points to be not at infinity (eg 100)
0008 %
0009 % X3   3x3 matrix with rows being scene points (X,Y,Z)
0010 % y3   3x3 matrix with rows being image directions
0011 
0012 
0013 function p_selected = rrs_select_three_points(X,y,Tol)
0014 
0015 global plot_option
0016 global print_option
0017 
0018 N = size(X.h,1);
0019 % number of trials of triplets
0020 K=30;
0021 dmax = 0;
0022 for k=1:K
0023     pe = randperm(N);
0024     % determinant of directions (should be large)
0025     d3 = abs(det([y.h(pe(1),:);y.h(pe(2),:);y.h(pe(3),:);]));
0026     % no point should be far away
0027     X_f_1 = abs(X.h(pe(1),4)) > 1/Tol;
0028     X_f_2 = abs(X.h(pe(2),4)) > 1/Tol;
0029     X_f_3 = abs(X.h(pe(3),4)) > 1/Tol;
0030     if d3 > dmax && X_f_1*X_f_2*X_f_3 == 1
0031         three = pe;
0032         dmax= d3;
0033         pe_selected = pe;
0034         X3 = [X.h(pe(1),1:3)/X.h(pe(1),4);...
0035               X.h(pe(2),1:3)/X.h(pe(2),4);...
0036               X.h(pe(3),1:3)/X.h(pe(3),4)];
0037         y3 = [y.h(pe(1),:);...
0038               y.h(pe(2),:);...
0039               y.h(pe(3),:)];
0040     end
0041 end
0042 if dmax < 0.00001 
0043     display('no adequate triple found')
0044     return
0045 end
0046 
0047 if print_option > 0
0048     display(['Points for approximate values : ',num2str(pe_selected(1:3))])
0049 end
0050 if plot_option > 0
0051     figure
0052     subplot(1,2,1)
0053     hold on
0054     plot3(0,0,0,'.b','MarkerSize',15)
0055     for n=1:3
0056         plot3(X3(n,1),X3(n,2),X3(n,3),'.r','MarkerSize',15)
0057     end
0058     axis equal
0059     subplot(1,2,2)
0060     hold on
0061     plot3(0,0,0,'.b','MarkerSize',15)
0062     for n=1:3
0063         plot3(y3(n,1),y3(n,2),y3(n,3),'.r','MarkerSize',15)
0064         plot3([0,y3(n,1)],[0,y3(n,2)],[0,y3(n,3)],'-r')
0065     end
0066     axis equal
0067 end
0068 
0069 % selected points
0070 p_selected=pe(1:3);
0071 
0072 end
0073