sugr_triangulate_two_spherical_cameras

% sugr_triangulate_two_spherical_cameras 

 Alg. 21: Optimal triangulation

 [X,sigma0,est_u,est_v, f] = sugr_triangulate_two_spherical_cameras(...
                   b,R,u,v,sigma,Tol, max_iter, k, print_option)

 input:
 b,Rot    = relative orientation
 u,v      = 3x1 directions, spherically normalized
 sigma    = radial error of direction
 Tol      = tolerance for stopping iterations
 max_iter = maximum iterations
 k        = critical value
 print_option = boolean, deciding on printout

 output:
 X       = 4x1 vector, 3d point
 sigma0  = estimated sigma_0
 est_u, est_v = fitted observations
 f            = 0 at infinity
              = 1 finite
              = 2 backward
              = 3 divergent

 Wolfgang Förstner 2014/04/09
 wfoerstn@uni-bonn.de

0001 %% sugr_triangulate_two_spherical_cameras
0002 %
0003 % Alg. 21: Optimal triangulation
0004 %
0005 % [X,sigma0,est_u,est_v, f] = sugr_triangulate_two_spherical_cameras(...
0006 %                   b,R,u,v,sigma,Tol, max_iter, k, print_option)
0007 %
0008 % input:
0009 % b,Rot    = relative orientation
0010 % u,v      = 3x1 directions, spherically normalized
0011 % sigma    = radial error of direction
0012 % Tol      = tolerance for stopping iterations
0013 % max_iter = maximum iterations
0014 % k        = critical value
0015 % print_option = boolean, deciding on printout
0016 %
0017 % output:
0018 % X       = 4x1 vector, 3d point
0019 % sigma0  = estimated sigma_0
0020 % est_u, est_v = fitted observations
0021 % f            = 0 at infinity
0022 %              = 1 finite
0023 %              = 2 backward
0024 %              = 3 divergent
0025 %
0026 % Wolfgang Förstner 2014/04/09
0027 % wfoerstn@uni-bonn.de
0028 
0029 function [X_est, est_sigma_0, est_u, est_v, f] = ...
0030     sugr_triangulate_two_spherical_cameras(...
0031     b, Rot, u, v, sigma, Tol, max_iter, k, print_option)
0032       
0033 % essentiaol matrix
0034 E = calc_S(b)*Rot';
0035 
0036 % critical value for constraint
0037 sigma_c= sigma*sqrt(u'*(E*E')*u+v'*(E'*E)*v);
0038 crit_E = k*sigma_c;
0039 
0040 cgl = u'*E*v;
0041 if print_option > 0
0042     cgl=cgl                                                                %#ok<ASGSL,NOPRT>
0043 end
0044 % check for coplanarity
0045 if abs(cgl) > crit_E
0046     X_est.h=zeros(4,1);
0047     X_est.Crr=zeros(3);
0048     X_est.type=3;
0049     est_sigma_0 = 0;
0050     f=2;
0051     est_u=u;
0052     est_v=v;
0053     return
0054 end
0055 
0056 % initialize
0057 f  = 0;
0058 ua = u;
0059 va = v;
0060 est_sigma_0=0;
0061 
0062 % iterate
0063 for nu = 1:max_iter
0064     g = ua'*E*va;               % constraint
0065     if print_option > 0
0066         nu=nu                                                              %#ok<FXSET,ASGSL,NOPRT>
0067         g=g                                                                %#ok<ASGSL,NOPRT>
0068     end
0069     % check for convergence
0070     if abs(g) < Tol*sigma
0071         break
0072     end
0073     J1 = null(ua');              % reducing Jacobians
0074     J2 = null(va');
0075     l  = [J1'*u;J2'*v];          % reduced observations
0076     B  = [J1'*E*va;J2'*E'*ua];   % Jacobian of constraint
0077     cg = -cgl;                   % residual constraint
0078     n = B'*B;
0079     Deltal = l + cg/n*B;         % corrections to reduced observations
0080     ua = ua+J1*Deltal(1:2);      % updated approximate values
0081     va = va+J2*Deltal(3:4);
0082     ua = ua/norm(ua);
0083     va = va/norm(va);
0084     est_sigma_0 = abs(cgl)/sigma_c; %/sqrt(n);  % estimated sigma_0
0085 end
0086 % determine homgeneous coordinates
0087 est_u = ua;                     % final estimates
0088 est_v = va;
0089 est_w = Rot'*est_v;               % v in left system
0090 if print_option > 0
0091     uvw= [est_u', est_v', est_w']                                          %#ok<NASGU,NOPRT>
0092 end
0093 m = calc_S(calc_S(b)*est_u)*b;
0094 m = m /norm(m);                 % auxiliary vector
0095 D = det([b, m, calc_S(est_u)*est_w]);
0096 rs = m' * [est_w, est_u];
0097 X = [rs(1)* est_u; D];
0098 %  Xe   = X'
0099 %  uvw  = [est_u,est_v,est_w]
0100 %  mDrs = [m',D,rs]
0101 X_est = sugr_cov_matr_X(X,sigma,est_u,est_w,rs);
0102 
0103 % check for infinity
0104 
0105 
0106 % critical value for Determinant
0107 sigma_D = 2*sigma;         % conservative approximation
0108 crit_D  = k*sigma_D;
0109 if abs(D) < eps
0110     if print_option > 0
0111         disp('point at infinity')
0112     end    
0113     return
0114 end
0115 d=rs/D;
0116 % check for backward direction and infinity
0117 if sign(d(1)) < 0 || sign(d(2)) < 0
0118     if abs(D) < crit_D
0119         % point at infinity
0120         if print_option > 0
0121             disp('point at infinity')
0122         end
0123         
0124         return
0125     else
0126         % rays divergent
0127         f=1;
0128         X_est.h=zeros(4,1);
0129         X_est.Crr=zeros(3);
0130         X_est.type=3;
0131         if print_option > 0
0132             disp('backward point')
0133         end
0134         %dbstop
0135         return
0136     end
0137 else
0138     if print_option > 0
0139         disp('signs ok')
0140     end
0141 end
0142 
0143 
0144