Home > General-Functions > SUGR > E-Matrix > sugr_triangulate_two_spherical_cameras.m

sugr_triangulate_two_spherical_cameras

PURPOSE ^

% sugr_triangulate_two_spherical_cameras

SYNOPSIS ^

function [X_est, est_sigma_0, est_u, est_v, f] =sugr_triangulate_two_spherical_cameras(b, Rot, u, v, sigma, Tol, max_iter, k, print_option)

DESCRIPTION ^

% sugr_triangulate_two_spherical_cameras

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

 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

 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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Mon 08-Jan-2018 17:21:49 by m2html © 2005