function SOLS = fivepoint_infinitesimal(u,up) %%function SOLS = fivepoint_infinitesimal(u,up) %% %% An very naive implementation to solve the %% infinitesimal motion problem for 5 points %% and a calibrated camera. %% %% This file relies on the two helpers: %% fivepoint_infinitestimal_compute_A.m %% fivepoint_infinitestimal_compute_AA.m %% And will not work without these. %% %% %% This code was written by Henrik Stewenius for the paper: %% %% %% An Efficient Minimal Solution for Infinitesimal Camera Motion %% Henrik Stewenius, Chris Engels and David Nister %% CVPR 2007. %% %% The code is provided as is an we do not gaurantee that it works %% not that it is fit for any particular purpose. You use it at your %% own risk. for i=1:3 for j=1:5 eval( sprintf('u%i%i = u(%i,%i);',i,j,i,j )); eval( sprintf('up%i%i = up(%i,%i);',i,j,i,j )); end end %%This should probably be written % [At1 At2 A1] = compute_AA( u, up) % But i was to lazy fivepoint_infinitestimal_compute_AA for i=1:size( A1,1) for j=1:4 B{i,j} = [zeros(1,12) At1(i,j) At2(i,j) A1(i,j)]; end end ss2 = [[1, 3, 6, 9]' [1, 5, 8, 12]', [2, 5, 9, 12]', [2, 6, 9, 12]', ... [3, 6, 9, 12]'] + 1; M = zeros( 5, 15); for pos = 1:5 ; for i=1:4 for j=1:4 C{i,j} = B{ss2(i,pos),j}; end end M(pos,:) = polydet( C ); end M = inv(M(:,1:5))*M; M(:,1:5) = eye(5); if 0 %% This if-statement is to figure out what does into the code below. %% %MONS = build_MONS(4 ); %MONS = MONS(1:2, find( not(MONS(end,:)))); %fprintf( '%i ', MONS(2,:)) MONS = [4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0 ]; outher_points = MONS(:,1:5); inner_points = MONS(:,6:end); N = []; for p=inner_points p = p+[0;1]; t= ismember( inner_points' , p', 'rows' )' ; if any( t ) disp( t ) ; N= [N; t]; else t= ismember(outher_points' , p', 'rows' )' ; I = find( t ); disp( I ); N = [N; -M(I,[6:end])]; end end else %%This was computed using the alternative if... %% This is the action matrix. N = [-M([2 3 4 5],6:end); 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 1 0]; end [V,D] = eig(N ); V = V./(ones(10,1)*V(end,:)); t0_vec = ones(1,10); t1_vec = V(end-2,:); t2_vec = V(end-1,:); SOLS = zeros(6,10); for i=1:10 t0= t0_vec(i); t1= t1_vec(i); t2= t2_vec(i); %%We compute A from u,up,t fivepoint_infinitestimal_compute_A rsol = null( A ); rsol = rsol(2:4)/rsol(1); SOLS(:,i) =[ rsol ; [t0;t1;t2] ]; end function out = polydet( A ) %%To compute a determinant of a matrix where the %%entries are polynomials. It only works for the %%very specific polynomials used in this code. %syms dummy %A = zeros(4)*dummy; %for i=1:4 % for j=1:4 % A(i,j) = sym(sprintf('A{%i,%i}',i,j)); % end %end out = +quadprod(A{1,1},A{2,2},A{3,3},A{4,4})... -quadprod(A{1,1},A{2,2},A{3,4},A{4,3})... -quadprod(A{1,1},A{3,2},A{2,3},A{4,4})... +quadprod(A{1,1},A{3,2},A{2,4},A{4,3})... +quadprod(A{1,1},A{4,2},A{2,3},A{3,4})... -quadprod(A{1,1},A{4,2},A{2,4},A{3,3})... -quadprod(A{2,1},A{1,2},A{3,3},A{4,4})... +quadprod(A{2,1},A{1,2},A{3,4},A{4,3})... +quadprod(A{2,1},A{3,2},A{1,3},A{4,4})... -quadprod(A{2,1},A{3,2},A{1,4},A{4,3})... -quadprod(A{2,1},A{4,2},A{1,3},A{3,4})... +quadprod(A{2,1},A{4,2},A{1,4},A{3,3})... +quadprod(A{3,1},A{1,2},A{2,3},A{4,4})... -quadprod(A{3,1},A{1,2},A{2,4},A{4,3})... -quadprod(A{3,1},A{2,2},A{1,3},A{4,4})... +quadprod(A{3,1},A{2,2},A{1,4},A{4,3})... +quadprod(A{3,1},A{4,2},A{1,3},A{2,4})... -quadprod(A{3,1},A{4,2},A{1,4},A{2,3})... -quadprod(A{4,1},A{1,2},A{2,3},A{3,4})... +quadprod(A{4,1},A{1,2},A{2,4},A{3,3})... +quadprod(A{4,1},A{2,2},A{1,3},A{3,4})... -quadprod(A{4,1},A{2,2},A{1,4},A{3,3})... -quadprod(A{4,1},A{3,2},A{1,3},A{2,4})... +quadprod(A{4,1},A{3,2},A{1,4},A{2,3}); function out = quadprod( a,b,c,d) %%Multiply a*b*c*d when a,b,c and d are polynomials. out = mult_atom(mult_atom(mult_atom(a,b),c),d); function c = mult_atom( a, b ) %%Multiply polynomial a with polynomial b. %%Both a and b are bivariate and assumed to be represented %%as scalar vectors refering to the monomial order given by MONS below. % % Here is the code how the code for the atom was generated. It will only % work if you have the symbolic toolkit and maple installed. % % % % MONS=[ % % 4 3 2 1 0 3 2 1 0 2 1 0 1 0 0 % % 0 1 2 3 4 0 1 2 3 0 1 2 0 1 0]; % % syms t1 t2 % % a = 0*t1; % % b = 0*t1; % % for i=1:15 % % a = a + sym(sprintf('a(%i)', i))*t1^MONS(1,i)*t2^MONS(2,i); % % b = b + sym(sprintf('b(%i)', i))*t1^MONS(1,i)*t2^MONS(2,i); % % end % % c = maple('expand',a*b); % % mcoeff=inline('maple(''coeff'', maple(''coeff'', t,''t1'',n(1)),''t2'',n(2))','t','n') ; % % for i=1:15 % % fprintf('c(%i) = %s;\n', i,char( mcoeff( c, MONS(:,i)) ) ); % % end c(1) = a(1)*b(15)+a(6)*b(13)+a(10)*b(10)+a(13)*b(6)+a(15)*b(1); c(2) = a(2)*b(15)+a(10)*b(11)+a(11)*b(10)+a(13)*b(7)+a(14)*b(6)+a(15)*b(2)+a(6)*b(14)+a(7)*b(13); c(3) = a(11)*b(11)+a(8)*b(13)+a(13)*b(8)+a(12)*b(10)+a(15)*b(3)+a(10)*b(12)+a(14)*b(7)+a(3)*b(15)+a(7)*b(14); c(4) = a(13)*b(9)+a(14)*b(8)+a(9)*b(13)+a(8)*b(14)+a(15)*b(4)+a(4)*b(15)+a(12)*b(11)+a(11)*b(12); c(5) = a(9)*b(14)+a(5)*b(15)+a(12)*b(12)+a(14)*b(9)+a(15)*b(5); c(6) = a(15)*b(6)+a(6)*b(15)+a(13)*b(10)+a(10)*b(13); c(7) = a(11)*b(13)+a(10)*b(14)+a(13)*b(11)+a(15)*b(7)+a(14)*b(10)+a(7)*b(15); c(8) = a(13)*b(12)+a(8)*b(15)+a(15)*b(8)+a(14)*b(11)+a(12)*b(13)+a(11)*b(14); c(9) = a(9)*b(15)+a(12)*b(14)+a(14)*b(12)+a(15)*b(9); c(10) = a(10)*b(15)+a(15)*b(10)+a(13)*b(13); c(11) = a(13)*b(14)+a(14)*b(13)+a(15)*b(11)+a(11)*b(15); c(12) = a(14)*b(14)+a(12)*b(15)+a(15)*b(12); c(13) = a(15)*b(13)+a(13)*b(15); c(14) = a(14)*b(15)+a(15)*b(14); c(15) = a(15)*b(15);