diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..2b5ae3b --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "visionary"] + path = visionary + url = git@github.com:CarlToft/visionary.git diff --git a/visionary b/visionary new file mode 160000 index 0000000..93b656a --- /dev/null +++ b/visionary @@ -0,0 +1 @@ +Subproject commit 93b656a577b5b589546b65f0fb04f0b54b2723e1 diff --git a/visionary/@imagedata/addconics.m b/visionary/@imagedata/addconics.m deleted file mode 100644 index 5d12125..0000000 --- a/visionary/@imagedata/addconics.m +++ /dev/null @@ -1,16 +0,0 @@ -function i=addconics(i,conics,covs) -%IMAGEDATA/ADDCONICS i=addconics(i,conics,covs) adds dual conics -% and covariances covs - -i.conics = [i.conics,conics]; -if nargin>=3, - if iscell(covs) - i.coniccov = [i.coniccov,covs]; - else - i.coniccov = [i.coniccov,{covs}]; - end -end - - - - diff --git a/visionary/@imagedata/addlines.m b/visionary/@imagedata/addlines.m deleted file mode 100644 index 20f17ec..0000000 --- a/visionary/@imagedata/addlines.m +++ /dev/null @@ -1,19 +0,0 @@ -function i=addlines(i,lines,covs) -%IMAGEDATA/ADDLINES i=addlines(i,lines,covs) adds homog. lines -% and covariances covs - -if size(i.lines,2)==0, - i.lines = lines; -else - i.lines = [i.lines,lines]; -end - -if nargin>=3, - if iscell(covs) - i.linecov = [i.linecov,covs]; - else - i.linecov = [i.linecov,{covs}]; - end -end - - diff --git a/visionary/@imagedata/addpoints.m b/visionary/@imagedata/addpoints.m deleted file mode 100644 index 4a42f84..0000000 --- a/visionary/@imagedata/addpoints.m +++ /dev/null @@ -1,18 +0,0 @@ -function i=addpoints(i,points,covs) -%IMAGEDATA/ADDPOINTS i=addpoints(i,points,cov) adds homog. or cart. points -% and covariances covs - -if size(points,1) == 2, - i.points = [i.points,[points;ones(1,size(points,2))]]; -else - i.points = [i.points,points]; -end - -if nargin>=3; - if iscell(covs), - i.pointcov=[i.pointcov,covs]; - else - i.pointcov=[i.pointcov,{covs}]; - end -end - diff --git a/visionary/@imagedata/changecsystem.m b/visionary/@imagedata/changecsystem.m deleted file mode 100644 index 0180df6..0000000 --- a/visionary/@imagedata/changecsystem.m +++ /dev/null @@ -1,20 +0,0 @@ -function i=changecsystem(i,T); -% IMAGEDATA/changecsystem i=changecsystem(i,T) changes coordinate system -% T is a 3x3 projective transformation - -if ~isempty(i.points) - i.points = T * i.points; -end -if ~isempty(i.lines) - if size(i.lines,1)==3, - i.lines = inv(T)' * i.lines; - else - i.lines = blkdiag(T,T) * i.lines; - end -end - -for q=1:size(i.conics,2); - m=v2m(i.conics(:,q)); - mnew = T*m*T'; - i.conics(:,q)=m2v(mnew); -end diff --git a/visionary/@imagedata/clearconics.m b/visionary/@imagedata/clearconics.m deleted file mode 100644 index 1ae23c0..0000000 --- a/visionary/@imagedata/clearconics.m +++ /dev/null @@ -1,7 +0,0 @@ -function m=clearconics(m) -%IMAGEDATA/CLEARCONICS m=clearconics(m) clears all conics - -m.conics = zeros(6,0); -m.coniccov={}; - - diff --git a/visionary/@imagedata/clearimage.m b/visionary/@imagedata/clearimage.m deleted file mode 100644 index 571e66b..0000000 --- a/visionary/@imagedata/clearimage.m +++ /dev/null @@ -1,6 +0,0 @@ -function i=clearimage(i) -%IMAGEDATA/CLEARIMAGE clearimage(i) returns imagedata i with cleared image - -i.im=[]; -i.filename = []; - diff --git a/visionary/@imagedata/clearlines.m b/visionary/@imagedata/clearlines.m deleted file mode 100644 index c596a0e..0000000 --- a/visionary/@imagedata/clearlines.m +++ /dev/null @@ -1,6 +0,0 @@ -function m=clearlines(m) -%IMAGEDATA/CLEARLINES m=clearlines(m) clear all lines - -m.lines = zeros(3,0); -m.linecov={}; - diff --git a/visionary/@imagedata/clearpoints.m b/visionary/@imagedata/clearpoints.m deleted file mode 100644 index da822a9..0000000 --- a/visionary/@imagedata/clearpoints.m +++ /dev/null @@ -1,7 +0,0 @@ -function m=clearpoints(m) -%IMAGEDATA/CLEARPOINTS m=clearpoints(m) clears all points - -m.points = zeros(3,0); -m.pointcov={}; - - diff --git a/visionary/@imagedata/display.m b/visionary/@imagedata/display.m deleted file mode 100644 index 782139e..0000000 --- a/visionary/@imagedata/display.m +++ /dev/null @@ -1,48 +0,0 @@ -function display(i) -%IMAGEDATA/DISPLAY displays imagedata object - -disp(' '); -disp([inputname(1),' = ']); -disp(' '); - -disp(' Imagedata:'); - -if ~isempty(i.filename), - disp([' Image file: ',i.filename]); -end -if ~isempty(i.im) - disp([' Size of image: ',num2str(size(i.im,1)),'x',num2str(size(i.im,2))]); -end - -np=size(i.points,2); -nl=size(i.lines,2); -nc=size(i.conics,2); -if np==0 & nl==0 & nc==0, - disp(' No features'); -else - if np>0, - if np==1, - disp([' 1 point']); - else - disp([' ',num2str(np),' points']); - end - end - if nl>0, - if nl==1, - disp([' 1 line']); - else - disp([' ',num2str(nl),' lines']); - end - end - if nc>0, - if nc==1, - disp([' 1 conic']); - else - disp([' ',num2str(nc),' conics']); - end - end -end - -disp(' '); - - diff --git a/visionary/@imagedata/getconics.m b/visionary/@imagedata/getconics.m deleted file mode 100644 index f8ee6c7..0000000 --- a/visionary/@imagedata/getconics.m +++ /dev/null @@ -1,19 +0,0 @@ -function [c,cc]=getconics(i,index) -%IMAGEDATA/GETCONICS [c,cc]=getconics(i,index) returns i's conics with index -% in dual coordinates and covariances cc - -if nargin==1, - c=i.conics; - cc=i.coniccov; -else - c=i.conics(:,index); - if nargout==2, - cc=i.coniccov(index); - if size(cc,2)==1; - cc=cc{1}; - end - end -end - - - diff --git a/visionary/@imagedata/getfilename.m b/visionary/@imagedata/getfilename.m deleted file mode 100644 index 5fbac63..0000000 --- a/visionary/@imagedata/getfilename.m +++ /dev/null @@ -1,4 +0,0 @@ -function s=getfilename(i) -%IMAGEDATA/GETFILENAME return filename of image in i - -s=i.filename; diff --git a/visionary/@imagedata/gethomogeneouslines.m b/visionary/@imagedata/gethomogeneouslines.m deleted file mode 100644 index 339f09f..0000000 --- a/visionary/@imagedata/gethomogeneouslines.m +++ /dev/null @@ -1,30 +0,0 @@ -function [l,lc]=gethomogeneouslines(i,index) -%IMAGEDATA/GETHOMOGENEOUSLINES [l,lc]=gethomogeneouslines(i,index) returns i's lines l with index -% in homog. coordinates and covariances lc - -if nargin==1, - l=i.lines; - lc=i.linecov; -else - l=i.lines(:,index); - if nargout==2, - if isempty(i.linecov), - lc=[1 0 0;0 1 0]; - else - lc=i.linecov(index); - if size(lc,2)==1; - lc=lc{1}; - end - end - end -end - -if size(l,1)==6, - ltmp=l; - l=zeros(3,size(ltmp,2)); - for ii=1:size(ltmp,2); - l(:,ii)=cross(ltmp(1:3,ii),ltmp(4:6,ii)); - end -end - - diff --git a/visionary/@imagedata/getimage.m b/visionary/@imagedata/getimage.m deleted file mode 100644 index e7a2392..0000000 --- a/visionary/@imagedata/getimage.m +++ /dev/null @@ -1,15 +0,0 @@ -function im=getimage(i) -%IMAGEDATA/GETIMAGE im=getimage(i) returns i's image matrix. - -if ~isempty(i.filename) & isempty(i.im), - if length(i.filename)>4 & strcmp(i.filename(end-3:end),'.pgm'), - im=readpgm(i.filename); - else - im = imread(i.filename); - end -else - im=i.im; -end - - - diff --git a/visionary/@imagedata/getlines.m b/visionary/@imagedata/getlines.m deleted file mode 100644 index 79f5204..0000000 --- a/visionary/@imagedata/getlines.m +++ /dev/null @@ -1,23 +0,0 @@ -function [l,lc]=getlines(i,index) -%IMAGEDATA/GETLINES [l,lc]=getlines(i,index) returns i's lines l with index -% in stored format and covariances lc - -if nargin==1, - l=i.lines; - lc=i.linecov; -else - l=i.lines(:,index); - if nargout==2, - if isempty(i.linecov), - lc=[1 0 0;0 1 0]; - else - lc=i.linecov(index); - if size(lc,2)==1; - lc=lc{1}; - end - end - end -end - - - diff --git a/visionary/@imagedata/getnormtrans.m b/visionary/@imagedata/getnormtrans.m deleted file mode 100644 index 5686f54..0000000 --- a/visionary/@imagedata/getnormtrans.m +++ /dev/null @@ -1,82 +0,0 @@ -function t=getnormtrans(i,option); -% IMAGEDATA/GETNORMTRANS t=getnormtrans(i,option) get normalising image transformation -% INPUT: -% i - imagedata object -% option: -% 'affine' - the transformation is restricted to be affine -% OUTPUT: -% t - 3x3 transformation matrix -% t is chosen such that all coordinates are with [-1,1] in the image. -% If not the image size is available, t is chosen such that the three -% coordinate vectors [x1,x2,...,xn],[y1,y2,...,yn] and [1,1,...,1] -% are orthogonal. -% The transformation should be used to improve numerical conditioning - -if nargin<=1, - option = ''; -end - -if ~isempty(i.im), - - imsize=size(i.im); - t =[2/imsize(2) 0 -1 - 0 2/imsize(1) -1 - 0 0 1]; - -else - %try to use points and conics - if size(i.conics)>0; - tmp=[i.points,i.conics(4:6,:)]; - else - tmp=i.points; - end - if size(tmp,2)>2, - index=find(isfinite(tmp(1,:))); - tmp=tmp(:,index); - end - if size(tmp,2)>2, - % use points and conics to normalise - if isempty(strmatch('affine',option)) - [u,s,v]=svd(pflat(tmp)); - t=inv(u*s(:,1:3)); - else - tmp=pflat(tmp); - c0=mean(tmp(1:2,:)')'; - mx=max(abs(tmp(1:2,:)-c0*ones(1,size(tmp,2)))'); - d0=diag(1./mx)*sqrt(2); - t=[[d0,-d0*c0];0,0,1]; - end - else - tmp=gethomogeneouslines(i); - if size(tmp,2)>2, - index=find(isfinite(tmp(1,:))); - tmp=tmp(:,index); - end - if size(tmp,2)>2, - % use lines to normalise - [u,s,v]=svd(pflat(tmp)); - if isempty(strmatch('affine',option)) - t=u*s(:,1:3); - else - t=s(:,1:3); - end - else - - %try again with points and do translation - if size(i.points,2)==1, - t=eye(3); - t(1:2,3)=-i.points(1:2); - else - %nothing to normalise with... - t = eye(3); - end - - - end - end -end - - - - - diff --git a/visionary/@imagedata/getpoints.m b/visionary/@imagedata/getpoints.m deleted file mode 100644 index db9fa0f..0000000 --- a/visionary/@imagedata/getpoints.m +++ /dev/null @@ -1,31 +0,0 @@ -function [p,pc]=getpoints(i,index) -%IMAGEDATA/GETPOINTS [p,pc]=getpoints(i,index) returns i's points p with index -% in homog. coordinates. and covariances pc - -if nargin==1, - p=i.points; - pc=i.pointcov; -else - p=i.points(:,index); - if nargout==2, - if isempty(i.pointcov), - %covariances are not set - if size(index,2)==1, - pc = [1 0 0;0 1 0]; - else - pc = cell(1,size(index,2)); - for t=1:size(index,2); - pc{i} = [1 0 0;0 1 0]; - end - end - else %covariances are set - pc=i.pointcov(index); - if size(pc,2)==1; - pc=pc{1}; - end - end - end -end - - - diff --git a/visionary/@imagedata/imagedata.m b/visionary/@imagedata/imagedata.m deleted file mode 100644 index ce757a4..0000000 --- a/visionary/@imagedata/imagedata.m +++ /dev/null @@ -1,79 +0,0 @@ -function i = imagedata(im,points,lines,conics) -%IMAGEDATA class constructor -% i=IMAGEDATA(im,points,lines,conics) creates an IMAGEDATA object where -% im: Filename of image or image matrix -% points : 3xn (or 2xn) matrix where columns are homogeneous -% (or cartesian) point coordinates -% lines : 3xm matrix where columns are homogeneous line coordinates -% (or 6xm matrix where columns are the two homogeneous end points -% of the line) -% conics : 6xp matrix where columns are dual conic coordinates -% -% All of the input arguments are optional -% -% Methods: -% -% addconics -% addlines -% addpoints -% changecsystem -% clearconics -% clearimage -% clearlines -% clearpoints -% display -% getconics -% getfilename -% getimage -% getlines -% getnormtrans -% getpoints -% imagedata -% loadimage -% plot -% plus -% size - - -if nargin == 1 & isa(im,'imagedata'), - i=im; -else - i.filename = []; - i.im = []; - i.pointcov={}; - i.linecov={}; - i.coniccov={}; - - if nargin >= 1 & ~isempty(im), - if ischar(im), - i.filename = im; - else - i.im = im; - end - end - if nargin >= 2 & ~isempty(points), - if size(points,1)==2. - i.points=[points;ones(1,size(points,2))]; - else - i.points = points; - end - else - i.points = zeros(3,0); - end - if nargin >= 3 & ~isempty(lines), - i.lines = lines; - else - i.lines = zeros(3,0); - end - if nargin >= 4 & ~isempty(conics), - i.conics = conics; - else - i.conics = zeros(6,0); - end - - i = class(i,'imagedata'); - -end - - - diff --git a/visionary/@imagedata/loadimage.m b/visionary/@imagedata/loadimage.m deleted file mode 100644 index a974002..0000000 --- a/visionary/@imagedata/loadimage.m +++ /dev/null @@ -1,10 +0,0 @@ -function i=loadimage(i) -%IMAGEDATA/LOADIMAGE loads and stores the image permanently - -if ~isempty(i.filename) - if length(i.filename)>4 & strcmp(i.filename(end-3:end),'.pgm'), - i.im=readpgm(i.filename); - else - i.im = imread(i.filename); - end -end diff --git a/visionary/@imagedata/plot.m b/visionary/@imagedata/plot.m deleted file mode 100644 index 508a342..0000000 --- a/visionary/@imagedata/plot.m +++ /dev/null @@ -1,79 +0,0 @@ -function plot(i,p,l,c) -%IMAGEDATA/PLOT Plot imagedata. -% PLOT(i,p,l,c) plot imagedata i, with styles p,l,c is for -% points, lines & conics, respectively. - -numberpoints=0; -if nargin>1 & strncmp(p,'numbered',8), - numberpoints=1; - p=p(9:end); -end -if nargin<=1 | isempty(p), - p = '*'; -end -if nargin<=2 | isempty(l), - l = '-'; -end -if nargin<=3 | isempty(c), - c = '.'; -end - -holdmode=ishold; - -if ~isempty(i.im), - %clf; - if issparse(i.im) - axis([0.5,size(i.im,2)+0.5,0.5,size(i.im,1)+0.5]); - axis('ij'); - else - image(i.im); - end - noimage=0; -elseif ~isempty(i.filename), - %clf; - if length(i.filename)>4 & strcmp(i.filename(end-3:end),'.pgm'), - image(readpgm(i.filename)); - else - image(imread(i.filename)); - end - noimage=0; -else - if holdmode == 0, - %clf; - axis('ij'); - end - noimage=1; -end -hold on; -if noimage == 0, - colormap(gray(256)); -end -ax = axis; - -if size(i.points,2)>0, - plot( (i.points(1,:)./i.points(3,:))',... - (i.points(2,:)./i.points(3,:))', p); - if numberpoints, - pts=[i.points(1,:)./i.points(3,:);... - i.points(2,:)./i.points(3,:)]; - ax=axis;dev=0.003*ax(2); - for pp=1:size(pts,2); - h=text(double(pts(1,pp)-dev),double(pts(2,pp)-dev),num2str(pp)); - set(h,'Fontsize',11);set(h,'Color','g'); - end - end - -end - -plotconic(i.conics,c); -plotline(i.lines,l); - -if holdmode == 0 - hold off; -end - - - - - - diff --git a/visionary/@imagedata/plus.m b/visionary/@imagedata/plus.m deleted file mode 100644 index 5e572f9..0000000 --- a/visionary/@imagedata/plus.m +++ /dev/null @@ -1,23 +0,0 @@ -function i=plus(i1,i2) -%IMAGEDATA/PLUS i=plus(i1,i2) adds i2's features to i1 - -i = imagedata(i1); -i2 = imagedata(i2); - -[p,pc]=getpoints(i2); -i.points = [i.points,p]; -i.pointcov = [i.pointcov,pc]; - -[p,pc]=getlines(i2); -if isempty(i.lines), - i.lines = [p]; - i.linecov = [pc]; -else - i.lines = [i.lines,p]; - i.linecov = [i.linecov,pc]; -end - -[p,pc]=getconics(i2); -i.conics = [i.conics,p]; -i.coniccov = [i.coniccov,pc]; - diff --git a/visionary/@imagedata/size.m b/visionary/@imagedata/size.m deleted file mode 100644 index 1a2fafd..0000000 --- a/visionary/@imagedata/size.m +++ /dev/null @@ -1,8 +0,0 @@ -function sz = size(i,index) -%IMAGEDATA/SIZE sz = size(i,index) returns nbr of pts, lines, conics - -sz=[size(i.points,2),size(i.lines,2),size(i.conics,2)]; - -if nargin==2, - sz = sz(index); -end diff --git a/visionary/@imconic/calcres.m b/visionary/@imconic/calcres.m deleted file mode 100644 index 06ed509..0000000 --- a/visionary/@imconic/calcres.m +++ /dev/null @@ -1,17 +0,0 @@ -function [r]=calcres(im,proj,ob); -% IMCONIC/CALCRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = m2v(P*U*P'); - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); - -r = L*(um-un); \ No newline at end of file diff --git a/visionary/@imconic/display.m b/visionary/@imconic/display.m deleted file mode 100644 index 5786e38..0000000 --- a/visionary/@imconic/display.m +++ /dev/null @@ -1,9 +0,0 @@ -function display(s); -% IMCONIC/DISPLAY Command window display of a imconic object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.u) - diff --git a/visionary/@imconic/dres.m b/visionary/@imconic/dres.m deleted file mode 100644 index eacbc73..0000000 --- a/visionary/@imconic/dres.m +++ /dev/null @@ -1,34 +0,0 @@ -function [r,drdP,drdU]=dres(im,proj,ob); -% IMCONIC/DRES function [r,drdP,drdU]=dres(im,proj,ob); -% calculates weighted residuals and their derivatives with -% respect to changes in structure and motion - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = m2v(P*U*P'); - -dupdU=zeros(6,sizedx(ob)); -for i=1:sizedx(ob); - dupdU(:,i)=m2v(P*squeeze(dU(:,:,i))*P'); -end; -dupdP=zeros(6,sizedx(proj)); -for i=1:sizedx(proj); - dupdP(:,i)=m2v( squeeze(dP(:,:,i))*U*P' + P*U*squeeze(dP(:,:,i))' ); -end; - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -dundup = (eye(size(up,1))/(up'*n) - up*n'/(up'*n)^2); -dundP = dundup*dupdP; -dundU = dundup*dupdU; - -r = L*(um-un); -drdP = -L*dundP; -drdU = -L*dundU; - -%keyboard; diff --git a/visionary/@imconic/getdata.m b/visionary/@imconic/getdata.m deleted file mode 100644 index c811315..0000000 --- a/visionary/@imconic/getdata.m +++ /dev/null @@ -1,8 +0,0 @@ -function [u,L,n]=getdata(s); -% IMCONIC/GETDATA -% - -u=s.u; -L=s.L; -n=s.n; - diff --git a/visionary/@imconic/imconic.m b/visionary/@imconic/imconic.m deleted file mode 100644 index 5a6efdf..0000000 --- a/visionary/@imconic/imconic.m +++ /dev/null @@ -1,18 +0,0 @@ -function s=imconic(u,L,n); -% IMCONIC/IMCONIC constructor -% s=imconic(u,L,n); -% - -if nargin == 0, - s.u = []; - s.L = []; - s.n = []; - s = class(s,'imconic'); -elseif isa(u,'imconic'); - s=u; -else - s.u = u; - s.L = L; - s.n = n; - s = class(s,'imconic'); -end; diff --git a/visionary/@imconic/manline.m b/visionary/@imconic/manline.m deleted file mode 100644 index ce2d899..0000000 --- a/visionary/@imconic/manline.m +++ /dev/null @@ -1,7 +0,0 @@ -function s=manline(bild); - -fh=figure; -colormap(gray); -imagesc(bild); -[x,y]=ginput2(2); - diff --git a/visionary/@imconic/plot.m b/visionary/@imconic/plot.m deleted file mode 100644 index 89f3686..0000000 --- a/visionary/@imconic/plot.m +++ /dev/null @@ -1,7 +0,0 @@ -function plot(s) -% IMLINE/PLOT -% plot(s) - -%rita(s.idealpoints,'b+'); -%rita(s.points,'r+'); -plotconic(s.u,'k-'); diff --git a/visionary/@imconic/plotres.m b/visionary/@imconic/plotres.m deleted file mode 100644 index 7633f0f..0000000 --- a/visionary/@imconic/plotres.m +++ /dev/null @@ -1,18 +0,0 @@ -function [r]=plotres(im,proj,ob); -% IMCONIC/CALCRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = m2v(P*U*P'); - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -ritac(un,'k--'); - -r = L*(um-un); diff --git a/visionary/@imconic/private/m2v.m b/visionary/@imconic/private/m2v.m deleted file mode 100644 index 9264d70..0000000 --- a/visionary/@imconic/private/m2v.m +++ /dev/null @@ -1,13 +0,0 @@ -function v=m2v(m); -% M2V v=m2v(m) returns vector of symmetric conic/quadric matrix - -if size(m,1)==4, - v=[m(1,1),m(1,2),m(2,2),m(1,3),m(2,3),m(3,3),... - m(1,4),m(2,4),m(3,4),m(4,4)]'; -elseif size(m,1)==3, - v=[m(1,1),m(1,2),m(2,2),m(1,3),m(2,3),m(3,3)]'; -else - error('Wrong matrix-dimension'); -end - - diff --git a/visionary/@imconic/private/v2m.m b/visionary/@imconic/private/v2m.m deleted file mode 100644 index e4c42e1..0000000 --- a/visionary/@imconic/private/v2m.m +++ /dev/null @@ -1,18 +0,0 @@ -function m=v2m(v); -% V2M m=v2m(v) returns symmetric matrix of conic/quadric vector - -if size(v,1)==10, - m = [ v(1),v(2),v(4),v(7);... - v(2),v(3),v(5),v(8);... - v(4),v(5),v(6),v(9);... - v(7),v(8),v(9),v(10)]; - -elseif size(v,1)==6, - m = [ v(1),v(2),v(4);... - v(2),v(3),v(5);... - v(4),v(5),v(6)]; - -else - error('Wrong vector dimension'); -end - diff --git a/visionary/@imconic/sizeres.m b/visionary/@imconic/sizeres.m deleted file mode 100644 index d70a63d..0000000 --- a/visionary/@imconic/sizeres.m +++ /dev/null @@ -1,5 +0,0 @@ -function sr = sizeres(im); -% IMCONIC/SIZERES -% sr = sizeres(im); - -sr=5; diff --git a/visionary/@imline/calcres.m b/visionary/@imline/calcres.m deleted file mode 100644 index 998e981..0000000 --- a/visionary/@imline/calcres.m +++ /dev/null @@ -1,18 +0,0 @@ -function [r]=calcres(im,proj,ob); -% IMLINE/CALCRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -upl = pflat(P*U); - - -r = upl'*im.u/im.stddevs; - -%up = pflat(cross(upl(:,1),upl(:,2))); -%L=im.L; -%n=im.n; -%un= up/(up'*n); -%r = L*(um-un); diff --git a/visionary/@imline/display.m b/visionary/@imline/display.m deleted file mode 100644 index dc102f1..0000000 --- a/visionary/@imline/display.m +++ /dev/null @@ -1,8 +0,0 @@ -function display(s); -% IMLINE/DISPLAY Command window display of a imline object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.u) diff --git a/visionary/@imline/dres.m b/visionary/@imline/dres.m deleted file mode 100644 index 18fa7c6..0000000 --- a/visionary/@imline/dres.m +++ /dev/null @@ -1,77 +0,0 @@ -function [r,drdP,drdU]=dres(im,proj,ob); -% IMLINE/DRES function [r,drdP,drdU]=dres(im,proj,ob); -% calculates weighted residuals and their derivatives with -% respect to changes in structure and motion - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -upl = P*U; - - -dupldU=zeros(size(upl,1),size(upl,2),sizedx(ob)); -for i=1:sizedx(ob); - dupldU(:,:,i)=P*squeeze(dU(:,:,i)); -end; -dupldP=zeros(size(upl,1),size(upl,2),sizedx(proj)); -for i=1:sizedx(proj); - dupldP(:,:,i)=squeeze(dP(:,:,i))*U; -end; - - -up1=upl(:,1); -up2=upl(:,2); -dup1dU=squeeze(dupldU(:,1,:)); -dup2dU=squeeze(dupldU(:,2,:)); -dup1dP=squeeze(dupldP(:,1,:)); -dup2dP=squeeze(dupldP(:,2,:)); - - - -um=im.u; - -n=[0,0,1]'; -un1=up1/(up1'*n); -un2=up2/(up2'*n); - - -dun1dup1 = (eye(size(up1,1))/(up1'*n) - up1*n'/(up1'*n)^2); -dun2dup2 = (eye(size(up2,1))/(up2'*n) - up2*n'/(up2'*n)^2); -dun1dP = dun1dup1*dup1dP; -dun1dU = dun1dup1*dup1dU; -dun2dP = dun2dup2*dup2dP; -dun2dU = dun2dup2*dup2dU; - -r=[un1,un2]'*um/im.stddevs; - -drdP=[um'*dun1dP;um'*dun2dP]/im.stddevs; -drdU=[um'*dun1dU;um'*dun2dU]/im.stddevs; - - - - -%up = cross(upl(:,1),upl(:,2)); -%dupdU=zeros(3,sizedx(ob)); -%for i=1:sizedx(ob); -% dupdU(:,i)=cross(dupldU(:,1,i),upl(:,2))+cross(upl(:,1),dupldU(:,2,i)); -%end; -%dupdP=zeros(3,sizedx(proj)); -%for i=1:sizedx(proj); -% dupdP(:,i)=cross(dupldP(:,1,i),upl(:,2))+cross(upl(:,1),dupldP(:,2,i)); -%end; - -%n=im.n; -%um=im.u; -%L=im.L; - -%un= up/(up'*n); -%dundup = (eye(size(up,1))/(up'*n) - up*n'/(up'*n)^2); -%dundP = dundup*dupdP; -%dundU = dundup*dupdU; - -%r = L*(um-un); -%drdP = -L*dundP; -%drdU = -L*dundU; - - diff --git a/visionary/@imline/fitline1.m b/visionary/@imline/fitline1.m deleted file mode 100644 index 4ac4d83..0000000 --- a/visionary/@imline/fitline1.m +++ /dev/null @@ -1,35 +0,0 @@ -function [l,L]=fitline(points,stddevs); -% [l,L]=fitline(points,stddevs); -% INPUT: -% points - image point positions in homogeneous coordinates (3xn matrix). -% stddevs - estimated standard deviation for each image point (1xn matrix) -% OUTPUT: -% l - line parameters -% L - cholesky factorisation of covariance of l. -% - -% Normalise points - points=pflat(points); - -% Rough estimate - [u,S,v]=svd(points); - l=u(:,3); - l=l/norm(l(1:2)); - -% Find line such that l'*idealpoints=0, with idealpoints as close -% to points as possible. - for i=1:5; - dldx = [0 0 100;-l(2) l(1) 0]'; - res = (points'*l)./stddevs'; - dresdx = points'./(stddevs'*ones(1,3))*dldx; - dl=-dldx*(dresdx\res); - l=l+dl; - l=l/norm(l(1:2)); - norm(dl) - end; - Fx = dresdx'*dresdx; - Lx = chol(Fx) -% Fl = dldx*Fx*dldx'; - L = Lx*dldx' - - diff --git a/visionary/@imline/getdata.m b/visionary/@imline/getdata.m deleted file mode 100644 index 2a84f69..0000000 --- a/visionary/@imline/getdata.m +++ /dev/null @@ -1,9 +0,0 @@ -function [u,stddevs]=getdata(ll); -% IMLINE/GETDATA -% - -u=ll.u; -stddevs=ll.stddevs; -%L=ll.L; -%n=ll.n; - diff --git a/visionary/@imline/imline.m b/visionary/@imline/imline.m deleted file mode 100644 index c7e9e96..0000000 --- a/visionary/@imline/imline.m +++ /dev/null @@ -1,23 +0,0 @@ -function s=imline(u,stddevs); -% IMLINE/IMLINE constructor -% s=imline(u,stddevs); -% - - -%old: function s=imline(u,L,n); - -if nargin == 0, - s.u = []; - s.stddevs = []; -% s.L = []; -% s.n = []; - s = class(s,'imline'); -elseif isa(u,'imline'); - s = u; -else - s.u = u; - s.stddevs = stddevs; - s = class(s,'imline'); -% s.L = L; -% s.n = n; -end; diff --git a/visionary/@imline/intersect.m b/visionary/@imline/intersect.m deleted file mode 100644 index 8262e17..0000000 --- a/visionary/@imline/intersect.m +++ /dev/null @@ -1,22 +0,0 @@ -function apoint=intersect(line1,line2); - -l1=line1.u; -l2=line2.u; -L1=line1.L; -L2=line2.L; -C1=pinv(L1'*L1); -C2=pinv(L2'*L2); - -T1=[0 -l1(3) l1(2);l1(3) 0 -l1(1);-l1(2) l1(1) 0]; -T2=[0 -l2(3) l2(2);l2(3) 0 -l2(1);-l2(2) l2(1) 0]; - -p1 = cross(l1,l2); -Cp = T1*C2*T1' + T2*C1*T2'; - -n=[0;0;1]; -pn= p1/(p1'*n); -dpndp1 = (eye(3)/(p1'*n)) - (p1*n')/(p1'*n)^2; -Cpn = dpndp1*Cp*dpndp1'; -[U,S,V]=svd(Cpn); -L=sqrtm(inv(S(1:2,1:2)))*U(:,1:2)'; -apoint = impoint(pn,L,n); diff --git a/visionary/@imline/manline.m b/visionary/@imline/manline.m deleted file mode 100644 index ce2d899..0000000 --- a/visionary/@imline/manline.m +++ /dev/null @@ -1,7 +0,0 @@ -function s=manline(bild); - -fh=figure; -colormap(gray); -imagesc(bild); -[x,y]=ginput2(2); - diff --git a/visionary/@imline/plot.m b/visionary/@imline/plot.m deleted file mode 100644 index 4602d1d..0000000 --- a/visionary/@imline/plot.m +++ /dev/null @@ -1,11 +0,0 @@ -function plot(line); -% IMLINE/PLOT -% plot(line) - -plotline(line.u); -%rita(line.idealpoints,'k--'); -%for i=1:size(line.points,2); -% rita(line.points(:,i)*ones(1,2) + line.normals(:,i)*[-5 5],'k-'); -%end; -%rita(line.points,'r+'); -%rita(line.points,'r+'); diff --git a/visionary/@imline/plotres.m b/visionary/@imline/plotres.m deleted file mode 100644 index 37dacee..0000000 --- a/visionary/@imline/plotres.m +++ /dev/null @@ -1,21 +0,0 @@ -function [r]=plotres(im,proj,ob); -% IMLINE/PLOTRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -%keyboard; -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -upl = P*U; - -up = cross(upl(:,1),upl(:,2)); - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -rital(un,'k--'); - -r = L*(um-un); diff --git a/visionary/@imline/sizeres.m b/visionary/@imline/sizeres.m deleted file mode 100644 index df61b40..0000000 --- a/visionary/@imline/sizeres.m +++ /dev/null @@ -1,5 +0,0 @@ -function sr = sizeres(im); -% IMLINE/SIZERES -% sr = sizeres(im); - -sr=2; diff --git a/visionary/@impatch/calcres.m b/visionary/@impatch/calcres.m deleted file mode 100644 index 8aa6ca8..0000000 --- a/visionary/@impatch/calcres.m +++ /dev/null @@ -1,17 +0,0 @@ -function [r]=calcres(im,proj,ob); -% IMCONIC/CALCRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = m2v(P*U*P'); - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); - -r = L*(um-un); diff --git a/visionary/@impatch/display.m b/visionary/@impatch/display.m deleted file mode 100644 index 5786e38..0000000 --- a/visionary/@impatch/display.m +++ /dev/null @@ -1,9 +0,0 @@ -function display(s); -% IMCONIC/DISPLAY Command window display of a imconic object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.u) - diff --git a/visionary/@impatch/dres.m b/visionary/@impatch/dres.m deleted file mode 100644 index eacbc73..0000000 --- a/visionary/@impatch/dres.m +++ /dev/null @@ -1,34 +0,0 @@ -function [r,drdP,drdU]=dres(im,proj,ob); -% IMCONIC/DRES function [r,drdP,drdU]=dres(im,proj,ob); -% calculates weighted residuals and their derivatives with -% respect to changes in structure and motion - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = m2v(P*U*P'); - -dupdU=zeros(6,sizedx(ob)); -for i=1:sizedx(ob); - dupdU(:,i)=m2v(P*squeeze(dU(:,:,i))*P'); -end; -dupdP=zeros(6,sizedx(proj)); -for i=1:sizedx(proj); - dupdP(:,i)=m2v( squeeze(dP(:,:,i))*U*P' + P*U*squeeze(dP(:,:,i))' ); -end; - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -dundup = (eye(size(up,1))/(up'*n) - up*n'/(up'*n)^2); -dundP = dundup*dupdP; -dundU = dundup*dupdU; - -r = L*(um-un); -drdP = -L*dundP; -drdU = -L*dundU; - -%keyboard; diff --git a/visionary/@impatch/getdata.m b/visionary/@impatch/getdata.m deleted file mode 100644 index c811315..0000000 --- a/visionary/@impatch/getdata.m +++ /dev/null @@ -1,8 +0,0 @@ -function [u,L,n]=getdata(s); -% IMCONIC/GETDATA -% - -u=s.u; -L=s.L; -n=s.n; - diff --git a/visionary/@impatch/impatch.m b/visionary/@impatch/impatch.m deleted file mode 100644 index 643362c..0000000 --- a/visionary/@impatch/impatch.m +++ /dev/null @@ -1,104 +0,0 @@ -function s=imconic(points,normals,stddevs); -% IMCONIC/IMCONIC constructor -% s=imconic(points,normals,stddevs); -% - -if nargin == 0, - s.points = []; - s.normals = []; - s.stddevs = []; - s.idealpoints = []; - s.u = []; - s.L = []; - s.n = []; - s = class(s,'imconic'); -elseif isa(points,'imconic'); - ps = points.points; - ns = points.normals; - stds = points.stddevs; - s=imconic(ps,ns,stds); -else -% keyboard; - K=diag([1/500 1/500 1]); - kpoints=K*points; - M=[]; - dC=zeros(3,3,6); - E=eye(6); - for i=1:6, dC(:,:,i)=v2m(E(:,i)); end; - for i=1:size(kpoints,2); - M=[M; m2v2(kpoints(:,i)*kpoints(:,i)')']; - end; - [U,S,V]=svd(M); - u=V(:,6); - ss=zeros(1,size(kpoints,2)); - for k=1:5; - u=u/norm(u); - C=v2m(u); - for kk=1:5; - idealpoints=kpoints+normals*diag(ss); - %diag(idealpoints'*C*idealpoints)' - deltass=-min(max((diag(idealpoints'*C*idealpoints))./ ... - (diag(normals'*C*idealpoints)+... - diag(idealpoints'*C*normals)),-0.02),0.02)'; - ss = ss+deltass; - normss=norm(deltass); - end; - idealpoints=kpoints+normals*diag(ss); - for i=1:6, - dss1dC(:,i) = -diag(idealpoints'*dC(:,:,i)*idealpoints)./ ... - (diag(normals'*C*idealpoints)+... - diag(idealpoints'*C*normals)) + ... - (diag(normals'*dC(:,:,i)*idealpoints)+... - diag(idealpoints'*dC(:,:,i)*normals)).* ... - diag(idealpoints'*C*idealpoints)./ ... - (diag(normals'*C*idealpoints)+... - diag(idealpoints'*C*normals)).^2; - end; -% u(1)=u(1)-litet; -% u(2)=u(2)+litet; -% ss1=ss'; -% [(ss1-ss0)/litet dss1dC(:,2)] - [U,S,V]=svd(dss1dC); - du = V(:,1:5)*inv(S(1:5,1:5))*U(:,1:5)'*(ss'); - u=u-du; - normdu=norm(du); - stddev=std(ss); - end; - normss=norm(deltass) - normdu=norm(du) - stddev=std(ss); - [U,S,V]=svd(dss1dC); - M= V(:,1:5)*inv(S(1:5,1:5))*U(:,1:5)'; - Cu = M*diag(stddev^2*ones(size(ss)))*M'; - % transformera till - E=eye(6); - C1 = K'*v2m(u)*K; - C2 = inv( K'*v2m(u)*K ); - up = m2v(inv( K'*v2m(u)*K )); - for i=1:6, - dupdu(:,i) = -m2v( C2*K'*v2m(E(:,i))*K*C2 ); - end; -% up0 = m2v(inv( K'*v2m(u)*K )); -% up1 = m2v(inv( K'*v2m(u+litet*E(:,i))*K )); -% [(up1-up0)/litet dupdu(:,i)]; - Cup = dupdu*Cu*dupdu'; - % PROJICERA NER up - n=up/norm(up); - un= up/(up'*n); - dundup = (eye(size(up,1))/(up'*n) - up*n'/(up'*n)^2); - Cun = dundup*Cup*dundup'; - [U,S,V]=svd(Cun); - U=U(:,1:5); - S=S(1:5,1:5); - L=inv(sqrtm(S))*U'; - n=un; - - s.points = points; - s.normals = normals; - s.stddevs = stddevs; - s.idealpoints = inv(K)*idealpoints; - s.u = un; - s.L = L; - s.n = n; - s = class(s,'imconic'); -end; diff --git a/visionary/@impatch/plot.m b/visionary/@impatch/plot.m deleted file mode 100644 index b7b5929..0000000 --- a/visionary/@impatch/plot.m +++ /dev/null @@ -1,7 +0,0 @@ -function plot(s) -% IMLINE/PLOT -% plot(s) - -%rita(s.idealpoints,'b+'); -%rita(s.points,'r+'); -ritac(s.u,'k-'); diff --git a/visionary/@impatch/plotres.m b/visionary/@impatch/plotres.m deleted file mode 100644 index 7633f0f..0000000 --- a/visionary/@impatch/plotres.m +++ /dev/null @@ -1,18 +0,0 @@ -function [r]=plotres(im,proj,ob); -% IMCONIC/CALCRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = m2v(P*U*P'); - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -ritac(un,'k--'); - -r = L*(um-un); diff --git a/visionary/@impatch/private/m2v.m b/visionary/@impatch/private/m2v.m deleted file mode 100644 index 9264d70..0000000 --- a/visionary/@impatch/private/m2v.m +++ /dev/null @@ -1,13 +0,0 @@ -function v=m2v(m); -% M2V v=m2v(m) returns vector of symmetric conic/quadric matrix - -if size(m,1)==4, - v=[m(1,1),m(1,2),m(2,2),m(1,3),m(2,3),m(3,3),... - m(1,4),m(2,4),m(3,4),m(4,4)]'; -elseif size(m,1)==3, - v=[m(1,1),m(1,2),m(2,2),m(1,3),m(2,3),m(3,3)]'; -else - error('Wrong matrix-dimension'); -end - - diff --git a/visionary/@impatch/private/v2m.m b/visionary/@impatch/private/v2m.m deleted file mode 100644 index e4c42e1..0000000 --- a/visionary/@impatch/private/v2m.m +++ /dev/null @@ -1,18 +0,0 @@ -function m=v2m(v); -% V2M m=v2m(v) returns symmetric matrix of conic/quadric vector - -if size(v,1)==10, - m = [ v(1),v(2),v(4),v(7);... - v(2),v(3),v(5),v(8);... - v(4),v(5),v(6),v(9);... - v(7),v(8),v(9),v(10)]; - -elseif size(v,1)==6, - m = [ v(1),v(2),v(4);... - v(2),v(3),v(5);... - v(4),v(5),v(6)]; - -else - error('Wrong vector dimension'); -end - diff --git a/visionary/@impatch/sizeres.m b/visionary/@impatch/sizeres.m deleted file mode 100644 index 75f5d57..0000000 --- a/visionary/@impatch/sizeres.m +++ /dev/null @@ -1,5 +0,0 @@ -function sr = sizeres(im); -% IMLINE/SIZERES -% sr = sizeres(im); - -sr=5; diff --git a/visionary/@impoint/calcres.m b/visionary/@impoint/calcres.m deleted file mode 100644 index a281c78..0000000 --- a/visionary/@impoint/calcres.m +++ /dev/null @@ -1,18 +0,0 @@ -function [r]=calcres(im,proj,ob,imnbr); -% IMPOINT/CALCRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -%[U,dU]=udu(ob); -U=udu(ob); -%[P,dP]=pdp(proj); -P=pdp(proj); - -up = P*U; - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); - -r = L*(um-un); diff --git a/visionary/@impoint/display.m b/visionary/@impoint/display.m deleted file mode 100644 index 162e8fb..0000000 --- a/visionary/@impoint/display.m +++ /dev/null @@ -1,8 +0,0 @@ -function display(s); -% IMPOINT/DISPLAY Command window display of a impoint object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.u) diff --git a/visionary/@impoint/dres.m b/visionary/@impoint/dres.m deleted file mode 100644 index 637af68..0000000 --- a/visionary/@impoint/dres.m +++ /dev/null @@ -1,27 +0,0 @@ -function [r,drdP,drdU]=dres(im,proj,ob); -% IMPOINT/DRES function [r,drdP,drdU]=dres(im,proj,ob); -% calculates weighted residuals and their derivatives with -% respect to changes in structure and motion - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = P*U; -dupdP = squeeze(dP(:,1,:)*U(1) + dP(:,2,:)*U(2) + dP(:,3,:)*U(3) + dP(:,4,:)*U(4)); -dupdU = P*dU; - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -dundup = (eye(size(up,1))/(up'*n) - up*n'/(up'*n)^2); -dundP = dundup*dupdP; -dundU = dundup*dupdU; - -r = L*(um-un); -drdP = -L*dundP; -drdU = -L*dundU; - -%keyboard; diff --git a/visionary/@impoint/getdata.m b/visionary/@impoint/getdata.m deleted file mode 100644 index 700b628..0000000 --- a/visionary/@impoint/getdata.m +++ /dev/null @@ -1,8 +0,0 @@ -function [u,L,n]=getdata(point); -% POINT/GETDATA -% - -u=point.u; -L=point.L; -n=point.n; - diff --git a/visionary/@impoint/impoint.m b/visionary/@impoint/impoint.m deleted file mode 100644 index 43d0847..0000000 --- a/visionary/@impoint/impoint.m +++ /dev/null @@ -1,15 +0,0 @@ -function s=impoint(u,L,n); - -if nargin == 0, - s.u = []; - s.L = []; - s.n = []; - s = class(s,'impoint'); -elseif isa(u,'impoint'); - s = u; -else - s.u = u; - s.L = L; - s.n = n; - s = class(s,'impoint'); -end; diff --git a/visionary/@impoint/plot.m b/visionary/@impoint/plot.m deleted file mode 100644 index 3fbb9a4..0000000 --- a/visionary/@impoint/plot.m +++ /dev/null @@ -1,5 +0,0 @@ -function plot(pp); -% IMPOINT/PLOT -% plot(pp) - -rita(pp.u,'y*'); diff --git a/visionary/@impoint/plotres.m b/visionary/@impoint/plotres.m deleted file mode 100644 index d18df53..0000000 --- a/visionary/@impoint/plotres.m +++ /dev/null @@ -1,19 +0,0 @@ -function [r]=plotres(im,proj,ob); -% IMPOINT/PLOTRES function [r]=res(im,proj,ob); -% calculates weighted residuals - -[U,dU]=udu(ob); - -[P,dP]=pdp(proj); - -up = P*U; - -n=im.n; -um=im.u; -L=im.L; - -un= up/(up'*n); -rita(pflat(un),'y*'); -rita(pflat(um),'r*'); - -r = L*(um-un); diff --git a/visionary/@impoint/sizeres.m b/visionary/@impoint/sizeres.m deleted file mode 100644 index 676e171..0000000 --- a/visionary/@impoint/sizeres.m +++ /dev/null @@ -1,3 +0,0 @@ -function sr = sizeres(im); - -sr=2; diff --git a/visionary/@motion/addcameras.m b/visionary/@motion/addcameras.m deleted file mode 100644 index b624628..0000000 --- a/visionary/@motion/addcameras.m +++ /dev/null @@ -1,15 +0,0 @@ -function n=addcameras(m,p) -%MOTION/ADDCAMERAS mout=addcameras(m,p) adds p cameras to motion m - -n = m; - -if iscell(p) - n.cam = {m.cam{:},p{:}}; -else - n.cam = {m.cam{:},p}; -end - - - - - diff --git a/visionary/@motion/changecsystem.m b/visionary/@motion/changecsystem.m deleted file mode 100644 index 2daf2d7..0000000 --- a/visionary/@motion/changecsystem.m +++ /dev/null @@ -1,23 +0,0 @@ -function m=changecsystem(m,T,v); -% MOTION/changecsystem m=changecsystem(m,T,v) changes coordinate system -% If T is a 4x4 projective transformation, then P'=P*inv(T) for view v -% If T is a 3x3 projective transformation, then P'=T*P for view v -% If v is not specified all cameras are changed - -if nargin<3, - v=1:length(m.cam); -end - -if size(T,1)==4, - invT=inv(T); - for i=v, - m.cam{i} = m.cam{i} * invT; - end -else - for i=v, - m.cam{i} = T*m.cam{i}; - end -end - - - diff --git a/visionary/@motion/display.m b/visionary/@motion/display.m deleted file mode 100644 index 12a369b..0000000 --- a/visionary/@motion/display.m +++ /dev/null @@ -1,15 +0,0 @@ -function display(m) -%MOTION/DISPLAY displays motion object -disp(' '); -disp([inputname(1),' = ']); -disp(' '); -nbr = length(m.cam); - disp([' Motion:']); -if nbr == 1, - disp([' 1 camera']); -else - disp([' ' num2str(nbr),' cameras']); -end - -disp(' '); - diff --git a/visionary/@motion/focalpoints.m b/visionary/@motion/focalpoints.m deleted file mode 100644 index 871f62c..0000000 --- a/visionary/@motion/focalpoints.m +++ /dev/null @@ -1,17 +0,0 @@ -function s=focalpoints(m) -%MOTION/FOCALPOINTS s=focalpoints(m) calculates camera positions -% INPUT: -% m - motion -% OUTPUT: -% s - structure containing 3D focal points - -nbrcameras=size(m); -c=zeros(4,nbrcameras); - -for i=1:nbrcameras; - p=getcameras(m,i); - c(:,i) = null(p); -end - -s=structure(c); - diff --git a/visionary/@motion/getcameras.m b/visionary/@motion/getcameras.m deleted file mode 100644 index d3dea63..0000000 --- a/visionary/@motion/getcameras.m +++ /dev/null @@ -1,15 +0,0 @@ -function p=getcameras(m,index) -%MOTION/GETCAMERAS getcameras(m,index) returns m's cameras with index - -if nargin==1, - p=m.cam; -else - if length(index)==1, - p=m.cam{index}; - else - p={m.cam{index}}; - end -end - - - diff --git a/visionary/@motion/motion.m b/visionary/@motion/motion.m deleted file mode 100644 index 70ff5ac..0000000 --- a/visionary/@motion/motion.m +++ /dev/null @@ -1,25 +0,0 @@ -function m = motion(p) -%MOTION class constructor -% m=motion(p) creates an MOTION object where -% p: is a cell array of 3x4 camera matrices or single 3x4 camera matrix - -if nargin == 1 & isa(p,'motion'), - m=p; -else - - if nargin == 0, - m.cam = {}; - else - if iscell(p), - m.cam = p; - else - if size(p,1)==3 & size(p,2)==4, - m.cam = {p}; - else - m.cam = {}; - end - end - end - m = class(m,'motion'); - -end diff --git a/visionary/@motion/plot.m b/visionary/@motion/plot.m deleted file mode 100644 index a0fb14c..0000000 --- a/visionary/@motion/plot.m +++ /dev/null @@ -1,52 +0,0 @@ -function plot(m,s,color) -%MOTION/PLOT plot 3D camera orbit (and structure) -% plot(m,style) plots 3D camera orbit with (optional) color -% plot(m,s,style) plots 3D camera orbit and structure with (optional) color -% INPUT: -% m - motion -% s - structure -% color - plot color string - -if nargin==1 | (nargin==2 & isa(s,'structure')), - color='r'; -elseif nargin==2, - color=s; -end - -holdmode=ishold; - -if nargin>=2 & isa(s,'structure'), - plot(s); - hold on; -end - - -focals=focalpoints(m); -%tmp=[pflat(s),pflat(focals)];tmp(4,:)=[];ddl=std(tmp(:)); -pp=pflat(getpoints(focals)); - -ax=axis; -ddl=std(ax)/2; - -dd=zeros(3,size(m)); -for i=1:size(m); - [K,P]=rq(getcameras(m,i)); - dd(:,i)=ddl*P(3,1:3)'; -end - - -plot(focals,[color,'*']); -hold on;axis equal; -plot(focals,[color,'o']); -plot(focals,[color,'-']); -quiver3(pp(1,:),pp(2,:),pp(3,:),dd(1,:), dd(2,:), dd(3,:),[color,'-'],'LineWidth',1.5,'MaxHeadSize',1.5); - -if holdmode == 0 - hold off; -end - - - - - - diff --git a/visionary/@motion/plus.m b/visionary/@motion/plus.m deleted file mode 100644 index 0a6e2f6..0000000 --- a/visionary/@motion/plus.m +++ /dev/null @@ -1,9 +0,0 @@ -function m=plus(m1,m2) -%MOTION/PLUS m=plus(m1,m2) adds m2's cameras to m1 - -m = motion(m1); -m2 = motion(m2); - -m.cam = [m.cam, getcameras(m2)]; - - diff --git a/visionary/@motion/size.m b/visionary/@motion/size.m deleted file mode 100644 index 6feb537..0000000 --- a/visionary/@motion/size.m +++ /dev/null @@ -1,5 +0,0 @@ -function sz = size(m) -%MOTION/SIZE sz = size(m) returns nbr of cameras - -sz=length(m.cam); - diff --git a/visionary/@obconic/display.m b/visionary/@obconic/display.m deleted file mode 100644 index c4a043f..0000000 --- a/visionary/@obconic/display.m +++ /dev/null @@ -1,8 +0,0 @@ -function display(s); -% OBCONIC/DISPLAY Command window display of a obpoint object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.U) diff --git a/visionary/@obconic/locparam.m b/visionary/@obconic/locparam.m deleted file mode 100644 index 128954b..0000000 --- a/visionary/@obconic/locparam.m +++ /dev/null @@ -1,13 +0,0 @@ -function obut = locparam(obin,dx); -% OBPOINT/LOCPARAM -% - -obut = obin; -dU=obin.dU; -for i=1:9; - obut.U = obut.U + squeeze(dU(:,:,i))*dx(i); -end; - -% Hör borde man normalisera om dvs hitta U som ligger så nära -% U som möjligt i frobeniusnorm och som har ortonormerade kolumner. - diff --git a/visionary/@obconic/obconic.m b/visionary/@obconic/obconic.m deleted file mode 100644 index ef05879..0000000 --- a/visionary/@obconic/obconic.m +++ /dev/null @@ -1,19 +0,0 @@ -function s=obconic(U); -% OBCONIC/OBCONIC constructor -% - -if nargin == 0, - s.U = []; - s.dU = []; - s = class(s,'obconic'); -elseif isa(U,'obconic'); - s = U; -else - U = U/norm(U,'fro'); - [uu,ss,vv]=svd(m2v(U)); - dU=zeros(4,4,9); - for i=1:9; dU(:,:,i)=v2m(uu(:,i+1)); end; - s.U = U; - s.dU = dU; - s = class(s,'obconic'); -end; diff --git a/visionary/@obconic/sizedx.m b/visionary/@obconic/sizedx.m deleted file mode 100644 index 7e529ff..0000000 --- a/visionary/@obconic/sizedx.m +++ /dev/null @@ -1,4 +0,0 @@ -function sx=sizedx(ob); -% OBLINE/SIZEDX - -sx=9; diff --git a/visionary/@obconic/udu.m b/visionary/@obconic/udu.m deleted file mode 100644 index ddd7fc4..0000000 --- a/visionary/@obconic/udu.m +++ /dev/null @@ -1,6 +0,0 @@ -function [U,dU]=udu(point); -% OBCONIC/UDU -% function [U,dU]=udu(point); - -U=point.U; -dU=point.dU; diff --git a/visionary/@obline/display.m b/visionary/@obline/display.m deleted file mode 100644 index 91cce5a..0000000 --- a/visionary/@obline/display.m +++ /dev/null @@ -1,8 +0,0 @@ -function display(s); -% OBLINE/DISPLAY Command window display of a obpoint object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.U) diff --git a/visionary/@obline/locparam.m b/visionary/@obline/locparam.m deleted file mode 100644 index 3d43e4e..0000000 --- a/visionary/@obline/locparam.m +++ /dev/null @@ -1,13 +0,0 @@ -function obut = locparam(obin,dx); -% OBPOINT/LOCPARAM -% - -obut = obin; -dU=obin.dU; -for i=1:4; - obut.U = obut.U + squeeze(dU(:,:,i))*dx(i); -end; - -% Hör borde man normalisera om dvs hitta U som ligger så nära -% U som möjligt i frobeniusnorm och som har ortonormerade kolumner. - diff --git a/visionary/@obline/obline.m b/visionary/@obline/obline.m deleted file mode 100644 index 0654639..0000000 --- a/visionary/@obline/obline.m +++ /dev/null @@ -1,25 +0,0 @@ -function s=obline(U); -% OBLINE/OBLINE constructor -% - -if nargin == 0, - s.U = []; - s.dU = []; - s = class(s,'obline'); -elseif isa(U,'obline'); - s = U; -else -% U=psphere(U); %not valid with end point norm -% du=null(U'); -% du1=du(:,1); -% du2=du(:,2); - U = pflat(U); - dir= U(:,2)-U(:,1); - [du,ds,dv]=svd([dir [0;0;0;1]]); - du1=du(:,3); - du2=du(:,4); - - s.U = U; - s.dU = cat(3,[du1 zeros(4,1)],[du2 zeros(4,1)],[zeros(4,1) du1],[zeros(4,1) du2]); - s = class(s,'obline'); -end; diff --git a/visionary/@obline/sizedx.m b/visionary/@obline/sizedx.m deleted file mode 100644 index 678fda0..0000000 --- a/visionary/@obline/sizedx.m +++ /dev/null @@ -1,4 +0,0 @@ -function sx=sizedx(ob); -% OBLINE/SIZEDX - -sx=4; diff --git a/visionary/@obline/udu.m b/visionary/@obline/udu.m deleted file mode 100644 index 6a83ad2..0000000 --- a/visionary/@obline/udu.m +++ /dev/null @@ -1,6 +0,0 @@ -function [U,dU]=udu(point); -% OBLINE/UDU -% function [U,dU]=udu(point); - -U=point.U; -dU=point.dU; diff --git a/visionary/@obpoint/display.m b/visionary/@obpoint/display.m deleted file mode 100644 index b3e7b08..0000000 --- a/visionary/@obpoint/display.m +++ /dev/null @@ -1,8 +0,0 @@ -function display(s); -% OBPOINT/DISPLAY Command window display of a obpoint object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.U) diff --git a/visionary/@obpoint/getparam.m b/visionary/@obpoint/getparam.m deleted file mode 100644 index a6cfc0a..0000000 --- a/visionary/@obpoint/getparam.m +++ /dev/null @@ -1,5 +0,0 @@ -function obparam = getparam(obin); -% OBPOINT/GETPARAM - -obparam=obin.dU'*obin.U; - diff --git a/visionary/@obpoint/locparam.m b/visionary/@obpoint/locparam.m deleted file mode 100644 index 02930c4..0000000 --- a/visionary/@obpoint/locparam.m +++ /dev/null @@ -1,6 +0,0 @@ -function obut = locparam(obin,dx); -% OBPOINT/LOCPARAM -% - -obut = obin; -obut.U = obut.U + obut.dU*dx; diff --git a/visionary/@obpoint/obpoint.m b/visionary/@obpoint/obpoint.m deleted file mode 100644 index 934a068..0000000 --- a/visionary/@obpoint/obpoint.m +++ /dev/null @@ -1,14 +0,0 @@ -function s=obpoint(U); - -if nargin == 0, - s.U = []; - s.dU = []; - s = class(s,'obpoint'); -elseif isa(U,'obpoint'); - s = U; -else -% s.U = pflat(U); %not necessary - causes numerical instabilities - s.U = U; - s.dU = [1 0 0;0 1 0;0 0 1;0 0 0]; - s = class(s,'obpoint'); -end; diff --git a/visionary/@obpoint/setparam.m b/visionary/@obpoint/setparam.m deleted file mode 100644 index a7e0bae..0000000 --- a/visionary/@obpoint/setparam.m +++ /dev/null @@ -1,8 +0,0 @@ -function obut = setparam(obin,xparam); -% OBPOINT/SETPARAM -% - -obut = obin; -currparam = getparam(obin); - -obut.U = obut.U + obut.dU*(xparam-currparam); diff --git a/visionary/@obpoint/sizedx.m b/visionary/@obpoint/sizedx.m deleted file mode 100644 index f9ef4cf..0000000 --- a/visionary/@obpoint/sizedx.m +++ /dev/null @@ -1,3 +0,0 @@ -function sx=sizedx(ob); - -sx=3; diff --git a/visionary/@obpoint/udu.m b/visionary/@obpoint/udu.m deleted file mode 100644 index cf4a42e..0000000 --- a/visionary/@obpoint/udu.m +++ /dev/null @@ -1,5 +0,0 @@ -function [U,dU]=udu(point); -% OBPOINT/UDU function function [U,dU]=udu(point); - -U=point.U; -dU=point.dU; diff --git a/visionary/@onepatch/calcintensities.m b/visionary/@onepatch/calcintensities.m deleted file mode 100644 index b678781..0000000 --- a/visionary/@onepatch/calcintensities.m +++ /dev/null @@ -1,3 +0,0 @@ -function intensities = calcintensities(basep,bild,H); - -[intensities, intengrad]=measure(bild,pflat(H*basep.points),basep.a); diff --git a/visionary/@onepatch/calcpatchresgrad.m b/visionary/@onepatch/calcpatchresgrad.m deleted file mode 100644 index ef1806e..0000000 --- a/visionary/@onepatch/calcpatchresgrad.m +++ /dev/null @@ -1,12 +0,0 @@ -function res = calcpatchresgrad(basep,bild,H,dhdx); - -[intensities, intengrad]=measure(bild,pflat(H*basep.points),basep.a); - -dHdx = homographyderiv(H,dhdx); -pph = H*basep.points; -pp = pflat(pph); - -[intensities, intengrad]=measure(bild,pp,basep.a); - -res = (intensities - basep.intensities)' - diff --git a/visionary/@onepatch/calcres.m b/visionary/@onepatch/calcres.m deleted file mode 100644 index 2d76b35..0000000 --- a/visionary/@onepatch/calcres.m +++ /dev/null @@ -1,13 +0,0 @@ -function [res,intm,intp] = calcres(basep,bild,H,dhdx); - -[intensities, intengrad]=measure(bild,pflat(H*basep.points),basep.a); - -%dHdx = homographyderiv(H,dhdx); -pph = H*basep.points; -pp = pflat(pph); - -[intensities, intengrad]=measure(bild,pp,basep.a); - -res = (intensities - basep.intensities)'; -intm = intensities'; -intp = basep.intensities'; diff --git a/visionary/@onepatch/calcresgrad.m b/visionary/@onepatch/calcresgrad.m deleted file mode 100644 index 62a7bb0..0000000 --- a/visionary/@onepatch/calcresgrad.m +++ /dev/null @@ -1,12 +0,0 @@ -function res = calcres(basep,bild,H,dhdx); - -[intensities, intengrad]=measure(bild,pflat(H*basep.points),basep.a); - -dHdx = homographyderiv(H,dhdx); -pph = H*basep.points; -pp = pflat(pph); - -[intensities, intengrad]=measure(bild,pp,basep.a); - -res = (intensities - basep.intensities)' - diff --git a/visionary/@onepatch/display.m b/visionary/@onepatch/display.m deleted file mode 100644 index 5786e38..0000000 --- a/visionary/@onepatch/display.m +++ /dev/null @@ -1,9 +0,0 @@ -function display(s); -% IMCONIC/DISPLAY Command window display of a imconic object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.u) - diff --git a/visionary/@onepatch/getintensities.m b/visionary/@onepatch/getintensities.m deleted file mode 100644 index 263d3e4..0000000 --- a/visionary/@onepatch/getintensities.m +++ /dev/null @@ -1,3 +0,0 @@ -function i = getintensities(bp,i); - -i = bp.intensities; diff --git a/visionary/@onepatch/onepatch.m b/visionary/@onepatch/onepatch.m deleted file mode 100644 index 5787f83..0000000 --- a/visionary/@onepatch/onepatch.m +++ /dev/null @@ -1,24 +0,0 @@ -function s=onepatch(points,intensities,a); -% IMCONIC/IMCONIC constructor -% s=imconic(points,normals,stddevs); -% - -if nargin == 0, - s.points = []; - s.intensities = []; - s.a = []; - s = class(s,'onepatch'); -elseif isa(points,'onepatch'); - s=patch; -else - if nargin<2, - s.points = points; - s.intensities = zeros(1,size(points,2)); - s.a = 1; - else - s.points = points; - s.intensities = intensities; - s.a = a; - end; - s = class(s,'onepatch'); -end; diff --git a/visionary/@onepatch/private/measure.m b/visionary/@onepatch/private/measure.m deleted file mode 100644 index c4ff916..0000000 --- a/visionary/@onepatch/private/measure.m +++ /dev/null @@ -1,10 +0,0 @@ -function [inten, intengrad]=measure(image,points,a); - -inten=0; - -for i=1:size(points,2) - [In, Igrad] = measurepoint(image,points(1:2,i),a); - inten(1,i) = In; - intengrad(1:2,i) = Igrad; -end; - diff --git a/visionary/@onepatch/private/measurepoint.m b/visionary/@onepatch/private/measurepoint.m deleted file mode 100644 index b660f82..0000000 --- a/visionary/@onepatch/private/measurepoint.m +++ /dev/null @@ -1,25 +0,0 @@ -function [inten, intengrad]=measurepoint(bild,punkt,a); - -if a<1 - error('Implementation only supports a-values larger than 1'); -end; - -x0=punkt(1); -y0=punkt(2); -[m,n]=size(bild); -NN=round(a*3); - - -cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -cutbild=bild(cuty,cutx); - -[x,y]=meshgrid(cutx,cuty); - -filter=exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^2*pi); -filtx=2*(x-x0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -filty=2*(y-y0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -intengrad(1,1)=sum(sum(filtx.*cutbild)); -intengrad(2,1)=sum(sum(filty.*cutbild)); -inten=sum(sum(filter.*cutbild)); - diff --git a/visionary/@onepatch/setintensities.m b/visionary/@onepatch/setintensities.m deleted file mode 100644 index e73d6a8..0000000 --- a/visionary/@onepatch/setintensities.m +++ /dev/null @@ -1,3 +0,0 @@ -function bp = setintensities(bp,i); - -bp.intensities = i; diff --git a/visionary/@pmatrix/display.m b/visionary/@pmatrix/display.m deleted file mode 100644 index 8fbeb2a..0000000 --- a/visionary/@pmatrix/display.m +++ /dev/null @@ -1,8 +0,0 @@ -function display(s); -% PMATRIX/DISPLAY Command window display of a impoint object; -% - -disp(' '); -disp([inputname(1),' = ']) -disp(' '); -disp(s.P) diff --git a/visionary/@pmatrix/locparam.m b/visionary/@pmatrix/locparam.m deleted file mode 100644 index 3cae5fb..0000000 --- a/visionary/@pmatrix/locparam.m +++ /dev/null @@ -1,9 +0,0 @@ -function obut = locparam(obin,dx); -% PMATRIX/LOCPARAM -% -obut = obin; -dP=obut.dP; -for i=1:size(dx,1); - obut.P = obut.P + squeeze(dP(:,:,i))*dx(i); -end; - diff --git a/visionary/@pmatrix/pdp.m b/visionary/@pmatrix/pdp.m deleted file mode 100644 index 043a444..0000000 --- a/visionary/@pmatrix/pdp.m +++ /dev/null @@ -1,5 +0,0 @@ -function [P,dP]=pdp(proj); -% PMATRIX/PDP function function [P,dP]=pdp(proj); - -P=proj.P; -dP=proj.dP; diff --git a/visionary/@pmatrix/pmatrix.m b/visionary/@pmatrix/pmatrix.m deleted file mode 100644 index a0f9bfb..0000000 --- a/visionary/@pmatrix/pmatrix.m +++ /dev/null @@ -1,13 +0,0 @@ -function s=pmatrix(P); - -if nargin == 0, - s.P = []; - s.dP = []; - s = class(s,'pmatrix'); -elseif isa(P,'pmatrix'); - s = P; -else - s.P = P; - s.dP = reshape(eye(12,12),3,4,12); - s = class(s,'pmatrix'); -end; diff --git a/visionary/@pmatrix/sizedx.m b/visionary/@pmatrix/sizedx.m deleted file mode 100644 index 6833f30..0000000 --- a/visionary/@pmatrix/sizedx.m +++ /dev/null @@ -1,3 +0,0 @@ -function sx=sizedx(pm); - -sx=12; diff --git a/visionary/@structure/addlines.m b/visionary/@structure/addlines.m deleted file mode 100644 index 78ff1c9..0000000 --- a/visionary/@structure/addlines.m +++ /dev/null @@ -1,16 +0,0 @@ -function s=addlines(s,lines) -%STRUCTURE/ADDLINES s=addlines(s,lines) adds 3D lines -% Each line is respresented by two 3D homogeneous points -% (or alternatively, by plucker coordinates) - -if size(lines,1)==8, - s.lines = [s.lines,lines]; - -elseif size(lines,1)==6, %plucker - zz=zeros(1,size(lines,2)); - s.lines=[s.lines,[zz;lines(1,:);lines(2,:);lines(3,:);... - -lines(1,:);zz;lines(6,:);-lines(5,:)]]; -else - error('Unknown line format'); -end - diff --git a/visionary/@structure/addpoints.m b/visionary/@structure/addpoints.m deleted file mode 100644 index f2ed731..0000000 --- a/visionary/@structure/addpoints.m +++ /dev/null @@ -1,9 +0,0 @@ -function s=addpoints(s,points) -%STRUCTURE/ADDPOINTS s=addpoints(s,points) adds homog. or cart. points - -if size(points,1) == 3, - s.points = [s.points,[points;ones(1,size(points,2))]]; -else - s.points = [s.points,points]; -end - diff --git a/visionary/@structure/addquadrics.m b/visionary/@structure/addquadrics.m deleted file mode 100644 index c1a484f..0000000 --- a/visionary/@structure/addquadrics.m +++ /dev/null @@ -1,4 +0,0 @@ -function s=addquadrics(s,quadrics) -%STRUCTURE/ADDQUADRICS s=addquadrics(s,quadrics) adds dual quadrics - -s.quadrics = [s.quadrics,quadrics]; diff --git a/visionary/@structure/changecsystem.m b/visionary/@structure/changecsystem.m deleted file mode 100644 index dfccbac..0000000 --- a/visionary/@structure/changecsystem.m +++ /dev/null @@ -1,16 +0,0 @@ -function s=changecsystem(s,T); -% STRUCTURE/changecsystem s=changecsystem(s,T) changes coordinate system -% T is a 4x4 projective transformation - -if ~isempty(s.points) - s.points = T * s.points; -end -if ~isempty(s.lines) - s.lines = [T,zeros(4);zeros(4),T] * s.lines; -end - -for i=1:size(s.quadrics,2); - m=v2m(s.quadrics(:,i)); - mnew = T*m*T'; - s.quadrics(:,i)=m2v(mnew); -end diff --git a/visionary/@structure/clearlines.m b/visionary/@structure/clearlines.m deleted file mode 100644 index 63c9b7d..0000000 --- a/visionary/@structure/clearlines.m +++ /dev/null @@ -1,6 +0,0 @@ -function s=clearlines(s) -%STRUCTURE/CLEARLINES s=clearlines(s) clears all lines - -s.lines = zeros(8,0); - - diff --git a/visionary/@structure/clearpoints.m b/visionary/@structure/clearpoints.m deleted file mode 100644 index 65d299b..0000000 --- a/visionary/@structure/clearpoints.m +++ /dev/null @@ -1,6 +0,0 @@ -function s=clearpoints(s) -%STRUCTURE/CLEARPOINTS s=clearpoints(s) clears all points - -s.points = zeros(4,0); - - diff --git a/visionary/@structure/clearquadrics.m b/visionary/@structure/clearquadrics.m deleted file mode 100644 index 2f4080e..0000000 --- a/visionary/@structure/clearquadrics.m +++ /dev/null @@ -1,6 +0,0 @@ -function s=clearquadrics(s) -%STRUCTURE/CLEARQUADRICS s=clearquadrics(s) clears all quadrics - -s.quadrics = zeros(10,0); - - diff --git a/visionary/@structure/display.m b/visionary/@structure/display.m deleted file mode 100644 index 4a7dced..0000000 --- a/visionary/@structure/display.m +++ /dev/null @@ -1,38 +0,0 @@ -function display(s) -%STRUCTURE/DISPLAY displays structure object - -disp(' '); -disp([inputname(1),' = ']); -disp(' '); -disp(' Structure:'); -np=size(s.points,2); -nl=size(s.lines,2); -nq=size(s.quadrics,2); -if np==0 & nl==0 & nq==0, - disp(' No features'); -else - if np>0, - if np==1, - disp([' 1 point']); - else - disp([' ',num2str(np),' points']); - end - end - if nl>0, - if nl==1, - disp([' 1 line']); - else - disp([' ',num2str(nl),' lines']); - end - end - if nq>0, - if nq==1, - disp([' 1 quadric']); - else - disp([' ',num2str(nq),' quadrics']); - end - end -end - -disp(' '); - diff --git a/visionary/@structure/getlines.m b/visionary/@structure/getlines.m deleted file mode 100644 index 4d00825..0000000 --- a/visionary/@structure/getlines.m +++ /dev/null @@ -1,13 +0,0 @@ -function l=getlines(s,index) -%STRUCTURE/GETLINES getlines(s,index) returns s's lines with index -% Each line is represented by two homogeneous 3D points. -% See also getpluckerlines. - -if nargin==1, - l=s.lines; -else - l=s.lines(:,index); -end - - - diff --git a/visionary/@structure/getpluckerlines.m b/visionary/@structure/getpluckerlines.m deleted file mode 100644 index 389513a..0000000 --- a/visionary/@structure/getpluckerlines.m +++ /dev/null @@ -1,23 +0,0 @@ -function l=getpluckerlines(s,index) -%STRUCTURE/GETPLUCKERLINES getpluckerlines(s,index) returns s's -% lines (with index) in plucker coordinates. - -if nargin==1, - tmp=s.lines; -else - tmp=s.lines(:,index); -end - -l=zeros(6,size(tmp,2)); -for i=1:size(tmp,2); - tt=[tmp(1:4,i)';tmp(5:8,i)']; - l(:,i)=[det(tt(:,[1,2]));det(tt(:,[1,3]));det(tt(:,[1,4]));... - det(tt(:,[3,4]));det(tt(:,[4,2]));det(tt(:,[2,3]))]; -end - -l=normc(l); - - - - - diff --git a/visionary/@structure/getpoints.m b/visionary/@structure/getpoints.m deleted file mode 100644 index c971a48..0000000 --- a/visionary/@structure/getpoints.m +++ /dev/null @@ -1,12 +0,0 @@ -function p=getpoints(s,index) -%STRUCTURE/GETPOINTS getpoints(s,index) returns s's points with index -% in homog. coordinates. - -if nargin==1, - p=s.points; -else - p=s.points(:,index); -end - - - diff --git a/visionary/@structure/getquadrics.m b/visionary/@structure/getquadrics.m deleted file mode 100644 index 3aded53..0000000 --- a/visionary/@structure/getquadrics.m +++ /dev/null @@ -1,12 +0,0 @@ -function q=getquadrics(s,index) -%STRUCTURE/GETQUADRICS getquadrics(s,index) returns s's quadrics with index -% in dual coordinates. - -if nargin==1, - q=s.quadrics; -else - q=s.quadrics(:,index); -end - - - diff --git a/visionary/@structure/plot.m b/visionary/@structure/plot.m deleted file mode 100644 index 26dfc48..0000000 --- a/visionary/@structure/plot.m +++ /dev/null @@ -1,46 +0,0 @@ -function plot(s,p,l) -%STRUCTURE/PLOT plot 3D structure -% plot(s,p,l) -% INPUT: -% s - structure -% p - point style (optional) -% l - line style (optional) - - -if nargin<=1 | isempty(p), - p = '*'; -end -if nargin<=2 | isempty(l), - l = '-'; -end - -holdmode=ishold; - -if holdmode == 0, - clf; - axis([0 1 0 1 0 1]); -end - -if size(s.points,2)>0, - plot3( (s.points(1,:)./s.points(4,:))',... - (s.points(2,:)./s.points(4,:))',... - (s.points(3,:)./s.points(4,:))',p); - hold on; -end -if size(s.quadrics,2)>0, - plotquadric(s.quadrics); - hold on; -end -if size(s.lines,2)>0, - plotline(s.lines,l); - hold on; -end -if holdmode == 0 - hold off; -end - - - - - - diff --git a/visionary/@structure/plus.m b/visionary/@structure/plus.m deleted file mode 100644 index cd3acf7..0000000 --- a/visionary/@structure/plus.m +++ /dev/null @@ -1,9 +0,0 @@ -function s=plus(s1,s2) -%STRUCTURE/PLUS s=plus(s1,s2) adds s2's features to s1 -s = structure(s1); -s2 = structure(s2); - -s.points = [s.points,getpoints(s2)]; -s.lines = [s.lines,getlines(s2)]; -s.quadrics = [s.quadrics,getquadrics(s2)]; - diff --git a/visionary/@structure/size.m b/visionary/@structure/size.m deleted file mode 100644 index a628772..0000000 --- a/visionary/@structure/size.m +++ /dev/null @@ -1,8 +0,0 @@ -function sz = size(s,index) -%STRUCTURE/SIZE sz = size(s,index) returns nbr of pts, lines, quadrics - -sz=[size(s.points,2),size(s.lines,2),size(s.quadrics,2)]; - -if nargin==2, - sz = sz(index); -end diff --git a/visionary/@structure/structure.m b/visionary/@structure/structure.m deleted file mode 100644 index cda6565..0000000 --- a/visionary/@structure/structure.m +++ /dev/null @@ -1,45 +0,0 @@ -function s = structure(points,lines,quadrics) -%STRUCTURE class constructor -% s=STRUCTURE(points,lines,quadrics) creates a STRUCTURE object where -% points : 4xn (or 3xn) matrix where columns are homogeneous -% (or cartesian) 3D point coordinates -% lines : 8xm (or 6xm) matrix where columns are two homogeneous 3D points -% on the line (or columns are plucker coordinates) -% quadrics : 10xp matrix where columns are dual quadric coordinates -% All of the input arguments are optional - -if nargin == 1 & isa(points,'structure'), - s=points; -else - if nargin >= 1, - if size(points,1)==3 - s.points=[points;ones(1,size(points,2))]; - else - s.points = points; - end - else - s.points = zeros(4,0); - end - if nargin >= 2 & ~isempty(lines), - if size(lines,1)==8, - s.lines = lines; - elseif size(lines,1)==6, %plucker - zz=zeros(1,size(lines,2)); - s.lines=[zz;lines(1,:);lines(2,:);lines(3,:);... - -lines(1,:);zz;lines(6,:);-lines(5,:)]; - else - error('Unknown line format'); - end - else - s.lines = zeros(8,0); - end - if nargin == 3, - s.quadrics = quadrics; - else - s.quadrics = zeros(10,0); - end - - s = class(s,'structure'); - -end - diff --git a/visionary/Contents.m b/visionary/Contents.m deleted file mode 100644 index ffb2a39..0000000 --- a/visionary/Contents.m +++ /dev/null @@ -1,130 +0,0 @@ -% VISIONARY - A computer vision toolbox. -% -% 0. Help on datastructures & classes -% datastructures.m - Explanation of notations, classes etc. -% -% 1. Intersection or structure from motion is the problem -% of calculating scene structure when the camera motion is known -% intseclinear - points, lines, quadrics with linear method -% intsecpoints - points, linear method -% intsec2views - optimal twoview point method (Hartley/Sturm) -% intseclines - lines, linear method -% intsecconics - conics/quadrics, linear method (three or more views) -% -% 2. Resection or absolute orientation or motion from structure -% is the problem of calculating camera position orientation -% and internal calibration when something is known about -% the structure in the scene. -% a) Resection: Calibrated. -% resec3pts - resection using 3 points (4 solutions) -% -% b) Resection: Uncalibrated. -% reseclinear - resection, linear method using points -% reseccam3 - given two cameras, calculate third camera using points -% -% 3. Structure and motion or relative orientation is the -% problem of calculating both scene structure and -% the relative positions of the cameras at the same time. -% a) Assuming known interior calibration: -% -% b) Assuming unknown and possibly varying interior calibration. -% smshape - shape based factorisation method for points -% smaffine - affine factorisation for points, lines and conics -% smaffineclos - affine reconstruction with closure constraints -% smeucl8p - euclidean reconstruction with 8 point algorithm -% smproj8p - projective reconstruction with 8 point algorithm -% focallength6pt - calculates the motion of 2 views with an unknown common -% focallength using the 6 point algorithm. -% smfocallengths - semicalibrated reconstruction with two unknown focallengths -% ransac2views - calculates the motion of 2 views using RANSAC -% ransac3views - calculates the motion of 3 views using RANSAC -% ransacfocallength - calculates the motion of 2 views with an -% unknown common focallength using RANSAC and the 6pt mincase. -% sm6points - calculates structure & motion for 6 points in 3 views -% calibrated_fivepoint - 5 point 10 solutions minimal case solver for 2 views -% -% 4. Bundle adjustment consists of routines for optimising both -% structure and motion with respect to image measurements. -% Bundle: -% bundleplc - bundle adjustment for points, lines and conics -% bundle adjustment for patches coming soon!!! -% -% 5. Selfcalibration or autocalibration is the -% problem of determining the interior calibration of -% the camera assuming some knowledge, e.g. that -% the intrinsic parameters are constant. -% flexlinear - Flexible calibration a la Pollefeys -% flexnonlinear - Flexible calibration optimizing the absolute quadric -% bundleplc - auto & flexible calibration in bundle adjustment -% -% -% 6. Multilinear geometry. -% trifocallinear - Linear estimate of the trifocal tensor -% tritop - trifocal to motion object -% ptotri - motion object to trifocal -% bitop - bifocal(fundamental matrix) to motion object -% ptobi - motion object to bifocal -% invessentialm - Essential matrix to t, R1, R2 -% -% Matrix generation and manipulation. -% m2v - transform symmetric matrix to vector form. -% v2m - transform vector to symmetric matrix form. -% rq - rq-factorisation -% -% Image Processing routines. -% harris - Harris corner detector -% harrispro - Harris corner detector + "semilocal" autocorrelation -% findpatch - find patch using affine correlation -% manline - extract a line from an image -% manconic - extract a conic from an image -% manpolygon - extract a polygon from an image (lines+points) -% -% Construct simulated data. -% randomscene - create random scene and images -% -% Quaternions and rotations. -% rot2quat - Rotation matrix to unit quaternion -% quat2rot - Unit quaternion to rotation matrix -% randrot - create a random rotation matrix. -% -% Visualize -% visualize_export - export a 3D reconstruction to our visualizer -% visualize_import - import a 3D reconstruction from our visualizer -% test_visualize - test script for exporting to visualize -% -% Snavely's bundler. -% bundler_import - import Noah Snavely files to visionary -% test_bundler - test script for importing bundler files -% imdist_radial - distort image points with 2-parameter radial model -% imundist_radial - undistort image points with 2-parameter radial model -% -% General routines. -% pflat - normalise homogeneous coordinates (Z=1) -% psphere - normalise homogeneous coordinates (sphere) -% pextend - extend to homogeneous coordinates (add ones) -% skew - return skew matrix from a vector -% coniccamera - calculate 6x10 conic projection matrix -% findproj - calculate projective transformation using points -% findaff - calculate affine transformation using points -% findeucl - calculate euclidean transformation using points -% project - project structure with motion to imagedata -% reproject - compare projected structure with imagedata -% rmspoints - calculate RMS for reprojected points -% rmslines - calculate RMS for reprojected lines -% plotline - plot 2D/3D lines -% plotconic - plot 2D conics -% plotquadric - plot 3D quadrics -% fitline - fit a line to point set with covariances -% fitconic - fit a conic to point set with convariances -% -% Local routines. -% ginput2 - alternative to Matlab's ginput -% -% Demo, see directory 'demo/'. -% -% Mathematical Imaging Group at the department of mathematics, -% Lund University, Lund, Sweden. -% - - - diff --git a/visionary/affineoptimize.m b/visionary/affineoptimize.m deleted file mode 100644 index 355468b..0000000 --- a/visionary/affineoptimize.m +++ /dev/null @@ -1,199 +0,0 @@ -function [pos2,residual]=affineoptimize(im1, im2, startpos, destpos, patchsize, stdev, Astart,maxsteps); -%local patch optimization over affine transformations -% with some regularity (first 2x2 submatrix should approximately be eye(2)) -if nargin<7, - Astart=eye(2); -end -if nargin<8, - maxsteps=30; -end -startpos=[startpos(1);startpos(2);1]; -destpos=[destpos(1);destpos(2);1]; - -borderignore=round(3*stdev+2*patchsize+6); -[imsz1,imsz2]=size(im2); - -%%%%%%%%%%%%%%%%%%%%%%%%%%% -%calculate patchpositions -%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -cnt=1; -for x=-patchsize:patchsize, - for y=-patchsize:patchsize, - patchpos(1:3,cnt)=[startpos(1)+x,startpos(2)+y 1]'; - cnt=cnt+1; - end; -end; - -%%%%%%%%%%%%%%%%%%%%%%%%%%% -%calculate start transformation -%%%%%%%%%%%%%%%%%%%%%%%%%%% -delta = destpos-startpos; -A = eye(3); -A(1,3)=delta(1); -A(2,3)=delta(2); -A00 = [1,0,-startpos(1);0,1,-startpos(2);0,0,1]; -u1m = std((A00*patchpos)'); -A0 = [ 1/u1m(1) 0 0 ; 0 1/u1m(2) 0 ; 0 0 1]*A00; -A=A0*A*inv(A0); -A(1:2,1:2)=Astart; - -gscale=1; -detA=det(A(1:2,1:2)); -i1 = measure(im1,patchpos,stdev); - - -[i2, i2grad] = measure(im2,inv(A0)*A*A0*patchpos,stdev); -[f0, Idiff, Idiffgrad] = affineres(i1,i2,i2grad,A,A0,patchpos,detA,gscale); - -dA=inf;f1=inf; -counter=0; -while log(norm(dA)+eps)/log(10)>-6 & counter=borderignore & pos2(1)<=imsz2-borderignore & ... - pos2(2)>=borderignore & pos2(2)<=imsz1-borderignore, - [i2, i2grad] = measure(im2,Aopt*patchpos,stdev); - [f1, Idiff, Idiffgrad] = affineres(i1,i2,i2grad,Anew,A0,patchpos,detA,gscalenew); - else - counter=maxsteps; - end - - while counterf0, - - if pos2(1)imsz2-borderignore | ... - pos2(2)imsz1-borderignore, - - counter=maxsteps; - else - dA=dA/2; -% f0,f1,disp('Det har tar vi en gang till'); - - Anew=A+reshape([dA(1:6) ; 0 ; 0 ; 0],3,3)';gscalenew=gscale+dA(7); - Aopt=inv(A0)*Anew*A0; pos2=Aopt*startpos; - [i2, i2grad] = measure(im2,Aopt*patchpos,stdev); - [f1, Idiff, Idiffgrad] = affineres(i1,i2,i2grad,Anew,A0,patchpos,detA,gscalenew); - counter=counter+1; - end - end; - - if f1<=f0, - f0=f1; - A = Anew;gscale=gscalenew; - end; - counter=counter+1; -end; - -residual=sqrt(f0)/(2*patchsize+1); - - -%%%end of affineoptimize - - -function [f,intendiff, intendiffgrad]=... - affineres(i1,i2,i2grad,A,A0,patchpos,detA,gscale); - -intendiff = (i2*gscale-i1)'; - -M = eye([9 9]); -d1 = inv(A0)*reshape(M(1,1:9),3,3)'*A0; -d2 = inv(A0)*reshape(M(2,1:9),3,3)'*A0; -d3 = inv(A0)*reshape(M(3,1:9),3,3)'*A0; -d4 = inv(A0)*reshape(M(4,1:9),3,3)'*A0; -d5 = inv(A0)*reshape(M(5,1:9),3,3)'*A0; -d6 = inv(A0)*reshape(M(6,1:9),3,3)'*A0; - -dAx1 = d1 * patchpos; -dAx2 = d2 * patchpos; -dAx3 = d3 * patchpos; -dAx4 = d4 * patchpos; -dAx5 = d5 * patchpos; -dAx6 = d6 * patchpos; - -dfel1 = sum(i2grad .* dAx1(1:2,:)); -dfel2 = sum(i2grad .* dAx2(1:2,:)); -dfel3 = sum(i2grad .* dAx3(1:2,:)); -dfel4 = sum(i2grad .* dAx4(1:2,:)); -dfel5 = sum(i2grad .* dAx5(1:2,:)); -dfel6 = sum(i2grad .* dAx6(1:2,:)); - -dfel7 = i2; - - -intendiffgrad = [ dfel1 ; dfel2 ; dfel3 ; dfel4 ; dfel5 ; dfel6 ; dfel7]'; - - -% add regularization det(A(1:2,1:2))=1 - -weight=100; -intendiff=[intendiff;weight*[A(1,1)-1;A(1,2);A(2,1);A(2,2)-1;gscale-1]]; -intendiffgrad=[intendiffgrad;... - weight*[1,0,0,0,0,0,0];... - weight*[0,1,0,0,0,0,0];... - weight*[0,0,0,1,0,0,0];... - weight*[0,0,0,0,1,0,0];... - weight*[0,0,0,0,0,0,1]]; - -f = intendiff'*intendiff; - - - -function [inten, intengrad]=measure(image,points,a); - -nbr=size(points,2); - -inten=zeros(1,nbr); -intengrad=zeros(2,nbr); - -NN=round(a*3); - - -for i=1:nbr, - - -%%%%%%%% [In, Igrad] = measurepoint(image,points(1:2,i),a); -%measurepoint -%function [inten, intengrad]=measurepoint(bild,punkt,a); - - -x0=points(1,i); -y0=points(2,i); - - -cutx=(round(x0)-NN):(round(x0)+NN); -cuty=(round(y0)-NN):(round(y0)+NN); -%cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -%cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -cutbild=image(cuty,cutx); - -[x,y]=meshgrid(cutx,cuty); - -filter=exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^2*pi); -filtx=2*(x-x0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -filty=2*(y-y0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); - -Igrad(1,1)=sum(sum(filtx.*cutbild)); -Igrad(2,1)=sum(sum(filty.*cutbild)); -In=sum(sum(filter.*cutbild)); - -%measurepoint -%%%%%%%%%%%%%%%%%%%% - - -inten(i) = In; -intengrad(1:2,i) = Igrad; - - - -end - - - - - - diff --git a/visionary/bitop.m b/visionary/bitop.m deleted file mode 100644 index b54c776..0000000 --- a/visionary/bitop.m +++ /dev/null @@ -1,15 +0,0 @@ -function m=bitop(F); -% function m=bitop(F); -% Determines two possible camera matrices from the fundamental matrix F -% -% Input: F=fundamental matrix -% Output: [P1,P2]=two camera matrices normalised such that -% P1=[I 0] P2(1,:)=[0 0 0 1] -% The 2 cameras are returned as MOTION m - -[E12,E21]=bitoone(F); -E=E12./E12(1); -P1=[1 0 0 0;0 1 0 0;0 0 1 0]; -P2=[0 0 0 1;F(1,3) F(2,3) F(3,3) E(2);-F(1,2) -F(2,2) -F(3,2) E(3)]; - -m=motion({P1,P2}); diff --git a/visionary/bundleAdjust/bundleAdjust.m b/visionary/bundleAdjust/bundleAdjust.m deleted file mode 100644 index 3893d2e..0000000 --- a/visionary/bundleAdjust/bundleAdjust.m +++ /dev/null @@ -1,283 +0,0 @@ -function [P, U, res] = bundleAdjust(P, varargin) - -parser = inputParser; -parser.FunctionName = mfilename; -parser.StructExpand = false; - -defaultu.pointnr = 0; -defaultu.points = {}; -defaultu.index = {}; - -addRequired(parser, 'P', @isCameraMatrixCellArray); -addOptional(parser, 'U', zeros(4,0), @(x) isreal(x) && 4==size(x,1)); -addOptional(parser, 'tracks', defaultu, @(x) isPointTracks(P,x)); -addOptional(parser, 'iterations', 20, @(x) isnumeric(x) && isscalar(x) && x >= 0); -addOptional(parser, 'lambda', 0.001, @(x) isnumeric(x) && isscalar(x) && x > 0); -addParameter(parser, 'relPoses', [], @(x) isRelPoseArray(P,x)); -addParameter(parser, 'absPoses', [], @(x) isAbsPoseArray(P,x)); -addParameter(parser, 'absPos', [], @(x) isreal(x) && size(x,1) == 3); -addParameter(parser, 'fixedCams', [], @(x) isCamArray(P,x)); -addParameter(parser, 'fixedPoint', false, @(x) islogical(x) && isscalar(x)); -addParameter(parser, 'fixAllPoints', false, @(x) islogical(x) && isscalar(x)); -addParameter(parser, 'minLambda', 1e-6, @(x) isnumeric(x) && isscalar(x) && x >= 0); -addParameter(parser, 'maxLambda', 10, @(x) isnumeric(x) && isscalar(x) && x > 0); -addParameter(parser, 'minResChange', 1e-4, @(x) isnumeric(x) && isscalar(x) && x >= 0); -addParameter(parser, 'verbosity', 0, @(x) isaninteger(x,0,2) && isscalar(x)); -addParameter(parser, 'schur', true, @(x) islogical(x) && isscalar(x)); -addParameter(parser, 'camWeight', 1/0.1, @(x) isnumeric(x) && isscalar(x) && x >= 0); -addParameter(parser, 'posWeight', 1e-3, @(x) isnumeric(x) && isscalar(x) && x >= 0); - -parse(parser,P,varargin{:}); -parm = parser.Results; - -%Further parameter checks -assert(size(parm.U,2) == parm.tracks.pointnr, ... - 'Number of 3d points must match between U and tracks'); - -assert(~parm.fixedPoint || parm.tracks.pointnr > 0, ... - 'There are no points to fix'); - -assert(~parm.fixAllPoints || parm.tracks.pointnr > 0, ... - 'There are no points to fix'); - -%Using Schur complement to eliminate points first only makes sense if there -%are any points -parm.schur = parm.schur && parm.tracks.pointnr > 0 && ~parm.fixAllPoints; -useProgressBar = exist('progressBar', 'file') > 0 && parm.verbosity==1; - -%There is a scheme for lambda where it decreases from the initial value -%by dividing by 1.5 every iteration -lambda = min(parm.maxLambda,parm.lambda); -nCams = length(P); -nPoints = parm.tracks.pointnr; - -for i = 1:nCams - [KKK,R] = rq(P{i}(:,1:3)); - KK{i} = KKK; - P{i} = KKK\P{i}; - if nPoints > 0 - parm.tracks.points{i} = pflat(KKK\parm.tracks.points{i}); - end -end -parm.P = P; - -Unew = parm.U; -Pnew = parm.P; -if parm.verbosity>=2 - fprintf('#\tResidual\tLambda\n') -end -%Setup and solve linear system and refine the linearization iteratively -for i = 1:parm.iterations - %Setup a linear system (linearized in the current guess of P and U) - [B,A] = setupLinSystem(parm); - - res = B'*B; - if i==1 && parm.verbosity>=2 - fprintf('%d\t%f\t%f',0,res,lambda) - end - resnew = Inf; - %If lambda is very small, step size may become too large and go - %outside the "trust region" so increase lambda until the new residual - %is smaller than the old. - while resnew > res - %If lambda is very large, step size will be very small and almost - %no progress towards the minumum will be made. - if lambda > parm.maxLambda - if parm.verbosity>=2 - fprintf('\nLambda has grown too large, exiting\n'); - end - %Assign return parameters - P = parm.P; - U = parm.U; - for k = 1:length(P) - P{k} = KK{k}*P{k}; - end - return - end - if ~parm.schur - %Marquardt update - C = A'*A; - ATA_diag = spdiags(sqrt(abs(diag(C))),0,sparse(size(A,2),size(A,2))); - C = (C+lambda*ATA_diag); - c = A'*B; - if parm.verbosity>=2 - fprintf('\t\tSolving mod-Newton system.\n'); - end - d = -C\c; - else - % Eliminate points first - if parm.fixedPoint - AU = A(:,1:3*nPoints-1); - AP = A(:,3*nPoints:end); - else - AU = A(:,1:3*nPoints); - AP = A(:,(3*nPoints+1):end); - end - UU = AU'*AU + lambda*speye(size(AU,2)); - invUU = invertUU(UU,parm.fixedPoint); - slask = AU'*AP; - slask = invUU*slask; - slask2 = AP'*AU; - slask = slask2*slask; - C = AP'*AP - slask; - if size(C,2) == 0 - ATA_diag = 0; - else - ATA_diag = spdiags(sqrt(abs(diag(C))),0,sparse(size(C,2),size(C,2))); - end - C = (C+lambda*ATA_diag); - slask = AU'*B; - slask = invUU*slask; - slask2 = AP'*AU; - c = slask2*slask - AP'*B; - if parm.verbosity>=2 - fprintf('\t\tSolving mod-Newton system.\n') - end - dR = C\c; - dU = -invUU*(AU'*(AP*dR+B)); - d = [dU; dR]; - end - [dpointvar, dcamvar] = restoreFixedVars(d, nPoints, nCams, ... - parm.fixedCams, parm.fixedPoint, parm.fixAllPoints); - [Pnew,Unew] = update_var(dpointvar, dcamvar, parm.P, parm.U); - r = calcResidual(Pnew,Unew,parm); - resnew = r'*r; - if resnew > res - lambda = lambda*2; - if parm.verbosity>=2 - fprintf('\t%f\t%f', resnew, lambda) - end - end - end - if lambda > parm.minLambda - lambda = max(parm.minLambda,lambda/1.5); - end - parm.U = Unew; - parm.P = Pnew; - if parm.verbosity>=2 - fprintf('%d\t%f\t%f',i,resnew,lambda) - end - if useProgressBar - progressBar(100*i/iter); - end - %Check that residual shrinks "fast enough" - if (res-resnew)/resnew < parm.minResChange - if parm.verbosity>=2 - fprintf('\nResidual is not decreasing fast enough, exiting\n'); - end - break - end -end - -%Assign return parameters -P = parm.P; -U = parm.U; -for k = 1:length(P) - P{k} = KK{k}*P{k}; -end - - if parm.verbosity>=2 - fprintf('\nDone.\n') - end -end - -function tf = isCameraMatrixCellArray(arg) - tf = iscell(arg) && all(2 == cellfun('ndims', arg)) && ... - all(cellfun('isreal', arg)) && ... - all(3==cellfun(@(x) size(x,1), arg)) && ... - all(12==cellfun(@numel, arg)); -end - -function tf = isRelPoseArray(P,arg) - N = numel(P); - j = [arg.j]; - tf = isAbsPoseArray(P,arg) && isfield(arg, 'j') && all(isaninteger(j,1,N)); -end - -function tf = isPointTracks(P,arg) - N = numel(P); - tf = isstruct(arg) && isfield(arg, 'pointnr') && ... - isfield(arg, 'points') && isfield(arg, 'index') && ... - numel(arg.points) == N && numel(arg.index) == N && ... - all(cellfun(@(x) all(isaninteger(x,1,arg.pointnr)), arg.index)); -end - -function tf = isAbsPoseArray(P,arg) - N = numel(P); - i = [arg.i]; - tf = isstruct(arg) && isfield(arg, 'i') && ... - isfield(arg, 'M') && isfield(arg, 'w') && all(isaninteger(i,1,N)); -end - -function tf = isCamArray(P,arg) - N = length(P); - tf = all(isaninteger(arg,1,N)); -end - -function tf = isaninteger(x,low,high) - tf = isnumeric(x) & x >= low & ... - x <= high & round(x) == x; -end - -function B = calcResidual(Pnew,Unew,parms) - nPoints = parms.tracks.pointnr; - B = []; - if ~isempty(Unew) - B = setupLinSystemCamera(Pnew,Unew,parms.tracks,... - parms.camWeight,B); - end - if ~isempty(parms.absPos) - B = setupLinSystemAbsPos(Pnew,parms.absPos,... - nPoints,parms.posWeight,B); - end - if ~isempty(parms.relPoses) - B = setupLinSystemRelCam(Pnew,parms.relPoses, ... - nPoints, B); - end - if ~isempty(parms.absPoses) - B = setupLinSystemAbsCam(Pnew,parms.absPoses, ... - nPoints, B); - end -end - -function [B, A] = setupLinSystem(parms) - P = parms.P; - nCams = length(P); - nPoints = parms.tracks.pointnr; - nVars = 3*nPoints + 6*nCams; - A = sparse(0,nVars); - B = []; - if ~isempty(parms.U) - [B,A] = setupLinSystemCamera(P,parms.U,parms.tracks,... - parms.camWeight,B,A); - end - if ~isempty(parms.absPos) - [B,A] = setupLinSystemAbsPos(P,parms.absPos,... - nPoints,parms.posWeight,B,A); - end - if ~isempty(parms.relPoses) - [B,A] = setupLinSystemRelCam(P,parms.relPoses, ... - nPoints, B, A); - end - if ~isempty(parms.absPoses) - [B,A] = setupLinSystemAbsCam(P,parms.absPoses, ... - nPoints, B, A); - end - - %Remove columns corresponding to fixed variables - %First the fixed cameras - camindex = parms.fixedCams; - if ~isempty(camindex) - index = setdiff(1:length(P),camindex); - keepcols = [(index-1)*6+1;(index-1)*6+2;(index-1)*6+3;(index-1)*6+4;(index-1)*6+5;(index-1)*6+6]; - keepcols = keepcols(:)'; - A = A(:,[1:3*nPoints (3*nPoints+keepcols)]); - end - %Fix points (first or all) - if parms.fixAllPoints - A = A(:,nPoints*3+1:end); - elseif parms.fixedPoint - A = A(:,2:end); - end -end - diff --git a/visionary/bundleAdjust/computeResCam.m b/visionary/bundleAdjust/computeResCam.m deleted file mode 100644 index 759f453..0000000 --- a/visionary/bundleAdjust/computeResCam.m +++ /dev/null @@ -1,17 +0,0 @@ -function res = computeResCam(P,U,u) -nRes = 0; -for i = 1:length(P) - nRes = nRes + length(u.index); -end -res = zeros(nRes,1); -iRes = 0; -for i = 1:length(P) - uu = NaN*ones(3,u.pointnr); - uu(:,u.index{i}) = pflat(u.points{i}); - vis = isfinite(uu(1,:)); - r = [ ... - ((P{i}(1,:)*U(:,vis))./(P{i}(3,:)*U(:,vis)) - uu(1,vis)); ... - ((P{i}(2,:)*U(:,vis))./(P{i}(3,:)*U(:,vis)) - uu(2,vis))]; - res(iRes+(1:numel(r))) = r(:); - iRes = iRes + numel(r); -end diff --git a/visionary/bundleAdjust/computeResPos.m b/visionary/bundleAdjust/computeResPos.m deleted file mode 100644 index 6ad7b67..0000000 --- a/visionary/bundleAdjust/computeResPos.m +++ /dev/null @@ -1,10 +0,0 @@ -function res = computeResPos(P,C_p) -nRes = length(P)*3; -res = zeros(nRes,1); -iRes = 0; -for i = 1:length(P) - if all(isfinite(C_p(:,i))) - res(iRes+(1:3)) = P{i}*[C_p(:,i); 1]; - iRes = iRes + 3; - end -end diff --git a/visionary/bundleAdjust/computeResRot.m b/visionary/bundleAdjust/computeResRot.m deleted file mode 100644 index 16c72e9..0000000 --- a/visionary/bundleAdjust/computeResRot.m +++ /dev/null @@ -1,8 +0,0 @@ -function res = computeResRot(P,R_p) -res = zeros(9*length(P),1); -for i = 1:length(P) - if ~isempty(R_p{i}) - dR = P{i}(1:3,1:3) - R_p{i}; - res((i-1)*9+(1:9)) = dR(:); - end -end \ No newline at end of file diff --git a/visionary/bundleAdjust/invertUU.m b/visionary/bundleAdjust/invertUU.m deleted file mode 100644 index bb48809..0000000 --- a/visionary/bundleAdjust/invertUU.m +++ /dev/null @@ -1,44 +0,0 @@ -function invUU = invertUU(UU,fixedPoint) -if nargin < 2 - fixedPoint = false; -end -% if fixedPoint is true, the first coordinate of the first point is fixed -% by omitting it in the variables. Replace so that all blocks are 3x3 -if fixedPoint - [i,j,data] = find(UU); - UU = sparse(i+1,j+1,data); - UU(1,1) = 1; -end - -%pick diagonal blocks -[i,j,data] = find(UU); -%much faster if I use full matrices here -A = full(sparse(i,mod(j-1,3)+1,data)); -invUU = speye(size(UU,1)); -[i,j,data] = find(invUU); -invA = full(sparse(i,mod(j-1,3)+1,data)); - -%Gauss elimination downwards -for i = 1:3 - %div by diagonal element - invA(i:3:end,:) = invA(i:3:end,:)./repmat(A(i:3:end,i),[1 3]); - A(i:3:end,:) = A(i:3:end,:)./repmat(A(i:3:end,i),[1 3]); - for j = i+1:3 - %subtract element - invA(j:3:end,:) = invA(j:3:end,:) - repmat(A(j:3:end,i),[1 3]).*invA(i:3:end,:); - A(j:3:end,:) = A(j:3:end,:) - repmat(A(j:3:end,i),[1 3]).*A(i:3:end,:); - end -end -%backsubstitution -for i = 3:-1:2 - for j = i-1:-1:1 - invA(j:3:end,:) = invA(j:3:end,:) - repmat(A(j:3:end,i),[1 3]).*invA(i:3:end,:); - A(j:3:end,:) = A(j:3:end,:) - repmat(A(j:3:end,i),[1 3]).*A(i:3:end,:); - end -end -[i,j,data] = find(invA); -if fixedPoint - invUU = sparse(i(2:end)-1,j(2:end)+(ceil(i(2:end)/3)-1)*3-1,data(2:end)); -else - invUU = sparse(i,j+(ceil(i/3)-1)*3,data); -end diff --git a/visionary/bundleAdjust/restoreFixedVars.m b/visionary/bundleAdjust/restoreFixedVars.m deleted file mode 100644 index 5d84b90..0000000 --- a/visionary/bundleAdjust/restoreFixedVars.m +++ /dev/null @@ -1,28 +0,0 @@ -function [dpointvar, dcamvar] = restoreFixedVars(... - d,nPoints,nCams, fixedCams, fixedPoint, fixAllPoints) - - firstCamI = 3*nPoints+1; - if fixAllPoints - dpointvar = zeros(3*nPoints,1); - firstCamI = 1; - elseif fixedPoint - dpointvar = [0; d(1:(3*nPoints-1))]; - firstCamI = firstCamI-1; - else - dpointvar = d(1:(3*nPoints)); - end - - if isempty(fixedCams) - dcamvar = d(firstCamI:end); - else - dcamvar = zeros(nCams*6,1); - index = setdiff(1:nCams,fixedCams); - keepcols = [(index-1)*6+1;(index-1)*6+2;(index-1)*6+3;... - (index-1)*6+4;(index-1)*6+5;(index-1)*6+6]; - keepcols = keepcols(:)'; - dcamvar(keepcols) = d(firstCamI:end); - end - - dpointvar = reshape(dpointvar, [3 nPoints]); - dcamvar = reshape(dcamvar,[6 nCams]); -end \ No newline at end of file diff --git a/visionary/bundleAdjust/rq.m b/visionary/bundleAdjust/rq.m deleted file mode 100644 index e1ade84..0000000 --- a/visionary/bundleAdjust/rq.m +++ /dev/null @@ -1,20 +0,0 @@ -function [r,q]=rq(a) -% RQ [r,q]=rq(a) factorises a such a=rq where r upper tri. and q unit matrix -% If a is not square, then q is equal q=[q1 q2] where q1 is unit matrix - -[m,n]=size(a); -e=eye(m); -p=e(:,[m:-1:1]); -[q0,r0]=qr(p*a(:,1:m)'*p); - -r=p*r0'*p; -q=p*q0'*p; - -fix=diag(sign(diag(r))); -r=r*fix; -q=fix*q; - - -if n>m - q=[q, inv(r)*a(:,m+1:n)]; -end diff --git a/visionary/bundleAdjust/setupLinSystemAbsCam.m b/visionary/bundleAdjust/setupLinSystemAbsCam.m deleted file mode 100644 index 98b6b72..0000000 --- a/visionary/bundleAdjust/setupLinSystemAbsCam.m +++ /dev/null @@ -1,88 +0,0 @@ -function [res, J] = setupLinSystemAbsCam(P, relPoses, nPoints, res, J) -%w is square root of the inverse of the covariance matrix for -%[x, y, z, a, b, c]^T - - - -N = length(relPoses); -m = 3+9; -resAdd = zeros(m*N,1); - -for i = 1:N - I = [P{relPoses(i).i}; 0 0 0 1] / relPoses(i).M; -% I = relPoses(i).M \ ... -% [P{relPoses(i).i}; 0 0 0 1]; - resAdd((i-1)*m+(1:3)) = I(1:3,4).*reshape(relPoses(i).w(1:3),3,1); - eAngle = (I(1:3,1:3)-eye(3)); - resAdd((i-1)*m+(4:12)) = eAngle(:)*mean(relPoses(i).w(4:end)); -end -res = [res; resAdd]; - -if nargout >= 2 - %Need to output Jacobian as well - nVars = size(J,2); - nCams = numel(P); - assert(nVars == nPoints*3 + nCams*6); - - %Rotation manifold bases - Bs(:,:,1) = [0 1 0; -1 0 0; 0 0 0]; - Bs(:,:,2) = [0 0 1; 0 0 0; -1 0 0]; - Bs(:,:,3) = [0 0 0; 0 0 1; 0 -1 0]; - - - %Analytical Jacobian - col = []; - row = []; - data = []; - - for k = 1:N - %Shorter variable names - i = relPoses(k).i; - Ri = P{i}(1:3,1:3); - tm = relPoses(k).M(1:3,4); - Rm = relPoses(k).M(1:3,1:3); - wR = mean(relPoses(k).w(4:end)); - wt = reshape(relPoses(k).w(1:3),3,1); - - %dRotation/d(a,b,c) - dRRi = zeros(3,3,3); - for l = 1:3 - dRRi(:,:,l) = (Bs(:,:,l)*Ri)*Rm'; - end - dRRi = wR.*dRRi; - %rows and cols - rowsRR = repmat((k-1)*m+(4:12)',3,1); - colsi = 3*nPoints+(i-1)*6+(1:3); - colsRR = repmat(colsi,9,1); - - %dTranslation/d(a,b,c) - dtRi = zeros(3,3); - for l = 1:3 - dtRi(:,l) = -(Bs(:,:,l)*Ri)*Rm'*tm; - end - dtRi = wt.*dtRi; - %rows and cols - rowstR = repmat((k-1)*m+(1:3)',3,1); - colsi = 3*nPoints+(i-1)*6+(1:3); - colstR = repmat(colsi,3,1); - - %dTranslation/d(t1,t2,t3) - dtti = wt.*eye(3); - %rows and cols - rowstt = repmat((k-1)*m+(1:3)',3,1); - colsi = 3*nPoints+(i-1)*6+(4:6); - colstt = repmat(colsi,3,1); - - %Append to row/col/data - row = [row; rowsRR; rowstR; rowstt]; - col = [col; colsRR(:); colstR(:); colstt(:)]; - data = [data; dRRi(:); dtRi(:); dtti(:)]; - - end - ix = abs(data) > 1e-10; - J = [J; sparse(row(ix),col(ix),data(ix),N*m, nVars)]; - -end - - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/setupLinSystemAbsPos.m b/visionary/bundleAdjust/setupLinSystemAbsPos.m deleted file mode 100644 index 2fd357c..0000000 --- a/visionary/bundleAdjust/setupLinSystemAbsPos.m +++ /dev/null @@ -1,82 +0,0 @@ -function [res,J] = setupLinSystemAbsPos(P,C_p,numpts,w,res,J) - -calcJac = nargin >= 6; - -hasPosCondition = all(isfinite(C_p),1); - -if calcJac - %Bas f�r tangentplanet till rotationsm�ngfalden. - Ba = [0 1 0; -1 0 0; 0 0 0]; - Bb = [0 0 1; 0 0 0; -1 0 0]; - Bc = [0 0 0; 0 0 1; 0 -1 0]; - - - daC = cell(size(P)); - dbC = cell(size(P)); - dcC = cell(size(P)); - dt1C = cell(size(P)); - dt2C = cell(size(P)); - dt3C = cell(size(P)); - - - - for i=1:length(P) - %a,b,c - rotations parametrar f�r kamera i - %t1,t2,t3 - translations parametrar kameran. - R0 = P{i}(:,1:3); - - - if hasPosCondition(i) - daC{i} = Ba*R0*C_p(:,i); - dbC{i} = Bb*R0*C_p(:,i); - dcC{i} = Bc*R0*C_p(:,i); - dtC = eye(3); - dt1C{i} = dtC(:,1); - dt2C{i} = dtC(:,2); - dt3C{i} = dtC(:,3); - end - end - - if(size(res,1) ~= size(J,1)) - error('BA:constraint', ... - 'Jacobian and residual must have same number of rows'); - end -end - -nPosConditions = sum(hasPosCondition); -iConstraint = 0; -nConditions = 3*nPosConditions; -resAdd = zeros(nConditions,1); - -if calcJac - nVars = 3*numpts + 6*length(P); - nnzElems = 3*6*nPosConditions; - row = zeros(nnzElems,1); - col = zeros(nnzElems,1); - data = zeros(nnzElems,1); - lastentry = 0; -end - -for i = 1:length(P) - - if all(isfinite(C_p(:,i))) - if calcJac - row(lastentry+(1:3*6)) = iConstraint+repmat((1:3)',6,1); - col(lastentry+(1:3*6)) = 3*numpts+(i-1)*6+reshape(repmat(1:6,3,1),18,1); - data(lastentry+(1:3*6)) = [daC{i}; dbC{i}; dcC{i}; dt1C{i}; dt2C{i}; dt3C{i}]; - lastentry = lastentry+3*6; - end - - e = P{i}*[C_p(:,i);1]; - resAdd(iConstraint+(1:3)) = e; - iConstraint = iConstraint+3; - end - -end - -if calcJac - J = [J; w*sparse(row,col,data,nConditions, nVars)]; -end -res = [res; w*resAdd]; - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/setupLinSystemCam.m b/visionary/bundleAdjust/setupLinSystemCam.m deleted file mode 100644 index ab13420..0000000 --- a/visionary/bundleAdjust/setupLinSystemCam.m +++ /dev/null @@ -1,547 +0,0 @@ -function [J,res] = setupLinSystemCam(J,res,P,U,u,w) -%Append constraints relating to reprojection errors in the images -%to Jacobian A and residual vector B - -if nargin < 6 - w = 1; -end - -numpts = size(U,2); -%Bas f�r tangentplanet till rotationsm�ngfalden. -Ba = [0 1 0; -1 0 0; 0 0 0]; -Bb = [0 0 1; 0 0 0; -1 0 0]; -Bc = [0 0 0; 0 0 1; 0 -1 0]; - -da1 = cell(size(P)); -db1 = cell(size(P)); -dc1 = cell(size(P)); -dt11 = cell(size(P)); -dt21 = cell(size(P)); -dt31 = cell(size(P)); -dU11 = cell(size(P)); -dU21 = cell(size(P)); -dU31 = cell(size(P)); -da2 = cell(size(P)); -db2 = cell(size(P)); -dc2 = cell(size(P)); -dt12 = cell(size(P)); -dt22 = cell(size(P)); -dt32 = cell(size(P)); -dU12 = cell(size(P)); -dU22 = cell(size(P)); -dU32 = cell(size(P)); - - -U = pflat(U); -U = U(1:3,:); -nCamConditions = 0; -nRotConditions = 0; - -for i=1:length(P) - %ber�kna derivator f�r b�da residualeran i alla bilder - %a,b,c - rotations parametrar f�r kamera i - %U1,U2,U3 - 3d punkt parametrar - %t1,t2,t3 - translations parametrar kameran. - vis = u.index{i}; - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - - - da1{i} = (Ba(1,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Ba(3,:)*R0*U(:,vis)); - - da2{i} = (Ba(2,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Ba(3,:)*R0*U(:,vis)); - - db1{i} = (Bb(1,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bb(3,:)*R0*U(:,vis)); - - db2{i} = (Bb(2,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bb(3,:)*R0*U(:,vis)); - - dc1{i} = (Bc(1,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bc(3,:)*R0*U(:,vis)); - - dc2{i} = (Bc(2,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bc(3,:)*R0*U(:,vis)); - - dU11{i} = R0(1,1)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(1,:)*U(:,vis) + t0(1))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,1); - - dU12{i} = R0(2,1)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(2,:)*U(:,vis) + t0(2))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,1); - - dU21{i} = R0(1,2)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(1,:)*U(:,vis) + t0(1))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,2); - - dU22{i} = R0(2,2)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(2,:)*U(:,vis) + t0(2))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,2); - - dU31{i} = R0(1,3)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(1,:)*U(:,vis) + t0(1))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,3); - - dU32{i} = R0(2,3)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(2,:)*U(:,vis) + t0(2))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,3); - - dt11{i} = 1./(R0(3,:)*U(:,vis)+t0(3)); - dt12{i} = zeros(size(dt11{i})); - - dt21{i} = zeros(size(dt11{i})); - dt22{i} = 1./(R0(3,:)*U(:,vis)+t0(3)); - - dt31{i} = -(R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2); - dt32{i} = -(R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2); - - - if 0 - aa = 2*rand(1,1)-1; - bb = 2*rand(1,1)-1; - cc = 2*rand(1,1)-1; - UU1 = 2*rand(1,1)-1; - UU2 = 2*rand(1,1)-1; - UU3 = 2*rand(1,1)-1; - tt1 = 2*rand(1,1)-1; - tt2 = 2*rand(1,1)-1; - tt3 = 2*rand(1,1)-1; - - f = []; - vis = find(isfinite(u{i}(1,:))); - kk = ceil(rand*length(vis)); - kk = vis(kk); - f01 = (-u{i}(1,kk)+(R0(1,:)*U(:,kk)+ t0(1))./(R0(3,:)*U(:,kk)+t0(3))); - f02 = (-u{i}(2,kk)+(R0(2,:)*U(:,kk)+t0(2))./(R0(3,:)*U(:,kk)+ t0(3))); - - f2 = []; - for s = -0.1:0.001:0.1 - R = expm(s*(Ba*aa+Bb*bb+Bc*cc))*R0; - t = t0+s*[tt1;tt2;tt3]; - UU = U+s*repmat([UU1;UU2;UU3], [1 size(U,2)]); - f = [f; (u{i}(1,kk)-(R(1,:)*UU(:,kk)+t(1))./(R(3,:)*UU(:,kk)+ t(3)))^2 + ... - (u{i}(2,kk)-(R(2,:)*UU(:,kk)+t(2))./(R(3,:)*UU(:,kk)+ t(3)))^2]; - f2 = [f2; (f01 + s*(da1{i}(kk)*aa + db1{i}(kk)*bb + dc1{i}(kk)*cc + ... - dU11{i}(kk)*UU1 + dU21{i}(kk)*UU2 + dU31{i}(kk)*UU3 + ... - dt11{i}(kk)*tt1 + dt21{i}(kk)*tt2 + dt31{i}(kk)*tt3)).^2 + ... - (f02 + s*(da2{i}(kk)*aa + db2{i}(kk)*bb + dc2{i}(kk)*cc + ... - dU12{i}(kk)*UU1 + dU22{i}(kk)*UU2 + dU32{i}(kk)*UU3 + ... - dt12{i}(kk)*tt1 + dt22{i}(kk)*tt2 + dt32{i}(kk)*tt3)).^2]; - end - %figure(1);plot(-0.1:0.001:0.1,[f f2(end:-1:1)]); - figure(1);plot(-0.1:0.001:0.1,[f f2]); - end -end - - -iConstraint = size(J,1); -assert(size(res,1)==iConstraint, 'BA:constraint', ... - 'Jacobian and residual must have same number of rows'); - -%Allokera hela vektorn f�rst tv� residualer f�r varje bild punkt -nImgResiduals = 0; -for i = 1:length(P) - nImgResiduals = nImgResiduals + size(u.points{i},2); -end -nConditions = 2*nImgResiduals; -nnzElems = nConditions*(6+3); -row = zeros(nnzElems,1); -col = zeros(nnzElems,1); -data = zeros(nnzElems,1); -lastentry = size(J,1); -res = [res; zeros(nConditions,1)]; - -for i = 1:length(P) - %fprintf('%d ',i); - uu = NaN*ones(3,u.pointnr); - uu(:,u.index{i}) = pflat(u.points{i}); - vis = find(isfinite(uu(1,:))); - - %F�rsta residualen: - %3D-punkt parametrar: - %U1-koeff - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+1]'; - data(lastentry+[1:length(vis)]) = dU11{i}'; - lastentry = lastentry+length(vis); - - %U2-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; [(vis-1)*3+2]']; - %data = [data; dU21{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+2]'; - data(lastentry+[1:length(vis)]) = dU21{i}'; - lastentry = lastentry+length(vis); - - %U3-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; [vis*3]']; - %data = [data; dU31{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [vis*3]'; - data(lastentry+[1:length(vis)]) = dU31{i}'; - lastentry = lastentry+length(vis); - - %Kameraparametrar - %a-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - %data = [data; da1{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+1)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = da1{i}'; - lastentry = lastentry+length(vis); - - %b-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - %data = [data; db1{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+2)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = db1{i}'; - lastentry = lastentry+length(vis); - - %c-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - %data = [data; dc1{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+3)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dc1{i}'; - lastentry = lastentry+length(vis); - - %t_1-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - %data = [data; dt11{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+4)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt11{i}'; - lastentry = lastentry+length(vis); - - %t_2-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - %data = [data; dt21{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+5)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt21{i}'; - lastentry = lastentry+length(vis); - - %t_3-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - %data = [data; dt31{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+i*6)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt31{i}'; - lastentry = lastentry+length(vis); - - %Andra residualen: - %3D-punkt parametrar: - %U1-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; [(vis-1)*3+1]']; - %data = [data; dU12{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+1]'; - data(lastentry+[1:length(vis)]) = dU12{i}'; - lastentry = lastentry+length(vis); - - - %U2-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; [(vis-1)*3+2]']; - %data = [data; dU22{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+2]'; - data(lastentry+[1:length(vis)]) = dU22{i}'; - lastentry = lastentry+length(vis); - - %U3-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; [vis*3]']; - %data = [data; dU32{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [vis*3]'; - data(lastentry+[1:length(vis)]) = dU32{i}'; - lastentry = lastentry+length(vis); - - %Kameraparametrar - %a-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - %data = [data; da2{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+1)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = da2{i}'; - lastentry = lastentry+length(vis); - - %b-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - %data = [data; db2{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+2)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = db2{i}'; - lastentry = lastentry+length(vis); - - %c-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - %data = [data; dc2{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+3)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dc2{i}'; - lastentry = lastentry+length(vis); - - %t_1-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - %data = [data; dt12{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+4)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt12{i}'; - lastentry = lastentry+length(vis); - - %t_2-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - %data = [data; dt22{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+5)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt22{i}'; - lastentry = lastentry+length(vis); - - %t_3-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - %data = [data; dt32{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+i*6)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt32{i}'; - lastentry = lastentry+length(vis); - - - %Konstant-termer - btmp = zeros(2*length(vis),1); - %F�rsta residualen - btmp(1:2:end) = (P{i}(1,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(1,vis); - %Andra residualen - btmp(2:2:end) = (P{i}(2,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(2,vis); - %B = [B; btmp]; - res(iConstraint+[1:length(btmp)]) = btmp; - iConstraint = iConstraint+2*length(vis); - - - if 0 - A = sparse(row,col,data); - UU = 2*rand(3,numpts)-1; - pvar = 2*rand(6,length(P))-1; - var = [UU(:); pvar(:)]; - PP = pvar(:,i); - vis = isfinite(u.points{i}(1,:)); - aa = pvar(1,i); - bb = pvar(2,i); - cc = pvar(3,i); - tt1 = pvar(4,i); - tt2 = pvar(5,i); - tt3 = pvar(6,i); - - UU1 = UU(1,:); - UU2 = UU(2,:); - UU3 = UU(3,:); - f1 = da1{i}*aa + db1{i}*bb + dc1{i}*cc + ... - dU11{i}.*UU1 + dU21{i}.*UU2 + dU31{i}.*UU3 + ... - dt11{i}*tt1 + dt21{i}*tt2 + dt31{i}*tt3; - f2 = da2{i}*aa + db2{i}*bb + dc2{i}*cc + ... - dU12{i}.*UU1 + dU22{i}.*UU2 + dU32{i}.*UU3 + ... - dt12{i}*tt1 + dt22{i}*tt2 + dt32{i}*tt3; - f12 = (A*var(1:size(A,2))); - - figure(1);plot(f1(vis)); - figure(2);plot(f12(1:2:end)); - figure(1);plot(f1(vis)-f12(1:2:end)'); - figure(2);plot(f2(vis)-f12(2:2:end)'); - - A = sparse(row,col,data); - dU = 2*rand(size(U(1:3,:)))-1; - dP = 2*rand(6,length(P)); - var = [dU(:); dP(:)]; - f = []; - f2 = []; - slask = -0.01:0.0001:0.01; - for lambda = slask - Unew = pextend(U(1:3,:) + lambda*dU); - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - R = expm(lambda*(Ba*dP(1,i) + Bb*dP(2,i) + Bc*dP(3,i)))*R0; - t = t0+lambda*dP(4:6,i); - Pnew = [R t]; - vis = isfinite(u{i}(1,:)); - f = [f;sum(sum((pflat(Pnew*Unew(:,vis))-u{i}(:,vis)).^2))]; - f2 = [f2; sum((A*var(1:size(A,2))*lambda+B).^2)]; - end - figure(1);plot(slask,[f f2]); - - end -end - -nVars = 3*numpts + 6*length(P); -J = [J; w*sparse(row,col,data,nConditions, nVars)]; -res(end-nConditions+1:end) = w*res(end-nConditions+1:end); - - - - - -if 0 - for i = 1:length(P) - %fprintf('%d ',i); - uu = NaN*ones(3,u.pointnr); - uu(:,u.index{i}) = pflat(u.points{i}); - vis = find(isfinite(uu(1,:))); - - %F�rsta residualen: - %3D-punkt parametrar: - %U1-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; [(vis-1)*3+1]']; - data = [data; dU11{i}']; - %U2-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; [(vis-1)*3+2]']; - data = [data; dU21{i}']; - %U3-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; [vis*3]']; - data = [data; dU31{i}']; - %Kameraparametrar - %a-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - data = [data; da1{i}']; - %b-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - data = [data; db1{i}']; - %c-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - data = [data; dc1{i}']; - %t_1-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - data = [data; dt11{i}']; - %t_2-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - data = [data; dt21{i}']; - %t_3-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - data = [data; dt31{i}']; - - %Andra residualen: - %3D-punkt parametrar: - %U1-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; [(vis-1)*3+1]']; - data = [data; dU12{i}']; - %U2-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; [(vis-1)*3+2]']; - data = [data; dU22{i}']; - %U3-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; [vis*3]']; - data = [data; dU32{i}']; - %Kameraparametrar - %a-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - data = [data; da2{i}']; - %b-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - data = [data; db2{i}']; - %c-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - data = [data; dc2{i}']; - %t_1-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - data = [data; dt12{i}']; - %t_2-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - data = [data; dt22{i}']; - %t_3-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - data = [data; dt32{i}']; - resnum = resnum+2*length(vis); - - %Konstant-termer - btmp = zeros(2*length(vis),1); - %F�rsta residualen - btmp(1:2:end) = (P{i}(1,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(1,vis); - %Andra residualen - btmp(2:2:end) = (P{i}(2,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(2,vis); - B = [B; btmp]; - - if 0 - A = sparse(row,col,data); - UU = 2*rand(3,numpts)-1; - pvar = 2*rand(6,length(P))-1; - var = [UU(:); pvar(:)]; - PP = pvar(:,i); - vis = isfinite(u{i}(1,:)); - aa = pvar(1,i); - bb = pvar(2,i); - cc = pvar(3,i); - tt1 = pvar(4,i); - tt2 = pvar(5,i); - tt3 = pvar(6,i); - - UU1 = UU(1,:); - UU2 = UU(2,:); - UU3 = UU(3,:); - f1 = da1{i}*aa + db1{i}*bb + dc1{i}*cc + ... - dU11{i}.*UU1 + dU21{i}.*UU2 + dU31{i}.*UU3 + ... - dt11{i}*tt1 + dt21{i}*tt2 + dt31{i}*tt3; - f2 = da2{i}*aa + db2{i}*bb + dc2{i}*cc + ... - dU12{i}.*UU1 + dU22{i}.*UU2 + dU32{i}.*UU3 + ... - dt12{i}*tt1 + dt22{i}*tt2 + dt32{i}*tt3; - f12 = (A*var(1:size(A,2))); - - figure(1);plot(f1(vis)); - figure(2);plot(f12(1:2:end)); - figure(1);plot(f1(vis)-f12(1:2:end)'); - figure(2);plot(f2(vis)-f12(2:2:end)'); - - A = sparse(row,col,data); - dU = 2*rand(size(U(1:3,:)))-1; - dP = 2*rand(6,length(P)); - var = [dU(:); dP(:)]; - f = []; - f2 = []; - slask = -0.01:0.0001:0.01; - for lambda = slask - Unew = pextend(U(1:3,:) + lambda*dU); - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - R = expm(lambda*(Ba*dP(1,i) + Bb*dP(2,i) + Bc*dP(3,i)))*R0; - t = t0+lambda*dP(4:6,i); - Pnew = [R t]; - vis = isfinite(u{i}(1,:)); - f = [f;sum(sum((pflat(Pnew*Unew(:,vis))-u{i}(:,vis)).^2))]; - f2 = [f2; sum((A*var(1:size(A,2))*lambda+B).^2)]; - end - figure(1);plot(slask,[f f2]); - - end - end - -end - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/setupLinSystemCamera.m b/visionary/bundleAdjust/setupLinSystemCamera.m deleted file mode 100644 index 0350ce2..0000000 --- a/visionary/bundleAdjust/setupLinSystemCamera.m +++ /dev/null @@ -1,553 +0,0 @@ -function [res,J] = setupLinSystemCamera(P,U,u,w,res,J) -%Append constraints relating to reprojection errors in the images -%to Jacobian A and residual vector B - -calcJac = nargin >= 6; - -numpts = size(U,2); -%Bas f???r tangentplanet till rotationsm???ngfalden. -Ba = [0 1 0; -1 0 0; 0 0 0]; -Bb = [0 0 1; 0 0 0; -1 0 0]; -Bc = [0 0 0; 0 0 1; 0 -1 0]; - -iConstraint = size(res,1); -U = pflat(U); -U = U(1:3,:); - - -if calcJac - - assert(size(J,1)==iConstraint, 'BA:constraint', ... - 'Jacobian and residual must have same number of rows'); - - da1 = cell(size(P)); - db1 = cell(size(P)); - dc1 = cell(size(P)); - dt11 = cell(size(P)); - dt21 = cell(size(P)); - dt31 = cell(size(P)); - dU11 = cell(size(P)); - dU21 = cell(size(P)); - dU31 = cell(size(P)); - da2 = cell(size(P)); - db2 = cell(size(P)); - dc2 = cell(size(P)); - dt12 = cell(size(P)); - dt22 = cell(size(P)); - dt32 = cell(size(P)); - dU12 = cell(size(P)); - dU22 = cell(size(P)); - dU32 = cell(size(P)); - - - for i=1:length(P) - %ber???kna derivator f???r b???da residualeran i alla bilder - %a,b,c - rotations parametrar f???r kamera i - %U1,U2,U3 - 3d punkt parametrar - %t1,t2,t3 - translations parametrar kameran. - vis = u.index{i}; - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - - - da1{i} = (Ba(1,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Ba(3,:)*R0*U(:,vis)); - - da2{i} = (Ba(2,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Ba(3,:)*R0*U(:,vis)); - - db1{i} = (Bb(1,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bb(3,:)*R0*U(:,vis)); - - db2{i} = (Bb(2,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bb(3,:)*R0*U(:,vis)); - - dc1{i} = (Bc(1,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bc(3,:)*R0*U(:,vis)); - - dc2{i} = (Bc(2,:)*R0*U(:,vis))./(R0(3,:)*U(:,vis)+t0(3)) - ... - (R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2).*(Bc(3,:)*R0*U(:,vis)); - - dU11{i} = R0(1,1)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(1,:)*U(:,vis) + t0(1))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,1); - - dU12{i} = R0(2,1)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(2,:)*U(:,vis) + t0(2))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,1); - - dU21{i} = R0(1,2)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(1,:)*U(:,vis) + t0(1))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,2); - - dU22{i} = R0(2,2)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(2,:)*U(:,vis) + t0(2))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,2); - - dU31{i} = R0(1,3)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(1,:)*U(:,vis) + t0(1))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,3); - - dU32{i} = R0(2,3)./(R0(3,:)*U(:,vis) + t0(3)) - ... - (R0(2,:)*U(:,vis) + t0(2))./((R0(3,:)*U(:,vis) + t0(3)).^2).*R0(3,3); - - dt11{i} = 1./(R0(3,:)*U(:,vis)+t0(3)); - dt12{i} = zeros(size(dt11{i})); - - dt21{i} = zeros(size(dt11{i})); - dt22{i} = 1./(R0(3,:)*U(:,vis)+t0(3)); - - dt31{i} = -(R0(1,:)*U(:,vis)+t0(1))./((R0(3,:)*U(:,vis)+t0(3)).^2); - dt32{i} = -(R0(2,:)*U(:,vis)+t0(2))./((R0(3,:)*U(:,vis)+t0(3)).^2); - - - if 0 - aa = 2*rand(1,1)-1; - bb = 2*rand(1,1)-1; - cc = 2*rand(1,1)-1; - UU1 = 2*rand(1,1)-1; - UU2 = 2*rand(1,1)-1; - UU3 = 2*rand(1,1)-1; - tt1 = 2*rand(1,1)-1; - tt2 = 2*rand(1,1)-1; - tt3 = 2*rand(1,1)-1; - - f = []; - vis = find(isfinite(u{i}(1,:))); - kk = ceil(rand*length(vis)); - kk = vis(kk); - f01 = (-u{i}(1,kk)+(R0(1,:)*U(:,kk)+ t0(1))./(R0(3,:)*U(:,kk)+t0(3))); - f02 = (-u{i}(2,kk)+(R0(2,:)*U(:,kk)+t0(2))./(R0(3,:)*U(:,kk)+ t0(3))); - - f2 = []; - for s = -0.1:0.001:0.1 - R = expm(s*(Ba*aa+Bb*bb+Bc*cc))*R0; - t = t0+s*[tt1;tt2;tt3]; - UU = U+s*repmat([UU1;UU2;UU3], [1 size(U,2)]); - f = [f; (u{i}(1,kk)-(R(1,:)*UU(:,kk)+t(1))./(R(3,:)*UU(:,kk)+ t(3)))^2 + ... - (u{i}(2,kk)-(R(2,:)*UU(:,kk)+t(2))./(R(3,:)*UU(:,kk)+ t(3)))^2]; - f2 = [f2; (f01 + s*(da1{i}(kk)*aa + db1{i}(kk)*bb + dc1{i}(kk)*cc + ... - dU11{i}(kk)*UU1 + dU21{i}(kk)*UU2 + dU31{i}(kk)*UU3 + ... - dt11{i}(kk)*tt1 + dt21{i}(kk)*tt2 + dt31{i}(kk)*tt3)).^2 + ... - (f02 + s*(da2{i}(kk)*aa + db2{i}(kk)*bb + dc2{i}(kk)*cc + ... - dU12{i}(kk)*UU1 + dU22{i}(kk)*UU2 + dU32{i}(kk)*UU3 + ... - dt12{i}(kk)*tt1 + dt22{i}(kk)*tt2 + dt32{i}(kk)*tt3)).^2]; - end - %figure(1);plot(-0.1:0.001:0.1,[f f2(end:-1:1)]); - figure(1);plot(-0.1:0.001:0.1,[f f2]); - end - end - -end - -%Allokera hela vektorn f???rst tv??? residualer f???r varje bild punkt -nImgResiduals = 0; -for i = 1:length(P) - nImgResiduals = nImgResiduals + size(u.points{i},2); -end -nConditions = 2*nImgResiduals; -if calcJac - nVars = 3*numpts + 6*length(P); - nnzElems = nConditions*(6+3); - row = zeros(nnzElems,1); - col = zeros(nnzElems,1); - data = zeros(nnzElems,1); - lastentry = iConstraint; -end -res = [res; zeros(nConditions,1)]; - -for i = 1:length(P) - %fprintf('%d ',i); - uu = NaN*ones(3,u.pointnr); - uu(:,u.index{i}) = pflat(u.points{i}); - vis = find(isfinite(uu(1,:))); - - if calcJac - %F???rsta residualen: - %3D-punkt parametrar: - %U1-koeff - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+1]'; - data(lastentry+[1:length(vis)]) = dU11{i}'; - lastentry = lastentry+length(vis); - - %U2-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; [(vis-1)*3+2]']; - %data = [data; dU21{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+2]'; - data(lastentry+[1:length(vis)]) = dU21{i}'; - lastentry = lastentry+length(vis); - - %U3-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; [vis*3]']; - %data = [data; dU31{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [vis*3]'; - data(lastentry+[1:length(vis)]) = dU31{i}'; - lastentry = lastentry+length(vis); - - %Kameraparametrar - %a-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - %data = [data; da1{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+1)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = da1{i}'; - lastentry = lastentry+length(vis); - - %b-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - %data = [data; db1{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+2)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = db1{i}'; - lastentry = lastentry+length(vis); - - %c-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - %data = [data; dc1{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+3)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dc1{i}'; - lastentry = lastentry+length(vis); - - %t_1-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - %data = [data; dt11{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+4)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt11{i}'; - lastentry = lastentry+length(vis); - - %t_2-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - %data = [data; dt21{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+5)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt21{i}'; - lastentry = lastentry+length(vis); - - %t_3-koeff - %row = [row; resnum+[1:2:2*length(vis)]']; - %col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - %data = [data; dt31{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[1:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+i*6)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt31{i}'; - lastentry = lastentry+length(vis); - - %Andra residualen: - %3D-punkt parametrar: - %U1-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; [(vis-1)*3+1]']; - %data = [data; dU12{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+1]'; - data(lastentry+[1:length(vis)]) = dU12{i}'; - lastentry = lastentry+length(vis); - - - %U2-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; [(vis-1)*3+2]']; - %data = [data; dU22{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [(vis-1)*3+2]'; - data(lastentry+[1:length(vis)]) = dU22{i}'; - lastentry = lastentry+length(vis); - - %U3-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; [vis*3]']; - %data = [data; dU32{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = [vis*3]'; - data(lastentry+[1:length(vis)]) = dU32{i}'; - lastentry = lastentry+length(vis); - - %Kameraparametrar - %a-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - %data = [data; da2{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+1)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = da2{i}'; - lastentry = lastentry+length(vis); - - %b-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - %data = [data; db2{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+2)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = db2{i}'; - lastentry = lastentry+length(vis); - - %c-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - %data = [data; dc2{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+3)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dc2{i}'; - lastentry = lastentry+length(vis); - - %t_1-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - %data = [data; dt12{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+4)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt12{i}'; - lastentry = lastentry+length(vis); - - %t_2-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - %data = [data; dt22{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+(i-1)*6+5)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt22{i}'; - lastentry = lastentry+length(vis); - - %t_3-koeff - %row = [row; resnum+[2:2:2*length(vis)]']; - %col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - %data = [data; dt32{i}']; - row(lastentry+[1:length(vis)]) = iConstraint+[2:2:2*length(vis)]'; - col(lastentry+[1:length(vis)]) = (3*numpts+i*6)*ones(length(vis),1); - data(lastentry+[1:length(vis)]) = dt32{i}'; - lastentry = lastentry+length(vis); - end - - %Konstant-termer - btmp = zeros(2*length(vis),1); - %F???rsta residualen - btmp(1:2:end) = (P{i}(1,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(1,vis); - %Andra residualen - btmp(2:2:end) = (P{i}(2,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(2,vis); - %B = [B; btmp]; - res(iConstraint+[1:length(btmp)]) = btmp; - iConstraint = iConstraint+2*length(vis); - - - if 0 - A = sparse(row,col,data); - UU = 2*rand(3,numpts)-1; - pvar = 2*rand(6,length(P))-1; - var = [UU(:); pvar(:)]; - PP = pvar(:,i); - vis = isfinite(u.points{i}(1,:)); - aa = pvar(1,i); - bb = pvar(2,i); - cc = pvar(3,i); - tt1 = pvar(4,i); - tt2 = pvar(5,i); - tt3 = pvar(6,i); - - UU1 = UU(1,:); - UU2 = UU(2,:); - UU3 = UU(3,:); - f1 = da1{i}*aa + db1{i}*bb + dc1{i}*cc + ... - dU11{i}.*UU1 + dU21{i}.*UU2 + dU31{i}.*UU3 + ... - dt11{i}*tt1 + dt21{i}*tt2 + dt31{i}*tt3; - f2 = da2{i}*aa + db2{i}*bb + dc2{i}*cc + ... - dU12{i}.*UU1 + dU22{i}.*UU2 + dU32{i}.*UU3 + ... - dt12{i}*tt1 + dt22{i}*tt2 + dt32{i}*tt3; - f12 = (A*var(1:size(A,2))); - - figure(1);plot(f1(vis)); - figure(2);plot(f12(1:2:end)); - figure(1);plot(f1(vis)-f12(1:2:end)'); - figure(2);plot(f2(vis)-f12(2:2:end)'); - - A = sparse(row,col,data); - dU = 2*rand(size(U(1:3,:)))-1; - dP = 2*rand(6,length(P)); - var = [dU(:); dP(:)]; - f = []; - f2 = []; - slask = -0.01:0.0001:0.01; - for lambda = slask - Unew = pextend(U(1:3,:) + lambda*dU); - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - R = expm(lambda*(Ba*dP(1,i) + Bb*dP(2,i) + Bc*dP(3,i)))*R0; - t = t0+lambda*dP(4:6,i); - Pnew = [R t]; - vis = isfinite(u{i}(1,:)); - f = [f;sum(sum((pflat(Pnew*Unew(:,vis))-u{i}(:,vis)).^2))]; - f2 = [f2; sum((A*var(1:size(A,2))*lambda+B).^2)]; - end - figure(1);plot(slask,[f f2]); - - end -end - -if calcJac - J = [J; w*sparse(row,col,data,nConditions, nVars)]; -end - -res(end-nConditions+1:end) = w*res(end-nConditions+1:end); - - - - - -if 0 - for i = 1:length(P) - %fprintf('%d ',i); - uu = NaN*ones(3,u.pointnr); - uu(:,u.index{i}) = pflat(u.points{i}); - vis = find(isfinite(uu(1,:))); - - %F???rsta residualen: - %3D-punkt parametrar: - %U1-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; [(vis-1)*3+1]']; - data = [data; dU11{i}']; - %U2-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; [(vis-1)*3+2]']; - data = [data; dU21{i}']; - %U3-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; [vis*3]']; - data = [data; dU31{i}']; - %Kameraparametrar - %a-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - data = [data; da1{i}']; - %b-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - data = [data; db1{i}']; - %c-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - data = [data; dc1{i}']; - %t_1-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - data = [data; dt11{i}']; - %t_2-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - data = [data; dt21{i}']; - %t_3-koeff - row = [row; resnum+[1:2:2*length(vis)]']; - col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - data = [data; dt31{i}']; - - %Andra residualen: - %3D-punkt parametrar: - %U1-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; [(vis-1)*3+1]']; - data = [data; dU12{i}']; - %U2-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; [(vis-1)*3+2]']; - data = [data; dU22{i}']; - %U3-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; [vis*3]']; - data = [data; dU32{i}']; - %Kameraparametrar - %a-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+1)*ones(length(vis),1)]; - data = [data; da2{i}']; - %b-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+2)*ones(length(vis),1)]; - data = [data; db2{i}']; - %c-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+3)*ones(length(vis),1)]; - data = [data; dc2{i}']; - %t_1-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+4)*ones(length(vis),1)]; - data = [data; dt12{i}']; - %t_2-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+(i-1)*6+5)*ones(length(vis),1)]; - data = [data; dt22{i}']; - %t_3-koeff - row = [row; resnum+[2:2:2*length(vis)]']; - col = [col; (3*numpts+i*6)*ones(length(vis),1)]; - data = [data; dt32{i}']; - resnum = resnum+2*length(vis); - - %Konstant-termer - btmp = zeros(2*length(vis),1); - %F???rsta residualen - btmp(1:2:end) = (P{i}(1,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(1,vis); - %Andra residualen - btmp(2:2:end) = (P{i}(2,:)*pextend(U(:,vis)))./(P{i}(3,:)*pextend(U(:,vis)))-uu(2,vis); - B = [B; btmp]; - - if 0 - A = sparse(row,col,data); - UU = 2*rand(3,numpts)-1; - pvar = 2*rand(6,length(P))-1; - var = [UU(:); pvar(:)]; - PP = pvar(:,i); - vis = isfinite(u{i}(1,:)); - aa = pvar(1,i); - bb = pvar(2,i); - cc = pvar(3,i); - tt1 = pvar(4,i); - tt2 = pvar(5,i); - tt3 = pvar(6,i); - - UU1 = UU(1,:); - UU2 = UU(2,:); - UU3 = UU(3,:); - f1 = da1{i}*aa + db1{i}*bb + dc1{i}*cc + ... - dU11{i}.*UU1 + dU21{i}.*UU2 + dU31{i}.*UU3 + ... - dt11{i}*tt1 + dt21{i}*tt2 + dt31{i}*tt3; - f2 = da2{i}*aa + db2{i}*bb + dc2{i}*cc + ... - dU12{i}.*UU1 + dU22{i}.*UU2 + dU32{i}.*UU3 + ... - dt12{i}*tt1 + dt22{i}*tt2 + dt32{i}*tt3; - f12 = (A*var(1:size(A,2))); - - figure(1);plot(f1(vis)); - figure(2);plot(f12(1:2:end)); - figure(1);plot(f1(vis)-f12(1:2:end)'); - figure(2);plot(f2(vis)-f12(2:2:end)'); - - A = sparse(row,col,data); - dU = 2*rand(size(U(1:3,:)))-1; - dP = 2*rand(6,length(P)); - var = [dU(:); dP(:)]; - f = []; - f2 = []; - slask = -0.01:0.0001:0.01; - for lambda = slask - Unew = pextend(U(1:3,:) + lambda*dU); - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - R = expm(lambda*(Ba*dP(1,i) + Bb*dP(2,i) + Bc*dP(3,i)))*R0; - t = t0+lambda*dP(4:6,i); - Pnew = [R t]; - vis = isfinite(u{i}(1,:)); - f = [f;sum(sum((pflat(Pnew*Unew(:,vis))-u{i}(:,vis)).^2))]; - f2 = [f2; sum((A*var(1:size(A,2))*lambda+B).^2)]; - end - figure(1);plot(slask,[f f2]); - - end - end - -end - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/setupLinSystemPosPrior.m b/visionary/bundleAdjust/setupLinSystemPosPrior.m deleted file mode 100644 index f1c96a7..0000000 --- a/visionary/bundleAdjust/setupLinSystemPosPrior.m +++ /dev/null @@ -1,73 +0,0 @@ -function [J,res] = setupLinSystemPosPrior(J,res,P,C_p,numpts,w) -%Bas f�r tangentplanet till rotationsm�ngfalden. -Ba = [0 1 0; -1 0 0; 0 0 0]; -Bb = [0 0 1; 0 0 0; -1 0 0]; -Bc = [0 0 0; 0 0 1; 0 -1 0]; - - -daC = cell(size(P)); -dbC = cell(size(P)); -dcC = cell(size(P)); -dt1C = cell(size(P)); -dt2C = cell(size(P)); -dt3C = cell(size(P)); - - -nPosConditions = 0; - -for i=1:length(P) - %a,b,c - rotations parametrar f�r kamera i - %t1,t2,t3 - translations parametrar kameran. - R0 = P{i}(:,1:3); - - - if all(isfinite(C_p(:,i))) - daC{i} = Ba*R0*C_p(:,i); - dbC{i} = Bb*R0*C_p(:,i); - dcC{i} = Bc*R0*C_p(:,i); - dtC = eye(3); - dt1C{i} = dtC(:,1); - dt2C{i} = dtC(:,2); - dt3C{i} = dtC(:,3); - nPosConditions = nPosConditions + 1; - end -end - -if nPosConditions < 2 - error('BA:constraint', 'Not enough camera position priors'); -end - -if(size(res,1) ~= size(J,1)) - error('BA:constraint', ... - 'Jacobian and residual must have same number of rows'); -end - -nVars = 3*numpts + 6*length(P); -iConstraint = 0; -nnzElems = 3*6*nPosConditions; -nConditions = 3*nPosConditions; -row = zeros(nnzElems,1); -col = zeros(nnzElems,1); -data = zeros(nnzElems,1); -lastentry = 0; -resAdd = zeros(nConditions,1); - -for i = 1:length(P) - - if all(isfinite(C_p(:,i))) - row(lastentry+(1:3*6)) = iConstraint+repmat((1:3)',6,1); - col(lastentry+(1:3*6)) = 3*numpts+(i-1)*6+reshape(repmat(1:6,3,1),18,1); - data(lastentry+(1:3*6)) = [daC{i}; dbC{i}; dcC{i}; dt1C{i}; dt2C{i}; dt3C{i}]; - lastentry = lastentry+3*6; - - e = P{i}*[C_p(:,i);1]; - resAdd(iConstraint+(1:3)) = e; - iConstraint = iConstraint+3; - end - -end - -J = [J; w*sparse(row,col,data,nConditions, nVars)]; -res = [res; w*resAdd]; - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/setupLinSystemRelCam.m b/visionary/bundleAdjust/setupLinSystemRelCam.m deleted file mode 100644 index d18bbd3..0000000 --- a/visionary/bundleAdjust/setupLinSystemRelCam.m +++ /dev/null @@ -1,99 +0,0 @@ -function [res, J] = setupLinSystemRelCam(P, relPoses, nPoints, res, J) -%w is square root of the inverse of the covariance matrix for -%[x, y, z, a, b, c]^T - - - -N = length(relPoses); -m = 3+9; -resAdd = zeros(m*N,1); - -for i = 1:N - I = (relPoses(i).M*[P{relPoses(i).i}; 0 0 0 1]) / ... - [P{relPoses(i).j}; 0 0 0 1]; - resAdd((i-1)*m+(1:3)) = I(1:3,4).*reshape(relPoses(i).w(1:3),3,1); - eAngle = (I(1:3,1:3)-eye(3)); - resAdd((i-1)*m+(4:12)) = eAngle(:)*mean(relPoses(i).w(4:end)); -end -res = [res; resAdd]; - -if nargout >= 2 - %Need to output Jacobian as well - nVars = size(J,2); - nCams = numel(P); - assert(nVars == nPoints*3 + nCams*6); - - %Rotation manifold bases - Bs(:,:,1) = [0 1 0; -1 0 0; 0 0 0]; - Bs(:,:,2) = [0 0 1; 0 0 0; -1 0 0]; - Bs(:,:,3) = [0 0 0; 0 0 1; 0 -1 0]; - - - %Analytical Jacobian - col = []; - row = []; - data = []; - - for k = 1:N - %Shorter variable names - i = relPoses(k).i; - j = relPoses(k).j; - Ri = P{i}(1:3,1:3); - Rj = P{j}(1:3,1:3); - Rm = relPoses(k).M(1:3,1:3); - tj = P{j}(1:3,4); - wR = mean(relPoses(k).w(4:end)); - wt = reshape(relPoses(k).w(1:3),3,1); - - %dRotation/d(a,b,c) - dRRi = zeros(3,3,3); - dRRj = zeros(3,3,3); - for l = 1:3 - dRRi(:,:,l) = Rm*Bs(:,:,l)*Ri*Rj'; - dRRj(:,:,l) = Rm*Ri*(Bs(:,:,l)*Rj)'; - end - dRRi = wR*dRRi; - dRRj = wR*dRRj; - %rows and cols - rowsRR = repmat((k-1)*m+(4:12)',6,1); - colsi = 3*nPoints+(i-1)*6+(1:3); - colsj = 3*nPoints+(j-1)*6+(1:3); - colsRR = [repmat(colsi,9,1), repmat(colsj,9,1)]; - - %dTranslation/d(a,b,c) - dtRi = zeros(3,3); - dtRj = zeros(3,3); - for l = 1:3 - dtRi(:,l) = -Rm*Bs(:,:,l)*Ri*Rj'*tj; - dtRj(:,l) = -Rm*Ri*(Bs(:,:,l)*Rj)'*tj; - end - dtRi = wt.*dtRi; - dtRj = wt.*dtRj; - %rows and cols - rowstR = repmat((k-1)*m+(1:3)',6,1); - colsi = 3*nPoints+(i-1)*6+(1:3); - colsj = 3*nPoints+(j-1)*6+(1:3); - colstR = [repmat(colsi,3,1), repmat(colsj,3,1)]; - - %dTranslation/d(t1,t2,t3) - dtti = wt.*Rm; - dttj = wt.*(-Rm*Ri*Rj'); - %rows and cols - rowstt = repmat((k-1)*m+(1:3)',6,1); - colsi = 3*nPoints+(i-1)*6+(4:6); - colsj = 3*nPoints+(j-1)*6+(4:6); - colstt = [repmat(colsi,3,1), repmat(colsj,3,1)]; - - %Append to row/col/data - row = [row; rowsRR; rowstR; rowstt]; - col = [col; colsRR(:); colstR(:); colstt(:)]; - data = [data; dRRi(:); dRRj(:); dtRi(:); dtRj(:); dtti(:); dttj(:)]; - - end - ix = abs(data) > 1e-10; - J = [J; sparse(row(ix),col(ix),data(ix),N*m, nVars)]; - -end - - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/setupLinSystemRotPrior.m b/visionary/bundleAdjust/setupLinSystemRotPrior.m deleted file mode 100644 index af2912b..0000000 --- a/visionary/bundleAdjust/setupLinSystemRotPrior.m +++ /dev/null @@ -1,67 +0,0 @@ -function [J,res] = setupLinSystemRotPrior(J,res,P,R_p,numpts,w) -%Bas f�r tangentplanet till rotationsm�ngfalden. -Ba = [0 1 0; -1 0 0; 0 0 0]; -Bb = [0 0 1; 0 0 0; -1 0 0]; -Bc = [0 0 0; 0 0 1; 0 -1 0]; - - -daR = cell(size(P)); -dbR = cell(size(P)); -dcR = cell(size(P)); - -nRotConditions = 0; - -for i=1:length(P) - %ber�kna derivator f�r b�da residualeran i alla bilder - %a,b,c - rotations parametrar f�r kamera i - %U1,U2,U3 - 3d punkt parametrar - %t1,t2,t3 - translations parametrar kameran. - R0 = P{i}(:,1:3); - - - if ~isempty(R_p{i}) - daR{i} = Ba*R0; - dbR{i} = Bb*R0; - dcR{i} = Bc*R0; - nRotConditions = nRotConditions + 1; - end -end - -if nRotConditions < 2 - error('BA:constraint', 'Not enough camera rotation priors'); -end - -if(size(res,1)~=size(J,1)) - error('BA:constraint', ... - 'Jacobian and residual must have same number of rows'); -end - -nVars = 3*numpts + 6*length(P); -iConstraint = 0; -nnzElems = 3*9*nRotConditions; -nConditions = 9*nRotConditions; -row = zeros(nnzElems,1); -col = zeros(nnzElems,1); -data = zeros(nnzElems,1); -lastentry = 0; -resAdd = zeros(nConditions,1); - -for i = 1:length(P) - if ~isempty(R_p{i}) - row(lastentry+(1:9*3)) = iConstraint+repmat((1:9)',3,1); - col(lastentry+(1:9*3)) = 3*numpts+(i-1)*6+reshape(repmat(1:3,9,1),27,1); - data(lastentry+(1:9*3)) = [daR{i}(:); dbR{i}(:); dcR{i}(:)]; - lastentry = lastentry+9*3; - - R0 = P{i}(1:3,1:3); - e = R0-R_p{i}; - resAdd(iConstraint+(1:9)) = e(:); - iConstraint = iConstraint+9; - end - -end - -J = [J; w*sparse(row,col,data,nConditions, nVars)]; -res = [res; w*resAdd]; - -end \ No newline at end of file diff --git a/visionary/bundleAdjust/update_var.m b/visionary/bundleAdjust/update_var.m deleted file mode 100644 index 2df8507..0000000 --- a/visionary/bundleAdjust/update_var.m +++ /dev/null @@ -1,16 +0,0 @@ -function [Pnew,Unew] = update_var(dpointvar,dcamvar,P,U) -Ba = [0 1 0; -1 0 0; 0 0 0]; -Bb = [0 0 1; 0 0 0; -1 0 0]; -Bc = [0 0 0; 0 0 1; 0 -1 0]; - -Unew = pextend(U(1:3,:) + dpointvar); - -Pnew = cell(size(P)); -for i=1:length(P) - R0 = P{i}(:,1:3); - t0 = P{i}(:,4); - R = expm(Ba*dcamvar(1,i) + Bb*dcamvar(2,i) + Bc*dcamvar(3,i))*R0; - t = t0 + dcamvar(4:6,i); - Pnew{i} = [R t]; -end - diff --git a/visionary/bundleplc.m b/visionary/bundleplc.m deleted file mode 100644 index d718965..0000000 --- a/visionary/bundleplc.m +++ /dev/null @@ -1,356 +0,0 @@ -function [s,m,residual,resnew,lambda] = bundleplc(s,m,imseq,option); -% function [s,m,redidual,resnew] = bundleplc(s,m,imseq,option); -% Bundle adjustment for combinations of points, lines and conics -% Input: -% s - structure object -% m - motion object -% imseq - cell list of imagedata objects -% option: (cell of strings) -% 'affine' - affine s & m -% 'iteration=X' - iterate X times -% 'lambda=X' - set lambda to X -% 'outputoff' - No results displayed -% 'autocalib=XXXXX' - Autocalibration -% specify for each of focal length, aspect ratio, skew, -% principal point x, principal point y, -% if the parameter is -% 1 known and nominal, -% 2 unknown but constant in the sequence, -% 3 unknown and varying. -% 'structure' - Only optimize structure -% 'motion' - Only optimize motion -% 'knownrotation' - Only optimize camera centres and structure -% 'focal=[f,sigma]' - Focal length at f with std sigma -% 'ar=[a,sigma]' - Aspect ratio at a with std sigma -% 'pp=[u,v,sigma]' - Principal point at (u,v) with std sigma -% 'lockcsystem' - Lock coordinate system. Locks first camera -% and one of second camera centre coordinates. NB: Works -% only in autocalibration mode. -% 'pflat' - 3D points are pflat'ed (default is psphere). -% 'stdlines=X' - standard deviation of line noise -% 'zplane=[index1,index2,...] - index of points on z=0 plane -% Output: -% s - updated structure -% m - updated motion -% residual - total reprojection error -% resnew - calculated residuals -% Default is projective bundle. -warningstate=warning('off'); - -if ~isa(s,'structure'), - s=structure(s); -end -if ~isa(m,'motion'), - m=motion(m); -end - -caliboptions=[]; -mode = 1; -modestr={'projective','affine','autocalibration','structure','motion','autocalibration+motion','knownrotation'}; -iteration=10; -startlambda=10; -outputon=1; -focal=[]; -ar=[]; -pp=[]; -lockcsystem=0; -lockstr={'coordinate system free','coordinate system locked'}; -pflatpoints=0; -stdlines=1; % standard deviation for lines (endpoint distance) - -zplane=[]; -if nargin>3, - if strmatch('affine',option), - mode = 2; - end - if strmatch('outputoff',option), - outputon=0; - end - if strmatch('iteration=',option), - q=strmatch('iteration=',option); - striter = option{q}(11:length(option{q})); - iteration=str2num(striter); - end - if strmatch('lambda=',option), - q=strmatch('lambda=',option); - strlambda = option{q}(8:length(option{q})); - startlambda=str2num(strlambda); - end - if strmatch('autocalib=',option), - mode = 3; - q=strmatch('autocalib=',option); - strautocalib = option{q}(11:length(option{q})); - caliboptions=[str2num(strautocalib(1)) str2num(strautocalib(2)) str2num(strautocalib(3)) str2num(strautocalib(4)) str2num(strautocalib(5))]; - end - if strmatch('structure',option), - mode = 4; - end - if strmatch('motion',option), - if mode==3, - mode=6; %autocalibration+motion - else - mode = 5; %motion (i.e. projective) - end - end - if strmatch('knownrotation',option), - mode=7; - end - if strmatch('focal=',option) & ~isempty(caliboptions), - if caliboptions(1)>1, - q=strmatch('focal=',option); - tmp=option{q}; - tmp(end+1)=';'; - eval(tmp); - focal(3)=caliboptions(1); - end - end - if strmatch('ar=',option) & ~isempty(caliboptions), - if caliboptions(2)>1, - q=strmatch('ar=',option); - tmp=option{q}; - tmp(end+1)=';'; - eval(tmp); - ar(3)=caliboptions(2); - end - end - if strmatch('pp=',option) & ~isempty(caliboptions), - if sum(caliboptions(4:5)>1)>0, - q=strmatch('pp=',option); - tmp=option{q}; - tmp(end+1)=';'; - eval(tmp); - pp(4:5)=caliboptions(4:5); - end - end - if strmatch('lockcsystem',option) & mode == 3, - lockcsystem=1; - end - if strmatch('pflat',option), - pflatpoints=1; - end - if strmatch('stdlines=',option), - q=strmatch('stdlines=',option); - 'I accidently removed these lines....' - 'redo them before use....' - keyboard; - end - if strmatch('zplane=',option), - q=strmatch('zplane=',option); - tmp=option{q}; - tmp(end+1)=';'; - eval(tmp); - end -end -% Extract points, lines and conics from imseq -antalbilder = length(imseq); -fs=size(imseq{1}); -nbrpts=fs(1); nbrlines=fs(2); nbrconics=fs(3); -pointindex=zeros(1,nbrpts); -lineindex=zeros(1,nbrlines); -conicindex=zeros(1,nbrconics); - -nbrfeatures = nbrpts + nbrlines + nbrconics; -mm=cell(antalbilder,1); -ss=cell(1,0); -im=cell(antalbilder,0); - -if mode==3 | mode==6, -% P1=getcameras(m,1); -% K1=rq(P1); -% T=[inv(K1)*P1;0,0,0,1]; -% m=changecsystem(m,T); -% s=changecsystem(s,T); - - for i=1:antalbilder, - Pi=getcameras(m,i); - Ki=rq(Pi); - mm{i}=pmatrix(Pi/Ki(3,3)); - end -else - for i=1:antalbilder, - temp=getcameras(m,i);if mode~=2, temp=temp/norm(temp);end - mm{i}=pmatrix(temp); - end -end - -nbrpts0=0;nbrlines0=0;nbrconics0=0; -% Add points -for j=1:nbrpts, - temp=getpoints(s,j); - if ~isempty(find(j==zplane))>0, - temp(3)=0; - end - %check that it is visible - visible=0; - for i=1:antalbilder, - sl=getpoints(imseq{i},j); - visible=max(visible,~isnan(sl(1))); - end - if ~isnan(temp(1)) & visible, - nbrpts0=nbrpts0+1; - pointindex(j)=nbrpts0; - if pflatpoints==1, - ss{nbrpts0}=obpoint(pflat(temp)); - else - ss{nbrpts0}=obpoint(psphere(temp)); - end - for i=1:antalbilder, - [temp,L] = getpoints(imseq{i},j); - if ~isnan(temp(1)), - im{i,nbrpts0} = impoint(pflat(temp),L,[0;0;1]); - end; - end; - end -end; -joffset=nbrpts0; - -% Add lines -for j=1:nbrlines, - temp=getlines(s,j); - if ~isnan(temp(1)), - nbrlines0=nbrlines0+1; - lineindex(j)=nbrlines0; - temp=pflat(reshape(temp,4,2)); - ss{nbrlines0+joffset}=obline(temp); - for i=1:antalbilder, -% [temp,L] = gethomogeneouslines(imseq{i},j); - temp = gethomogeneouslines(imseq{i},j); - if ~isnan(temp(1)), - temp=temp/norm(temp(1:2)); -% im{i,nbrlines0+joffset} = imline(temp,L,[temp(1:2,1);0]); - im{i,nbrlines0+joffset} = imline(temp,stdlines); - end; - end; - end; -end; -joffset=joffset+nbrlines0; - -% Add conics -for j=1:nbrconics, - temp=getquadrics(s,j); - if ~isnan(temp(1)), - nbrconics0=nbrconics0+1; - conicindex(j)=nbrconics0; - temp=v2m(temp); - ss{nbrconics0+joffset}=obconic(temp); - for i=1:antalbilder, - [temp,L] = getconics(imseq{i},j); - if ~isnan(temp(1)), - temp=pflat(temp); - ntemp=[0,0,0,0,0,1]'; -% temp=temp/norm(temp); - im{i,nbrconics0+joffset} = imconic(temp,L,ntemp); - end; - end; - end -end; -cont=1; -lambda=startlambda; - -if outputon==0, - loops=iteration; -else - loops=0; -end; - -if lockcsystem>0, - c1=pflat(null(pdp(mm{1})));c2=pflat(null(pdp(mm{2}))); - [slask,lockcsystem]=max(abs(c1(1:3)-c2(1:3))); -end - - -%calculate residuals -[mi,si,imi,offsetmotion,offsetsm,offsetres]=calcindexplc(im,ss,mm,mode,zplane); -resnew=calcresplc(im,ss,mm,imi,si,mi,offsetres,offsetsm,pp,focal,ar); -if outputon, - disp(['Current error: ',num2str(norm(resnew)/sqrt(length(resnew)))]); - plot(resnew,'.');title('Residuals'); - disp(['Mode: ',modestr{mode},', ',lockstr{(lockcsystem>0)+1}]); -end; - -while cont, -%%%%%%%%%%%%% -% BUNDLE LOOOP -%%%%%%%%%%%%% - for i=loops:-1:1; - if outputon, - disp(['Iterations left: ' num2str(i)]); - end - [ss,mm,step,lambda,oldf,newf,resnew]=bundlestepplc(im,ss,mm,lambda,mode,caliboptions,lockcsystem,pp,focal,ar,zplane); - if outputon, - disp(['Old error: ',num2str(oldf/sqrt(length(resnew))),' New error: ',num2str(newf/sqrt(length(resnew)))]); - disp(['Lognorm of gradient step: ' num2str(log(norm(step)))]); - plot(resnew,'.');title('Residuals');drawnow; - end - end -%%%%%%%%%%%%% -% BUNDLE LOOOP - end -%%%%%%%%%%%%% - - if outputon, - disp(['Current lambda: ',num2str(lambda),' Nbr of iterations: ',num2str(iteration)]); - askuser=1; - else - askuser=0; - cont=0; - end; - -%%%%%%%%%%%%% -% ASK USER -%%%%%%%%%%%%% - while askuser, - disp('(c)ontinue, (s)et lambda, (i)terations, (k)eyboard'); - opt=input('(h)istogram, (l)ock/unlock coordinate system, (q)uit? ','s'); - switch opt - case 'c', askuser=0; - case 's', lambda=input('Lambda: '); - case 'i', iteration=input('Iterations: '); - case 'k', keyboard; - case 'h', hist(resnew,21);title('Histogram of residuals');estimestd=sqrt(resnew'*resnew/(size(resnew,1)-3*nbrpts0-4*nbrlines0-9*nbrconics0-15));disp(['Estimated standard deviation: ',num2str(estimestd)]); - case 'l', if lockcsystem==0, c1=pflat(null(pdp(mm{1})));c2=pflat(null(pdp(mm{2}))); [slask,lockcsystem]=max(abs(c1(1:3)-c2(1:3))); else lockcsystem=0;end;if outputon,disp(['Mode: ',modestr{mode},', ',lockstr{(lockcsystem>0)+1}]);end; - case 'q', askuser=0;cont=0; - end - end; -% END of askuser - - loops=iteration; -end; %END of cont - - -% Update structure s and motion m - -s=structure; -m=motion; - -for j=1:nbrpts; - if pointindex(j)~=0, - s=addpoints(s,udu(ss{pointindex(j)})); - else - s=addpoints(s,NaN*ones(4,1)); - end -end; -joffset=nbrpts0; -for j=1:nbrlines; - if lineindex(j)~=0, - s=addlines(s,reshape(udu(ss{lineindex(j)+joffset}),8,1)); - else - s=addlines(s,NaN*ones(8,1)); - end -end; -joffset=nbrpts0+nbrlines0; -for j=1:nbrconics; - if conicindex(j)~=0, - s=addquadrics(s,m2v(udu(ss{conicindex(j)+joffset}))); - else - s=addquadrics(s,NaN*ones(10,1)); - end -end; - -for i=1:antalbilder; - m=addcameras(m,pdp(mm{i})); -end; - - - -residual=norm(resnew); -warning(warningstate); diff --git a/visionary/bundler_import.m b/visionary/bundler_import.m deleted file mode 100644 index 9fcc2e4..0000000 --- a/visionary/bundler_import.m +++ /dev/null @@ -1,126 +0,0 @@ -function [str, mot, Kmot, f, pp, kk, im, imundist, rgb] = bundler_import(bibdir) -%Import results from Noah Snavely's Bundler (Photo Tourism) to visionary -% INPUT: filename -% OUTPUT: str - structure -% mot - motion (without Calibration matrix) -% Kmot - motion (with calibration matrix) -% f - focal lengths -% pp - principal points -% kk - radial distortions -% im - cell of imagadata, original coords & image files -% imundist - cell of imagedata, undistorted, normalized coords -% rgb - 3xn matrix of point colors -% -% See "test_bundler" for examples -% -filename = [bibdir,'/bundle/bundle.out']; -fid = fopen(filename); - -fscanf(fid, '%s', 4); - -nbr_cameras = fscanf(fid, '%d',1); -nbr_points = fscanf(fid, '%d',1); - -% Set up camera output -mot = motion; - -f=zeros(1,0); -kk=zeros(2,0); -index=zeros(1,nbr_cameras); -im={}; -im0={}; -for camera = 1:nbr_cameras - focal_length = fscanf(fid, '%f',1); - k1k2 = fscanf(fid, '%f', 2); - R = fscanf(fid, '%f', [3,3])'; - t = fscanf(fid, '%f', 3); - if focal_length ~=0 -% mot = mot + motion(-[focal_length 0 0; 0 focal_length 0; 0 0 1]*[R t]); - mot = mot + motion(diag([1,-1,-1])*[R t]); - f(end+1)=focal_length; - kk(:,end+1)=k1k2'; - index(camera)=length(f); - im0{end+1}=NaN*ones(2,nbr_points); - end -end - -nbr_actual_cameras=size(mot); - -str = structure; -rgb=zeros(3,nbr_points); - -for point_ind = 1:nbr_points - % Read the position - point = fscanf(fid, '%f', 3); - - str = str + structure(point); - - % rgb color - rgbtmp = fscanf(fid, '%f', 3); - rgb(:,point_ind)=rgbtmp; - - % 2d information - nbr_views = fscanf(fid, '%d', 1); - tmp=reshape(fscanf(fid, '%f', 4*nbr_views),4,nbr_views); - for ii=1:nbr_views, - imindex=index(tmp(1,ii)+1); - im0{imindex}(:,point_ind)=diag([1,-1])*tmp(3:4,ii); - end -end - -% Close file -fclose(fid); - - - -% Open "list.txt" -filename = [bibdir,filesep,'list.txt']; -fid = fopen(filename); - -imnames={}; -for ii=1:nbr_cameras, - % Read image filename - - tline = fgetl(fid); - imnames{ii}=sscanf(tline, '%s', 1); -end -% Close file -fclose(fid); - - -for ii=1:length(im0), - tmpindex=find(index==ii); - tmpfile = [bibdir,filesep,imnames{tmpindex}]; - tmpim=imread(tmpfile); - pp(1,ii)=size(tmpim,2)/2; - pp(2,ii)=size(tmpim,1)/2; - tmp=im0{ii}; - tmp(1,:)=tmp(1,:)+pp(1,ii); - tmp(2,:)=tmp(2,:)+pp(2,ii)-1; - im{ii}=imagedata(tmpfile,tmp); -end - -imundist=undist_radial(im0,f,kk); - -Kmot=motion; -for ii=1:size(mot), - Puncalib=getcameras(mot,ii); - K=diag([f(ii),f(ii),1]); - K(1:2,3)=pp(:,ii); - Kmot=addcameras(Kmot,K*Puncalib); -end - -%normalize coordinate system -X=pflat(str);X(4,:)=[];tr=mean(X')'; -sc=1/std(X(:)); -T=sc*eye(4);T(4,4)=1; -T(1:3,4)=-sc*tr; -str=changecsystem(str,T); -mot=changecsystem(mot,T); -Kmot=changecsystem(Kmot,T); - - -%imdist=dist_radial(imundist,f,kk) -%ii=13;imindex=2;tmp=pflat(P{imindex}*X(:,ii));tmp(3)=[];l=norm(tmp);[tmp*(1+l^2*kk(1,imindex)+l^4*kk(2,imindex))*f(imindex),im0{imindex}(:,ii)] -%ii=13;imindex=2;tmp=pflat(P{imindex}*X(:,ii));[tmp,getpoints(imundist{imindex},ii)] -%1+l^2*kk(1,imindex)+l^4*kk(2,imindex))*f(imindex),im0{imindex}(:,ii)] diff --git a/visionary/calibrated_fivepoint.m b/visionary/calibrated_fivepoint.m deleted file mode 100644 index 149e3d7..0000000 --- a/visionary/calibrated_fivepoint.m +++ /dev/null @@ -1,41 +0,0 @@ -function Evec = calibrated_fivepoint(Q1, Q2, init) -% Evec = calibrated_fivepoint(Q1, Q2, init) -% -% If init == 0 it will not check if there is a compiled version of -% calibrated_fivepoint_helper.c -% -% Q1 end Q2 are correspoding image points. -% -% Code to verify that it works: -% Q1 = rand(3,5); -% Q2 = rand(3,5); -% Evec = calibrated_fivepoint( Q1,Q2); -% for i=1:size(Evec,2) -% E = reshape(Evec(:,i),3,3); -% % Check determinant constraint! -% det( E) -% % Check trace constraint -% 2 *E*transpose(E)*E -trace( E*transpose(E))*E -% % Check reprojection errors -% diag( Q1'*E*Q2) -% end -% -% PS: Please note that due to varying standards of which is Q1 and Q2 -% it is very possible that you get essential matrices which are -% the transpose of what your expected. - -if nargin < 3 - init = 1; -end - -if init - try - calibrated_fivepoint_helper(); - catch le; - if strcmp(le.identifier,'MATLAB:UndefinedFunction') - mex calibrated_fivepoint_helper.c - end - end -end - -Evec = calibrated_fivepoint_gb(Q1,Q2); diff --git a/visionary/calibrated_fivepoint_gb.m b/visionary/calibrated_fivepoint_gb.m deleted file mode 100644 index 636e9eb..0000000 --- a/visionary/calibrated_fivepoint_gb.m +++ /dev/null @@ -1,98 +0,0 @@ -function Evec = calibrated_fivepoint_gb(Q1,Q2) -% Function Evec = calibrated_fivepoint_gb(Q1,Q2) -% Henrik Stewenius 20040722 -% -% -% ARTICLE{stewenius-engels-nister-isprsj-2006, -% AUTHOR = {H. Stew\'enius and C. Engels and D. Nist\'er}, -% TITLE = {Recent Developments on Direct Relative Orientation}, -% JOURNAL = {ISPRS Journal of Photogrammetry and Remote Sensing}, -% URL = {http://dx.doi.org/10.1016/j.isprsjprs.2006.03.005}, -% VOLUME = {60}, -% ISSUE = {4}, -% PAGES = {284--294}, -% MONTH = JUN, -% CODE = {http://vis.uky.edu/~stewe/FIVEPOINT}, -% PDF = {http://www.vis.uky.edu/~stewe/publications/stewenius_engels_nister_5pt_isprs.pdf}, -% YEAR = 2006 -%} -% -% -% For more information please see: -% Grobner Basis Methods for Minimal Problems in Computer Vision -% Henrik Stewenius, -% PhD Thesis, Lund University, 2005 -% http://www.maths.lth.se/matematiklth/personal/stewe/THESIS/ -% -% -% If this implementation is too slow for your needs please see: -% An Efficient Solution to the Five-Point Relative Pose -% -%@Article{ nister-itpam-04, -% author = {Nist\'er, D.}, -% journal = pami, -% month = {June}, -% number = {6}, -% title = {Problem}, -% pages = {756-770}, -% volume = {26}, -% year = {2004} -%} -% -% -% -% -% Code to veryfy that it works: -% Q1 = rand(3,5); -% Q2 = rand(3,5); -% Evec = calibrated_fivepoint( Q1,Q2); -% for i=1:size(Evec,2) -% E = reshape(Evec(:,i),3,3); -% % Check determinant constraint! -% det( E) -% % Check trace constraint -% 2 *E*transpose(E)*E -trace( E*transpose(E))*E -% % Check reprojection errors -% diag( Q1'*E*Q2) -% end -% -% PS: Please note that due to varying standards of which is Q1 and Q2 -% it is very possible that you get essential matrices which are -% the transpose of what your expected. - - -%1 Pose linear equations for the essential matrix. -Q1 = Q1'; -Q2 = Q2'; - -Q = [Q1(:,1).*Q2(:,1) , ... - Q1(:,2).*Q2(:,1) , ... - Q1(:,3).*Q2(:,1) , ... - Q1(:,1).*Q2(:,2) , ... - Q1(:,2).*Q2(:,2) , ... - Q1(:,3).*Q2(:,2) , ... - Q1(:,1).*Q2(:,3) , ... - Q1(:,2).*Q2(:,3) , ... - Q1(:,3).*Q2(:,3) ] ; - - -[U,S,V] = svd(Q,0); -EE = V(:,6:9); - -A = calibrated_fivepoint_helper( EE ) ; -A = A(:,1:10)\A(:,11:20); -M = -A([1 2 3 5 6 8], :); - -M(7,1) = 1; -M(8,2) = 1; -M(9,4) = 1; -M(10,7) = 1; - -[V,D] = eig(M ); -SOLS = V(7:9,:)./(ones(3,1)*V(10,:)); - -Evec = EE*[SOLS ; ones(1,10 ) ]; -Evec = Evec./ ( ones(9,1)*sqrt(sum( Evec.^2))); - -I = find(not(imag( Evec(1,:) ))); -Evec = Evec(:,I); diff --git a/visionary/calibrated_fivepoint_helper.c b/visionary/calibrated_fivepoint_helper.c deleted file mode 100644 index cd9db55..0000000 --- a/visionary/calibrated_fivepoint_helper.c +++ /dev/null @@ -1,380 +0,0 @@ -/* -Copyright Henrik Stewenius. - - -This Code was written for the paper: - -@ARTICLE{stewenius-engels-nister-isprsj-2006, - AUTHOR = {H. Stew\'enius and C. Engels and D. Nist\'er}, - TITLE = {Recent Developments on Direct Relative Orientation}, - JOURNAL = {ISPRS Journal of Photogrammetry and Remote Sensing}, - URL = {http://dx.doi.org/10.1016/j.isprsjprs.2006.03.005}, - VOLUME = {60}, - ISSUE = {4}, - PAGES = {284--294}, - MONTH = JUN, - CODE = {http://vis.uky.edu/~stewe/FIVEPOINT}, - PDF = {http://www.vis.uky.edu/~stewe/publications/stewenius_engels_nister_5pt_isprs.pdf}, - YEAR = 2006 -} - - You are free to use this code for academic research as long as you - refer to this paper. Commercial licence can be negotiated. Though if - you really want to use this in a product there are certain optimizations - which you would want. -*/ - - - -/************************************************* - Indata : 10x4 matrix - Data out: 10x20 matrix -*************************************************/ -#include "mex.h" -#include - - -void mexFunction(int nlhs, mxArray *plhs[], - int nrhs, const mxArray *prhs[]) -/*nlhs number of expected mxArrays. - plhs pointer to an array of NULL pointers. - nrhs number of input mxArrays. - prhs pointer to an array of input mxArrays. */ -{ - double *EE, *A; - double e00,e01,e02,e03,e04,e05,e06,e07,e08; - double e10,e11,e12,e13,e14,e15,e16,e17,e18; - double e20,e21,e22,e23,e24,e25,e26,e27,e28; - double e30,e31,e32,e33,e34,e35,e36,e37,e38; - - double e002,e012,e022,e032,e042,e052,e062,e072,e082; - double e102,e112,e122,e132,e142,e152,e162,e172,e182; - double e202,e212,e222,e232,e242,e252,e262,e272,e282; - double e302,e312,e322,e332,e342,e352,e362,e372,e382; - - double e003,e013,e023,e033,e043,e053,e063,e073,e083; - double e103,e113,e123,e133,e143,e153,e163,e173,e183; - double e203,e213,e223,e233,e243,e253,e263,e273,e283; - double e303,e313,e323,e333,e343,e353,e363,e373,e383; - - if(nrhs != 1 ) mexErrMsgTxt("EE (10*4)\n"); - EE = mxGetPr( prhs[0] ) ; - plhs[0] = mxCreateDoubleMatrix(10 ,20 , mxREAL); - A = mxGetPr( plhs[0]); - - -e00 = EE[0*9 + 0 ]; -e10 = EE[1*9 + 0 ]; -e20 = EE[2*9 + 0 ]; -e30 = EE[3*9 + 0 ]; -e01 = EE[0*9 + 1 ]; -e11 = EE[1*9 + 1 ]; -e21 = EE[2*9 + 1 ]; -e31 = EE[3*9 + 1 ]; -e02 = EE[0*9 + 2 ]; -e12 = EE[1*9 + 2 ]; -e22 = EE[2*9 + 2 ]; -e32 = EE[3*9 + 2 ]; -e03 = EE[0*9 + 3 ]; -e13 = EE[1*9 + 3 ]; -e23 = EE[2*9 + 3 ]; -e33 = EE[3*9 + 3 ]; -e04 = EE[0*9 + 4 ]; -e14 = EE[1*9 + 4 ]; -e24 = EE[2*9 + 4 ]; -e34 = EE[3*9 + 4 ]; -e05 = EE[0*9 + 5 ]; -e15 = EE[1*9 + 5 ]; -e25 = EE[2*9 + 5 ]; -e35 = EE[3*9 + 5 ]; -e06 = EE[0*9 + 6 ]; -e16 = EE[1*9 + 6 ]; -e26 = EE[2*9 + 6 ]; -e36 = EE[3*9 + 6 ]; -e07 = EE[0*9 + 7 ]; -e17 = EE[1*9 + 7 ]; -e27 = EE[2*9 + 7 ]; -e37 = EE[3*9 + 7 ]; -e08 = EE[0*9 + 8 ]; -e18 = EE[1*9 + 8 ]; -e28 = EE[2*9 + 8 ]; -e38 = EE[3*9 + 8 ]; - - -e002 =e00*e00; -e102 =e10*e10; -e202 =e20*e20; -e302 =e30*e30; -e012 =e01*e01; -e112 =e11*e11; -e212 =e21*e21; -e312 =e31*e31; -e022 =e02*e02; -e122 =e12*e12; -e222 =e22*e22; -e322 =e32*e32; -e032 =e03*e03; -e132 =e13*e13; -e232 =e23*e23; -e332 =e33*e33; -e042 =e04*e04; -e142 =e14*e14; -e242 =e24*e24; -e342 =e34*e34; -e052 =e05*e05; -e152 =e15*e15; -e252 =e25*e25; -e352 =e35*e35; -e062 =e06*e06; -e162 =e16*e16; -e262 =e26*e26; -e362 =e36*e36; -e072 =e07*e07; -e172 =e17*e17; -e272 =e27*e27; -e372 =e37*e37; -e082 =e08*e08; -e182 =e18*e18; -e282 =e28*e28; -e382 =e38*e38; - -e003 =e00*e00*e00; -e103 =e10*e10*e10; -e203 =e20*e20*e20; -e303 =e30*e30*e30; -e013 =e01*e01*e01; -e113 =e11*e11*e11; -e213 =e21*e21*e21; -e313 =e31*e31*e31; -e023 =e02*e02*e02; -e123 =e12*e12*e12; -e223 =e22*e22*e22; -e323 =e32*e32*e32; -e033 =e03*e03*e03; -e133 =e13*e13*e13; -e233 =e23*e23*e23; -e333 =e33*e33*e33; -e043 =e04*e04*e04; -e143 =e14*e14*e14; -e243 =e24*e24*e24; -e343 =e34*e34*e34; -e053 =e05*e05*e05; -e153 =e15*e15*e15; -e253 =e25*e25*e25; -e353 =e35*e35*e35; -e063 =e06*e06*e06; -e163 =e16*e16*e16; -e263 =e26*e26*e26; -e363 =e36*e36*e36; -e073 =e07*e07*e07; -e173 =e17*e17*e17; -e273 =e27*e27*e27; -e373 =e37*e37*e37; -e083 =e08*e08*e08; -e183 =e18*e18*e18; -e283 =e28*e28*e28; -e383 =e38*e38*e38; - - -A[0 + 10*0]=0.5*e003+0.5*e00*e012+0.5*e00*e022+0.5*e00*e032+e03*e01*e04+e03*e02*e05+0.5*e00*e062+e06*e01*e07+e06*e02*e08-0.5*e00*e042-0.5*e00*e052-0.5*e00*e072-0.5*e00*e082; -A[0 + 10*1]=e00*e11*e01+e00*e12*e02+e03*e00*e13+e03*e11*e04+e03*e01*e14+e03*e12*e05+e03*e02*e15+e13*e01*e04+e13*e02*e05+e06*e00*e16+1.5*e10*e002+0.5*e10*e012+0.5*e10*e022+0.5*e10*e062-0.5*e10*e042-0.5*e10*e052-0.5*e10*e072+0.5*e10*e032+e06*e11*e07+e06*e01*e17+e06*e12*e08+e06*e02*e18+e16*e01*e07+e16*e02*e08-e00*e14*e04-e00*e17*e07-e00*e15*e05-e00*e18*e08-0.5*e10*e082; -A[0 + 10*2]=e16*e02*e18+e03*e12*e15+e10*e11*e01+e10*e12*e02+e03*e10*e13+e03*e11*e14+e13*e11*e04+e13*e01*e14+e13*e12*e05+e13*e02*e15+e06*e10*e16+e06*e12*e18+e06*e11*e17+e16*e11*e07+e16*e01*e17+e16*e12*e08-e10*e14*e04-e10*e17*e07-e10*e15*e05-e10*e18*e08+1.5*e00*e102+0.5*e00*e122+0.5*e00*e112+0.5*e00*e132+0.5*e00*e162-0.5*e00*e152-0.5*e00*e172-0.5*e00*e182-0.5*e00*e142; -A[0 + 10*3]=0.5*e103+0.5*e10*e122+0.5*e10*e112+0.5*e10*e132+e13*e12*e15+e13*e11*e14+0.5*e10*e162+e16*e12*e18+e16*e11*e17-0.5*e10*e152-0.5*e10*e172-0.5*e10*e182-0.5*e10*e142; -A[0 + 10*4]=-e00*e28*e08-e00*e25*e05-e00*e27*e07-e00*e24*e04+e26*e02*e08+e26*e01*e07+e06*e02*e28+e06*e22*e08+e06*e01*e27+e06*e21*e07+e23*e02*e05+e23*e01*e04+e03*e02*e25+e03*e22*e05+e03*e01*e24+e03*e21*e04+e00*e22*e02+e00*e21*e01-0.5*e20*e082-0.5*e20*e052-0.5*e20*e072-0.5*e20*e042+e06*e00*e26+0.5*e20*e062+e03*e00*e23+0.5*e20*e022+1.5*e20*e002+0.5*e20*e032+0.5*e20*e012; -A[0 + 10*5]=-e10*e24*e04-e10*e27*e07-e10*e25*e05-e10*e28*e08-e20*e14*e04-e20*e17*e07-e20*e15*e05-e20*e18*e08-e00*e24*e14-e00*e25*e15-e00*e27*e17-e00*e28*e18+e06*e21*e17+e06*e22*e18+e06*e12*e28+e16*e00*e26+e16*e21*e07+e16*e01*e27+e16*e22*e08+e16*e02*e28+e26*e11*e07+e26*e01*e17+e26*e12*e08+e26*e02*e18+e06*e11*e27+e23*e11*e04+e23*e01*e14+e23*e12*e05+e23*e02*e15+e06*e20*e16+e06*e10*e26+e03*e21*e14+e03*e22*e15+e03*e12*e25+e13*e00*e23+e13*e21*e04+e13*e01*e24+e13*e22*e05+e13*e02*e25+e03*e11*e24+e03*e20*e13+e03*e10*e23+e00*e21*e11+3*e00*e20*e10+e00*e22*e12+e20*e12*e02+e20*e11*e01+e10*e22*e02+e10*e21*e01; -A[0 + 10*6]=-0.5*e20*e152+e26*e11*e17-e10*e24*e14-e10*e25*e15-e10*e27*e17-e10*e28*e18+0.5*e20*e162+e13*e10*e23+e13*e22*e15+e23*e12*e15+e23*e11*e14+e16*e10*e26+e16*e21*e17+e16*e11*e27+e16*e22*e18+e16*e12*e28+e26*e12*e18+e13*e12*e25+0.5*e20*e132+1.5*e20*e102+0.5*e20*e122+0.5*e20*e112+e10*e21*e11+e10*e22*e12+e13*e11*e24-0.5*e20*e172-0.5*e20*e182-0.5*e20*e142+e13*e21*e14; -A[0 + 10*7]=-e20*e25*e05-e20*e28*e08-0.5*e00*e272-0.5*e00*e282-0.5*e00*e242+0.5*e00*e262-0.5*e00*e252+e06*e20*e26+0.5*e00*e232+e06*e22*e28+e06*e21*e27+e26*e21*e07+e26*e01*e27+e26*e22*e08+e26*e02*e28-e20*e24*e04-e20*e27*e07+e03*e20*e23+e03*e22*e25+e03*e21*e24+e23*e21*e04+e23*e01*e24+e23*e22*e05+e23*e02*e25+e20*e21*e01+e20*e22*e02+1.5*e00*e202+0.5*e00*e222+0.5*e00*e212; -A[0 + 10*8]=e23*e21*e14+e23*e11*e24+e23*e22*e15+e23*e12*e25+e16*e20*e26+e16*e22*e28+e16*e21*e27+e26*e21*e17+e26*e11*e27+e26*e22*e18+e26*e12*e28+1.5*e10*e202+0.5*e10*e222+0.5*e10*e212+0.5*e10*e232+e20*e21*e11+e20*e22*e12+e13*e20*e23+e13*e22*e25+e13*e21*e24-e20*e24*e14-e20*e25*e15-e20*e27*e17-e20*e28*e18-0.5*e10*e272-0.5*e10*e282-0.5*e10*e242-0.5*e10*e252+0.5*e10*e262; -A[0 + 10*9]=0.5*e203+0.5*e20*e222+0.5*e20*e212+0.5*e20*e232+e23*e22*e25+e23*e21*e24+0.5*e20*e262+e26*e22*e28+e26*e21*e27-0.5*e20*e252-0.5*e20*e272-0.5*e20*e282-0.5*e20*e242; -A[0 + 10*10]=e06*e32*e08-0.5*e30*e082-0.5*e30*e042-0.5*e30*e052-0.5*e30*e072+0.5*e30*e012+0.5*e30*e022+0.5*e30*e032+0.5*e30*e062+1.5*e30*e002+e00*e31*e01+e00*e32*e02+e03*e31*e04+e03*e01*e34+e03*e32*e05+e03*e02*e35+e33*e01*e04+e33*e02*e05+e06*e00*e36+e06*e31*e07+e06*e01*e37+e06*e02*e38+e36*e01*e07+e36*e02*e08-e00*e34*e04-e00*e37*e07-e00*e35*e05-e00*e38*e08+e03*e00*e33; -A[0 + 10*11]=e06*e30*e16+e03*e30*e13+e16*e31*e07+e06*e10*e36-e10*e37*e07+3*e00*e30*e10+e00*e32*e12-e00*e38*e18-e10*e34*e04-e10*e35*e05-e10*e38*e08-e30*e14*e04-e30*e17*e07-e30*e15*e05-e30*e18*e08+e00*e31*e11+e10*e31*e01+e10*e32*e02+e30*e11*e01+e30*e12*e02+e03*e10*e33-e00*e34*e14-e00*e35*e15-e00*e37*e17+e03*e31*e14+e03*e11*e34+e03*e32*e15+e03*e12*e35+e13*e00*e33+e13*e31*e04+e13*e01*e34+e13*e32*e05+e13*e02*e35+e33*e11*e04+e33*e01*e14+e33*e12*e05+e33*e02*e15+e06*e31*e17+e06*e11*e37+e06*e32*e18+e06*e12*e38+e16*e00*e36+e16*e01*e37+e16*e32*e08+e16*e02*e38+e36*e11*e07+e36*e01*e17+e36*e12*e08+e36*e02*e18; -A[0 + 10*12]=e13*e10*e33+e33*e11*e14+e16*e10*e36+e16*e31*e17+e16*e11*e37+e16*e32*e18+e16*e12*e38+e36*e12*e18+e36*e11*e17-e10*e34*e14-e10*e35*e15-e10*e37*e17-e10*e38*e18+e10*e31*e11+e10*e32*e12+e13*e31*e14+e13*e11*e34+e13*e32*e15+e13*e12*e35+e33*e12*e15+1.5*e30*e102+0.5*e30*e122+0.5*e30*e112+0.5*e30*e132+0.5*e30*e162-0.5*e30*e152-0.5*e30*e172-0.5*e30*e182-0.5*e30*e142; -A[0 + 10*13]=e00*e32*e22+3*e00*e30*e20+e00*e31*e21+e20*e31*e01+e20*e32*e02+e30*e21*e01+e30*e22*e02+e03*e20*e33+e03*e32*e25+e03*e22*e35+e03*e31*e24+e03*e21*e34+e23*e00*e33+e23*e31*e04+e23*e01*e34+e23*e32*e05+e23*e02*e35+e33*e21*e04+e33*e01*e24+e33*e22*e05+e33*e02*e25+e06*e30*e26+e06*e20*e36+e06*e32*e28+e06*e22*e38+e06*e31*e27+e06*e21*e37+e26*e00*e36+e26*e31*e07+e03*e30*e23+e26*e01*e37+e26*e32*e08+e26*e02*e38+e36*e21*e07+e36*e01*e27+e36*e22*e08+e36*e02*e28-e00*e35*e25-e00*e37*e27-e00*e38*e28-e00*e34*e24-e20*e34*e04-e20*e37*e07-e20*e35*e05-e20*e38*e08-e30*e24*e04-e30*e27*e07-e30*e25*e05-e30*e28*e08; -A[0 + 10*14]=e16*e30*e26+e13*e21*e34+3*e10*e30*e20+e10*e32*e22+e10*e31*e21+e20*e31*e11+e20*e32*e12+e30*e21*e11+e30*e22*e12+e13*e30*e23+e13*e20*e33+e13*e32*e25+e13*e22*e35+e13*e31*e24+e23*e10*e33+e23*e31*e14+e23*e11*e34+e23*e32*e15+e23*e12*e35+e33*e21*e14+e33*e11*e24+e33*e22*e15+e33*e12*e25+e16*e20*e36+e16*e32*e28+e16*e22*e38+e16*e31*e27+e16*e21*e37+e26*e10*e36+e26*e31*e17+e26*e11*e37+e26*e32*e18+e26*e12*e38+e36*e21*e17+e36*e11*e27+e36*e22*e18+e36*e12*e28-e10*e35*e25-e10*e37*e27-e10*e38*e28-e10*e34*e24-e20*e34*e14-e20*e35*e15-e20*e37*e17-e20*e38*e18-e30*e24*e14-e30*e25*e15-e30*e27*e17-e30*e28*e18; -A[0 + 10*15]=-e20*e34*e24+0.5*e30*e262-0.5*e30*e252-0.5*e30*e272-0.5*e30*e282-0.5*e30*e242+1.5*e30*e202+0.5*e30*e222+0.5*e30*e212+0.5*e30*e232+e20*e32*e22+e20*e31*e21+e23*e20*e33+e23*e32*e25+e23*e22*e35+e23*e31*e24+e23*e21*e34+e33*e22*e25+e33*e21*e24+e26*e20*e36+e26*e32*e28+e26*e22*e38+e26*e31*e27+e26*e21*e37+e36*e22*e28+e36*e21*e27-e20*e35*e25-e20*e37*e27-e20*e38*e28; -A[0 + 10*16]=0.5*e00*e322+e30*e32*e02+e30*e31*e01+1.5*e00*e302+0.5*e00*e312+e03*e32*e35+e33*e31*e04+e33*e01*e34+e33*e32*e05+e33*e02*e35+e06*e30*e36+e06*e31*e37+e06*e32*e38+e36*e31*e07+e36*e01*e37+e36*e32*e08+e36*e02*e38-e30*e34*e04-e30*e37*e07-e30*e35*e05-e30*e38*e08+0.5*e00*e332+0.5*e00*e362-0.5*e00*e382-0.5*e00*e352-0.5*e00*e342-0.5*e00*e372+e03*e30*e33+e03*e31*e34; -A[0 + 10*17]=0.5*e10*e362-0.5*e10*e382-0.5*e10*e352-0.5*e10*e342-0.5*e10*e372+e36*e31*e17+e36*e11*e37+e36*e32*e18+e36*e12*e38-e30*e34*e14-e30*e35*e15-e30*e37*e17-e30*e38*e18+1.5*e10*e302+0.5*e10*e312+0.5*e10*e322+0.5*e10*e332+e30*e31*e11+e30*e32*e12+e13*e30*e33+e13*e31*e34+e13*e32*e35+e33*e31*e14+e33*e11*e34+e33*e32*e15+e33*e12*e35+e16*e30*e36+e16*e31*e37+e16*e32*e38; -A[0 + 10*18]=e33*e31*e24+e33*e21*e34+e26*e30*e36+e26*e31*e37+e26*e32*e38+e36*e32*e28+e36*e22*e38+e36*e31*e27+e36*e21*e37-e30*e35*e25-e30*e37*e27-e30*e38*e28-e30*e34*e24+e33*e22*e35+1.5*e20*e302+0.5*e20*e312+0.5*e20*e322+0.5*e20*e332+0.5*e20*e362-0.5*e20*e382-0.5*e20*e352-0.5*e20*e342-0.5*e20*e372+e30*e32*e22+e30*e31*e21+e23*e30*e33+e23*e31*e34+e23*e32*e35+e33*e32*e25; -A[0 + 10*19]=0.5*e303+0.5*e30*e312+0.5*e30*e322+0.5*e30*e332+e33*e31*e34+e33*e32*e35+0.5*e30*e362+e36*e31*e37+e36*e32*e38-0.5*e30*e382-0.5*e30*e352-0.5*e30*e342-0.5*e30*e372; -A[1 + 10*0]=e00*e01*e04+0.5*e002*e03+e00*e02*e05+0.5*e033+0.5*e03*e042+0.5*e03*e052+0.5*e03*e062+e06*e04*e07+e06*e05*e08-0.5*e03*e012-0.5*e03*e072-0.5*e03*e022-0.5*e03*e082; -A[1 + 10*1]=e03*e14*e04+e10*e01*e04+e16*e05*e08+e00*e10*e03+e00*e11*e04+e00*e01*e14+e00*e12*e05+e00*e02*e15+e10*e02*e05+e03*e15*e05+e06*e03*e16+e06*e14*e07+e06*e04*e17+e06*e15*e08+e06*e05*e18+0.5*e002*e13+1.5*e13*e032+0.5*e13*e042+0.5*e13*e052+0.5*e13*e062-0.5*e13*e012-0.5*e13*e072-0.5*e13*e022-0.5*e13*e082+e16*e04*e07-e03*e12*e02-e03*e11*e01-e03*e17*e07-e03*e18*e08; -A[1 + 10*2]=-e13*e11*e01+e00*e10*e13+e00*e12*e15+e00*e11*e14+e10*e11*e04+e10*e01*e14+e10*e12*e05+e10*e02*e15+e13*e14*e04+e13*e15*e05+e06*e13*e16+e06*e15*e18+e06*e14*e17+e16*e14*e07+e16*e04*e17+e16*e15*e08+e16*e05*e18-e13*e12*e02-e13*e17*e07-e13*e18*e08+0.5*e102*e03+1.5*e03*e132+0.5*e03*e152+0.5*e03*e142+0.5*e03*e162-0.5*e03*e112-0.5*e03*e172-0.5*e03*e122-0.5*e03*e182; -A[1 + 10*3]=0.5*e102*e13+e10*e11*e14+e10*e12*e15+0.5*e133+0.5*e13*e152+0.5*e13*e142+0.5*e13*e162+e16*e15*e18+e16*e14*e17-0.5*e13*e112-0.5*e13*e122-0.5*e13*e172-0.5*e13*e182; -A[1 + 10*4]=-e03*e28*e08-e03*e27*e07-e03*e21*e01-e03*e22*e02+e26*e05*e08+e26*e04*e07+e06*e05*e28+e06*e25*e08+e06*e04*e27+e06*e24*e07+e03*e25*e05+e03*e24*e04+e20*e02*e05+e20*e01*e04+e00*e02*e25+e00*e22*e05+e00*e01*e24+e00*e21*e04+e00*e20*e03-0.5*e23*e072-0.5*e23*e082-0.5*e23*e022-0.5*e23*e012+e06*e03*e26+0.5*e23*e052+0.5*e23*e062+1.5*e23*e032+0.5*e23*e042+0.5*e002*e23; -A[1 + 10*5]=e00*e21*e14+e00*e11*e24+e00*e10*e23+e00*e22*e15+e00*e12*e25+e20*e12*e05+e20*e01*e14+e20*e11*e04+e00*e20*e13+e10*e02*e25+e10*e22*e05+e10*e01*e24+e10*e21*e04+e10*e20*e03+e23*e15*e05+e23*e14*e04+e13*e25*e05+e13*e24*e04+e03*e24*e14+e03*e25*e15+3*e03*e23*e13+e20*e02*e15+e16*e03*e26+e06*e14*e27-e23*e18*e08+e06*e24*e17+e06*e15*e28+e06*e25*e18+e06*e13*e26+e06*e23*e16+e26*e04*e17+e26*e14*e07+e16*e05*e28+e16*e25*e08+e16*e04*e27+e16*e24*e07-e03*e22*e12-e03*e21*e11+e26*e05*e18+e26*e15*e08-e03*e27*e17-e03*e28*e18-e13*e22*e02-e13*e28*e08-e13*e27*e07-e13*e21*e01-e23*e17*e07-e23*e11*e01-e23*e12*e02; -A[1 + 10*6]=-0.5*e23*e182-0.5*e23*e172-0.5*e23*e112-0.5*e23*e122-e13*e22*e12-e13*e27*e17-e13*e28*e18+e26*e15*e18+e26*e14*e17-e13*e21*e11+e20*e12*e15+e13*e25*e15+e13*e24*e14+e16*e13*e26+e16*e25*e18+e16*e15*e28+e16*e24*e17+e16*e14*e27+1.5*e23*e132+0.5*e23*e152+0.5*e23*e142+0.5*e23*e162+e10*e20*e13+e10*e21*e14+e10*e11*e24+e10*e22*e15+e10*e12*e25+e20*e11*e14+0.5*e102*e23; -A[1 + 10*7]=e26*e04*e27+e00*e22*e25-e23*e28*e08+0.5*e03*e262-0.5*e03*e212-0.5*e03*e272-0.5*e03*e222-0.5*e03*e282+e23*e24*e04+e23*e25*e05+0.5*e202*e03+e06*e23*e26+e06*e24*e27+e06*e25*e28+e26*e24*e07+e26*e25*e08+e26*e05*e28-e23*e22*e02-e23*e21*e01-e23*e27*e07+e00*e20*e23+e00*e21*e24+e20*e21*e04+e20*e01*e24+e20*e22*e05+e20*e02*e25+1.5*e03*e232+0.5*e03*e242+0.5*e03*e252; -A[1 + 10*8]=e20*e11*e24-0.5*e13*e212-0.5*e13*e272-0.5*e13*e222-0.5*e13*e282-e23*e27*e17-e23*e28*e18+e26*e25*e18+e26*e24*e17+e26*e14*e27-e23*e21*e11-e23*e22*e12+e26*e15*e28+e23*e25*e15+e23*e24*e14+e16*e23*e26+e16*e24*e27+e16*e25*e28+0.5*e13*e262+e20*e21*e14+e20*e22*e15+e20*e12*e25+0.5*e13*e242+0.5*e13*e252+0.5*e202*e13+1.5*e13*e232+e10*e20*e23+e10*e22*e25+e10*e21*e24; -A[1 + 10*9]=0.5*e202*e23+e20*e22*e25+e20*e21*e24+0.5*e233+0.5*e23*e242+0.5*e23*e252+0.5*e23*e262+e26*e24*e27+e26*e25*e28-0.5*e23*e212-0.5*e23*e272-0.5*e23*e222-0.5*e23*e282; -A[1 + 10*10]=e00*e30*e03+0.5*e33*e062-0.5*e33*e012-0.5*e33*e022-0.5*e33*e072+e03*e35*e05+e06*e03*e36+e06*e34*e07+e06*e04*e37+e06*e35*e08+e06*e05*e38+e36*e04*e07+e36*e05*e08-e03*e32*e02-e03*e31*e01-e03*e37*e07+e00*e31*e04+e00*e01*e34+e00*e32*e05+e00*e02*e35+e30*e01*e04+e30*e02*e05+e03*e34*e04-e03*e38*e08+0.5*e002*e33+1.5*e33*e032+0.5*e33*e042+0.5*e33*e052-0.5*e33*e082; -A[1 + 10*11]=e06*e35*e18+e06*e33*e16+e00*e30*e13+e00*e10*e33+e00*e31*e14+e00*e11*e34+e00*e32*e15+e00*e12*e35+e10*e30*e03-e33*e17*e07-e33*e18*e08+e10*e31*e04+e10*e01*e34+e10*e32*e05+e10*e02*e35+e30*e11*e04+e30*e01*e14+e30*e12*e05+e30*e02*e15+3*e03*e33*e13+e03*e35*e15+e03*e34*e14+e13*e34*e04+e13*e35*e05+e33*e14*e04+e33*e15*e05+e06*e13*e36+e06*e15*e38+e06*e34*e17+e06*e14*e37+e16*e03*e36+e16*e34*e07+e16*e04*e37+e16*e35*e08+e16*e05*e38+e36*e14*e07+e36*e04*e17+e36*e15*e08+e36*e05*e18-e03*e31*e11-e03*e32*e12-e03*e37*e17-e03*e38*e18-e13*e32*e02-e13*e31*e01-e13*e37*e07-e13*e38*e08-e33*e12*e02-e33*e11*e01; -A[1 + 10*12]=e16*e13*e36+e10*e11*e34+0.5*e33*e152+0.5*e33*e142+0.5*e33*e162-0.5*e33*e112-0.5*e33*e122-0.5*e33*e172-0.5*e33*e182+0.5*e102*e33+1.5*e33*e132+e10*e30*e13+e10*e31*e14+e10*e32*e15+e10*e12*e35+e30*e11*e14+e30*e12*e15+e13*e35*e15+e13*e34*e14+e16*e35*e18+e16*e15*e38+e16*e34*e17+e16*e14*e37+e36*e15*e18+e36*e14*e17-e13*e31*e11-e13*e32*e12-e13*e37*e17-e13*e38*e18; -A[1 + 10*13]=e06*e35*e28+e36*e04*e27+e00*e20*e33+e00*e30*e23+3*e03*e33*e23+e03*e34*e24+e03*e35*e25+e23*e34*e04+e23*e35*e05+e33*e24*e04+e33*e25*e05+e06*e33*e26+e06*e23*e36+e06*e34*e27+e06*e24*e37+e06*e25*e38+e26*e03*e36+e26*e34*e07+e26*e04*e37+e26*e35*e08+e26*e05*e38+e36*e24*e07+e36*e25*e08+e36*e05*e28-e03*e31*e21-e03*e37*e27-e03*e32*e22-e03*e38*e28-e23*e32*e02-e23*e31*e01-e23*e37*e07-e23*e38*e08-e33*e22*e02-e33*e21*e01-e33*e27*e07-e33*e28*e08+e00*e32*e25+e00*e22*e35+e00*e31*e24+e00*e21*e34+e20*e30*e03+e20*e31*e04+e20*e01*e34+e20*e32*e05+e20*e02*e35+e30*e21*e04+e30*e01*e24+e30*e22*e05+e30*e02*e25; -A[1 + 10*14]=e10*e30*e23+e10*e20*e33+e10*e22*e35+e10*e32*e25+e10*e31*e24+e10*e21*e34+e20*e30*e13+e20*e31*e14+e20*e11*e34+e20*e32*e15+e20*e12*e35+e30*e21*e14+e30*e11*e24+e30*e22*e15+e30*e12*e25+3*e13*e33*e23+e13*e34*e24+e13*e35*e25+e23*e35*e15+e23*e34*e14+e33*e25*e15+e33*e24*e14+e16*e33*e26+e16*e23*e36+e16*e34*e27+e16*e24*e37+e16*e35*e28+e16*e25*e38+e26*e13*e36+e26*e35*e18+e26*e15*e38+e26*e34*e17+e26*e14*e37+e36*e25*e18+e36*e15*e28+e36*e24*e17+e36*e14*e27-e13*e31*e21-e13*e37*e27-e13*e32*e22-e13*e38*e28-e23*e31*e11-e23*e32*e12-e23*e37*e17-e23*e38*e18-e33*e21*e11-e33*e22*e12-e33*e27*e17-e33*e28*e18; -A[1 + 10*15]=-0.5*e33*e212-0.5*e33*e272-0.5*e33*e222-0.5*e33*e282+e26*e23*e36+e20*e30*e23+e20*e32*e25+e20*e22*e35+e20*e31*e24+e20*e21*e34+e30*e22*e25+e30*e21*e24+e23*e34*e24+e23*e35*e25+e26*e34*e27+e26*e24*e37+e26*e35*e28+e26*e25*e38+e36*e24*e27+e36*e25*e28-e23*e31*e21-e23*e37*e27-e23*e32*e22-e23*e38*e28+0.5*e202*e33+1.5*e33*e232+0.5*e33*e242+0.5*e33*e252+0.5*e33*e262; -A[1 + 10*16]=e33*e35*e05+e30*e32*e05+0.5*e03*e362+0.5*e302*e03+1.5*e03*e332+0.5*e03*e352+0.5*e03*e342+e00*e30*e33+e00*e31*e34+e00*e32*e35+e30*e31*e04+e30*e01*e34+e30*e02*e35+e33*e34*e04+e06*e33*e36+e06*e35*e38+e06*e34*e37+e36*e34*e07+e36*e04*e37+e36*e35*e08+e36*e05*e38-e33*e32*e02-e33*e31*e01-e33*e37*e07-e33*e38*e08-0.5*e03*e322-0.5*e03*e382-0.5*e03*e312-0.5*e03*e372; -A[1 + 10*17]=-e33*e31*e11-e33*e32*e12-e33*e38*e18+e30*e11*e34+e30*e32*e15+e30*e12*e35+e33*e35*e15+e33*e34*e14+e16*e33*e36+e16*e35*e38+e16*e34*e37+e36*e35*e18+e36*e15*e38+e36*e34*e17+e36*e14*e37-e33*e37*e17+0.5*e302*e13+1.5*e13*e332+0.5*e13*e352+0.5*e13*e342+0.5*e13*e362-0.5*e13*e322-0.5*e13*e382-0.5*e13*e312-0.5*e13*e372+e10*e30*e33+e10*e31*e34+e10*e32*e35+e30*e31*e14; -A[1 + 10*18]=e36*e25*e38+0.5*e302*e23+1.5*e23*e332+0.5*e23*e352+0.5*e23*e342+0.5*e23*e362-0.5*e23*e322-0.5*e23*e382-0.5*e23*e312-0.5*e23*e372+e20*e30*e33+e20*e31*e34+e20*e32*e35+e30*e32*e25+e30*e22*e35+e30*e31*e24+e30*e21*e34+e33*e34*e24+e33*e35*e25+e26*e33*e36+e26*e35*e38+e26*e34*e37+e36*e34*e27+e36*e24*e37+e36*e35*e28-e33*e31*e21-e33*e37*e27-e33*e32*e22-e33*e38*e28; -A[1 + 10*19]=0.5*e302*e33+e30*e31*e34+e30*e32*e35+0.5*e333+0.5*e33*e352+0.5*e33*e342+0.5*e33*e362+e36*e35*e38+e36*e34*e37-0.5*e33*e322-0.5*e33*e382-0.5*e33*e312-0.5*e33*e372; -A[2 + 10*0]=0.5*e002*e06+e00*e01*e07+e00*e02*e08+0.5*e032*e06+e03*e04*e07+e03*e05*e08+0.5*e063+0.5*e06*e072+0.5*e06*e082-0.5*e06*e012-0.5*e06*e022-0.5*e06*e042-0.5*e06*e052; -A[2 + 10*1]=e00*e10*e06+0.5*e002*e16+0.5*e032*e16+1.5*e16*e062+0.5*e16*e072+0.5*e16*e082-0.5*e16*e012-0.5*e16*e022-0.5*e16*e042-0.5*e16*e052+e00*e11*e07+e00*e01*e17+e00*e12*e08+e00*e02*e18+e10*e01*e07+e10*e02*e08+e03*e13*e06+e03*e14*e07+e03*e04*e17+e03*e15*e08+e03*e05*e18+e13*e04*e07+e13*e05*e08+e06*e17*e07+e06*e18*e08-e06*e12*e02-e06*e11*e01-e06*e14*e04-e06*e15*e05; -A[2 + 10*2]=e13*e14*e07+0.5*e102*e06+e00*e10*e16+e00*e12*e18+e00*e11*e17+e10*e11*e07+e10*e01*e17+e10*e12*e08+e10*e02*e18+e03*e13*e16+e03*e15*e18+e03*e14*e17+e13*e04*e17+e13*e15*e08+e13*e05*e18+e16*e17*e07+e16*e18*e08-e16*e12*e02-e16*e11*e01-e16*e14*e04-e16*e15*e05+0.5*e132*e06+1.5*e06*e162+0.5*e06*e182+0.5*e06*e172-0.5*e06*e112-0.5*e06*e122-0.5*e06*e142-0.5*e06*e152; -A[2 + 10*3]=0.5*e102*e16+e10*e12*e18+e10*e11*e17+0.5*e132*e16+e13*e15*e18+e13*e14*e17+0.5*e163+0.5*e16*e182+0.5*e16*e172-0.5*e16*e112-0.5*e16*e122-0.5*e16*e142-0.5*e16*e152; -A[2 + 10*4]=e06*e27*e07+e23*e05*e08+e23*e04*e07+e03*e05*e28+e03*e25*e08+e03*e04*e27+e03*e24*e07+e20*e02*e08+e20*e01*e07+e00*e02*e28+e00*e22*e08+e00*e01*e27+e00*e21*e07+e00*e20*e06-e06*e25*e05-e06*e24*e04-e06*e21*e01-e06*e22*e02+e06*e28*e08-0.5*e26*e042-0.5*e26*e052-0.5*e26*e012-0.5*e26*e022+0.5*e26*e082+0.5*e26*e072+1.5*e26*e062+0.5*e002*e26+e03*e23*e06+0.5*e032*e26; -A[2 + 10*5]=e13*e05*e28+e00*e12*e28+e13*e25*e08+e13*e04*e27+e13*e24*e07+e13*e23*e06+e03*e14*e27+e03*e24*e17+e03*e15*e28+e03*e25*e18+e03*e13*e26+e03*e23*e16+e20*e02*e18+e20*e12*e08+e20*e01*e17+e20*e11*e07+e00*e21*e17+e10*e02*e28+e10*e22*e08+e10*e01*e27+e10*e21*e07+e10*e20*e06+e00*e11*e27-e26*e15*e05-e26*e14*e04-e26*e11*e01-e26*e12*e02-e16*e25*e05-e16*e24*e04-e16*e21*e01-e16*e22*e02-e06*e24*e14-e06*e22*e12-e06*e21*e11-e06*e25*e15+e00*e20*e16+e00*e22*e18+e00*e10*e26+e26*e18*e08+e26*e17*e07+e16*e28*e08+e16*e27*e07+e06*e27*e17+e06*e28*e18+3*e06*e26*e16+e23*e05*e18+e23*e15*e08+e23*e04*e17+e23*e14*e07; -A[2 + 10*6]=e10*e22*e18+0.5*e26*e182+0.5*e26*e172+e16*e28*e18+e16*e27*e17-e16*e25*e15-e16*e21*e11-e16*e22*e12+1.5*e26*e162+e13*e15*e28+e13*e24*e17+e13*e14*e27+e23*e15*e18+e23*e14*e17+e10*e12*e28+e10*e21*e17+e10*e11*e27+e20*e12*e18+e20*e11*e17+e13*e23*e16+e13*e25*e18+e10*e20*e16+0.5*e102*e26-0.5*e26*e122-0.5*e26*e142-0.5*e26*e152-e16*e24*e14-0.5*e26*e112+0.5*e132*e26; -A[2 + 10*7]=-0.5*e06*e212-0.5*e06*e252-0.5*e06*e242+0.5*e06*e272+0.5*e06*e282-0.5*e06*e222+e20*e02*e28+e03*e23*e26+e03*e24*e27+e03*e25*e28+e23*e24*e07+e23*e04*e27+e23*e25*e08+e23*e05*e28+e26*e28*e08-e26*e22*e02-e26*e21*e01-e26*e24*e04-e26*e25*e05+e26*e27*e07+e00*e20*e26+e00*e21*e27+e00*e22*e28+e20*e21*e07+e20*e01*e27+e20*e22*e08+0.5*e202*e06+0.5*e232*e06+1.5*e06*e262; -A[2 + 10*8]=-e26*e24*e14-0.5*e16*e212-0.5*e16*e252-0.5*e16*e242-e26*e25*e15-0.5*e16*e222-e26*e21*e11+e26*e28*e18+e26*e27*e17-e26*e22*e12+e23*e15*e28+e23*e24*e17+e23*e14*e27+0.5*e232*e16+1.5*e16*e262+0.5*e16*e272+0.5*e16*e282+e10*e20*e26+e10*e21*e27+e10*e22*e28+e20*e22*e18+e20*e12*e28+e20*e21*e17+e20*e11*e27+e13*e23*e26+e13*e24*e27+e13*e25*e28+e23*e25*e18+0.5*e202*e16; -A[2 + 10*9]=0.5*e202*e26+e20*e21*e27+e20*e22*e28+0.5*e232*e26+e23*e24*e27+e23*e25*e28+0.5*e263+0.5*e26*e272+0.5*e26*e282-0.5*e26*e222-0.5*e26*e212-0.5*e26*e252-0.5*e26*e242; -A[2 + 10*10]=e03*e34*e07+0.5*e032*e36+1.5*e36*e062+e03*e33*e06+e00*e31*e07+e00*e01*e37+e00*e32*e08+e00*e02*e38+e30*e01*e07+e30*e02*e08+e03*e04*e37+e03*e35*e08+e03*e05*e38+0.5*e002*e36-0.5*e36*e022-0.5*e36*e042-0.5*e36*e052+0.5*e36*e072+0.5*e36*e082-0.5*e36*e012+e33*e04*e07+e33*e05*e08+e06*e37*e07+e06*e38*e08-e06*e32*e02-e06*e31*e01-e06*e34*e04-e06*e35*e05+e00*e30*e06; -A[2 + 10*11]=e13*e33*e06+e13*e34*e07+e13*e04*e37+e13*e35*e08+e13*e05*e38+e33*e14*e07+e33*e04*e17+e33*e15*e08+e33*e05*e18+3*e06*e36*e16+e06*e38*e18+e06*e37*e17+e16*e37*e07+e16*e38*e08+e36*e17*e07+e36*e18*e08-e06*e35*e15-e06*e31*e11-e06*e32*e12+e00*e31*e17+e00*e11*e37+e10*e30*e06+e10*e31*e07+e10*e01*e37+e10*e32*e08+e10*e02*e38+e30*e11*e07+e30*e01*e17+e30*e12*e08+e30*e02*e18+e03*e33*e16+e03*e13*e36+e03*e35*e18+e03*e15*e38+e03*e34*e17+e03*e14*e37+e00*e30*e16+e00*e12*e38-e06*e34*e14-e16*e32*e02-e16*e31*e01-e16*e34*e04-e16*e35*e05-e36*e12*e02-e36*e11*e01-e36*e14*e04-e36*e15*e05+e00*e10*e36+e00*e32*e18; -A[2 + 10*12]=0.5*e36*e182+0.5*e36*e172-0.5*e36*e112-0.5*e36*e122-0.5*e36*e142-0.5*e36*e152+0.5*e102*e36+0.5*e132*e36+1.5*e36*e162+e10*e30*e16+e10*e32*e18+e10*e12*e38+e10*e31*e17+e10*e11*e37+e30*e12*e18+e30*e11*e17+e13*e33*e16+e13*e35*e18+e13*e15*e38+e13*e34*e17+e13*e14*e37+e33*e15*e18+e33*e14*e17+e16*e38*e18+e16*e37*e17-e16*e35*e15-e16*e31*e11-e16*e32*e12-e16*e34*e14; -A[2 + 10*13]=e00*e20*e36+e00*e31*e27+e00*e21*e37+e00*e32*e28+e00*e22*e38+e20*e30*e06+e20*e31*e07+e20*e01*e37+e20*e32*e08+e20*e02*e38+e30*e21*e07+e30*e01*e27+e30*e22*e08+e30*e02*e28+e03*e33*e26+e03*e23*e36+e03*e34*e27+e03*e24*e37+e03*e35*e28-e26*e31*e01-e26*e35*e05-e36*e22*e02-e36*e21*e01-e36*e24*e04-e36*e25*e05-e26*e34*e04+e03*e25*e38+e23*e34*e07+e23*e04*e37+e23*e35*e08+e23*e05*e38+e33*e24*e07+e33*e04*e27+e33*e25*e08+e33*e05*e28+3*e06*e36*e26+e06*e37*e27+e06*e38*e28+e26*e37*e07+e26*e38*e08+e36*e27*e07+e36*e28*e08-e06*e32*e22-e06*e31*e21-e06*e35*e25-e06*e34*e24-e26*e32*e02+e00*e30*e26+e23*e33*e06; -A[2 + 10*14]=e10*e30*e26+e10*e20*e36+e10*e31*e27+e10*e21*e37+e10*e32*e28+e10*e22*e38+e20*e30*e16+e20*e32*e18+e20*e12*e38+e20*e31*e17+e20*e11*e37+e30*e22*e18+e30*e12*e28+e30*e21*e17+e30*e11*e27+e13*e33*e26+e13*e23*e36+e13*e34*e27+e13*e24*e37+e13*e35*e28+e13*e25*e38+e23*e33*e16+e23*e35*e18+e23*e15*e38+e23*e34*e17+e23*e14*e37+e33*e25*e18+e33*e15*e28+e33*e24*e17+e33*e14*e27+3*e16*e36*e26+e16*e37*e27+e16*e38*e28+e26*e38*e18+e26*e37*e17+e36*e28*e18+e36*e27*e17-e16*e32*e22-e16*e31*e21-e16*e35*e25-e16*e34*e24-e26*e35*e15-e26*e31*e11-e26*e32*e12-e26*e34*e14-e36*e25*e15-e36*e21*e11-e36*e22*e12-e36*e24*e14; -A[2 + 10*15]=e33*e25*e28+e20*e30*e26+e20*e32*e28+e20*e31*e27+e20*e21*e37+e20*e22*e38+e30*e21*e27+e30*e22*e28+e23*e33*e26+e23*e34*e27+e23*e24*e37+e23*e35*e28+e23*e25*e38+e33*e24*e27+e26*e37*e27+e26*e38*e28-e26*e32*e22-e26*e31*e21-e26*e35*e25-e26*e34*e24+0.5*e202*e36+0.5*e232*e36+1.5*e36*e262+0.5*e36*e272+0.5*e36*e282-0.5*e36*e222-0.5*e36*e212-0.5*e36*e252-0.5*e36*e242; -A[2 + 10*16]=e00*e30*e36+e00*e32*e38+e00*e31*e37+e30*e31*e07+e30*e01*e37+e30*e32*e08+e30*e02*e38+e03*e33*e36-0.5*e06*e342+e03*e35*e38+e33*e34*e07+e33*e04*e37+e33*e35*e08+e33*e05*e38+e36*e37*e07+e36*e38*e08-e36*e32*e02-e36*e31*e01-e36*e34*e04-e36*e35*e05+e03*e34*e37+0.5*e302*e06+0.5*e332*e06+1.5*e06*e362+0.5*e06*e382+0.5*e06*e372-0.5*e06*e352-0.5*e06*e312-0.5*e06*e322; -A[2 + 10*17]=-e36*e35*e15+e10*e30*e36+0.5*e302*e16+0.5*e332*e16+1.5*e16*e362+0.5*e16*e382+0.5*e16*e372-0.5*e16*e352-0.5*e16*e312-0.5*e16*e322-0.5*e16*e342+e10*e32*e38+e10*e31*e37+e30*e32*e18+e30*e12*e38+e30*e31*e17+e30*e11*e37+e13*e33*e36+e13*e35*e38+e13*e34*e37+e33*e35*e18+e33*e15*e38+e33*e34*e17+e33*e14*e37+e36*e38*e18+e36*e37*e17-e36*e31*e11-e36*e32*e12-e36*e34*e14; -A[2 + 10*18]=-e36*e35*e25+e30*e32*e28+0.5*e302*e26+0.5*e332*e26+1.5*e26*e362+0.5*e26*e382+0.5*e26*e372-0.5*e26*e352-0.5*e26*e312-0.5*e26*e322-0.5*e26*e342+e20*e30*e36+e20*e32*e38+e20*e31*e37+e30*e31*e27+e30*e21*e37+e30*e22*e38+e23*e33*e36+e23*e35*e38+e23*e34*e37+e33*e34*e27+e33*e24*e37+e33*e35*e28+e33*e25*e38+e36*e37*e27+e36*e38*e28-e36*e32*e22-e36*e31*e21-e36*e34*e24; -A[2 + 10*19]=0.5*e302*e36+e30*e32*e38+e30*e31*e37+0.5*e332*e36+e33*e35*e38+e33*e34*e37+0.5*e363+0.5*e36*e382+0.5*e36*e372-0.5*e36*e352-0.5*e36*e312-0.5*e36*e322-0.5*e36*e342; -A[3 + 10*0]=0.5*e01*e002+0.5*e013+0.5*e01*e022+e04*e00*e03+0.5*e01*e042+e04*e02*e05+e07*e00*e06+0.5*e01*e072+e07*e02*e08-0.5*e01*e032-0.5*e01*e052-0.5*e01*e062-0.5*e01*e082; -A[3 + 10*1]=1.5*e11*e012+0.5*e11*e002+0.5*e11*e022+0.5*e11*e042+0.5*e11*e072-0.5*e11*e032-0.5*e11*e052-0.5*e11*e062-0.5*e11*e082+e01*e10*e00+e01*e12*e02+e04*e10*e03+e04*e00*e13+e04*e01*e14+e04*e12*e05+e04*e02*e15+e14*e00*e03+e14*e02*e05+e07*e10*e06+e07*e00*e16+e07*e01*e17+e07*e12*e08+e07*e02*e18+e17*e00*e06+e17*e02*e08-e01*e13*e03-e01*e16*e06-e01*e15*e05-e01*e18*e08; -A[3 + 10*2]=e17*e02*e18+e14*e10*e03+e11*e12*e02-e11*e18*e08+0.5*e01*e102+0.5*e01*e122+1.5*e01*e112+0.5*e01*e142+0.5*e01*e172-0.5*e01*e132-0.5*e01*e152-0.5*e01*e162-0.5*e01*e182+e11*e10*e00+e04*e10*e13+e04*e12*e15+e04*e11*e14+e14*e00*e13+e14*e12*e05+e14*e02*e15+e07*e10*e16+e07*e12*e18+e07*e11*e17+e17*e10*e06+e17*e00*e16+e17*e12*e08-e11*e13*e03-e11*e16*e06-e11*e15*e05; -A[3 + 10*3]=0.5*e11*e102+0.5*e11*e122+0.5*e113+e14*e10*e13+e14*e12*e15+0.5*e11*e142+e17*e10*e16+e17*e12*e18+0.5*e11*e172-0.5*e11*e132-0.5*e11*e152-0.5*e11*e162-0.5*e11*e182; -A[3 + 10*4]=-e01*e25*e05-e01*e26*e06-e01*e23*e03+e27*e02*e08+e27*e00*e06+e07*e02*e28+e07*e22*e08+e07*e01*e27+e07*e00*e26+e24*e02*e05+e24*e00*e03+e04*e02*e25+e04*e22*e05+e04*e01*e24+e04*e00*e23+e04*e20*e03+e01*e22*e02+e01*e20*e00-e01*e28*e08+e07*e20*e06+0.5*e21*e072+0.5*e21*e042+0.5*e21*e022+0.5*e21*e002+1.5*e21*e012-0.5*e21*e082-0.5*e21*e052-0.5*e21*e062-0.5*e21*e032; -A[3 + 10*5]=e11*e20*e00+e07*e20*e16+3*e01*e21*e11+e01*e22*e12-e21*e18*e08-e21*e15*e05-e21*e16*e06-e21*e13*e03-e11*e28*e08-e11*e25*e05-e11*e26*e06-e11*e23*e03-e01*e28*e18-e01*e23*e13-e01*e25*e15-e01*e26*e16+e27*e02*e18+e27*e12*e08+e27*e00*e16+e27*e10*e06+e17*e02*e28+e17*e22*e08+e17*e01*e27+e17*e00*e26+e17*e20*e06+e07*e11*e27+e07*e21*e17+e07*e12*e28+e07*e22*e18+e07*e10*e26+e24*e02*e15+e24*e12*e05+e24*e00*e13+e24*e10*e03+e14*e02*e25+e14*e22*e05+e14*e01*e24+e14*e00*e23+e14*e20*e03+e04*e11*e24+e04*e21*e14+e04*e12*e25+e04*e22*e15+e21*e12*e02+e04*e20*e13+e01*e20*e10+e11*e22*e02+e21*e10*e00+e04*e10*e23; -A[3 + 10*6]=1.5*e21*e112+0.5*e21*e102+0.5*e21*e122+e11*e20*e10+e11*e22*e12+e14*e10*e23+e14*e22*e15+e14*e12*e25-0.5*e21*e162-0.5*e21*e152-0.5*e21*e132-0.5*e21*e182+e27*e12*e18-e11*e26*e16-e11*e25*e15-e11*e23*e13-e11*e28*e18+e17*e20*e16+e17*e10*e26+e17*e22*e18+e17*e12*e28+e17*e11*e27+e27*e10*e16+0.5*e21*e172+e14*e11*e24+e24*e10*e13+e24*e12*e15+0.5*e21*e142+e14*e20*e13; -A[3 + 10*7]=-0.5*e01*e262-0.5*e01*e282-0.5*e01*e252-0.5*e01*e232+0.5*e01*e272+e27*e22*e08+e27*e02*e28-e21*e23*e03-e21*e26*e06-e21*e25*e05-e21*e28*e08+e04*e22*e25+e24*e20*e03+e24*e00*e23+e24*e22*e05+e24*e02*e25+e07*e20*e26+e07*e21*e27+e07*e22*e28+e27*e20*e06+e27*e00*e26+e21*e20*e00+e21*e22*e02+e04*e20*e23+e04*e21*e24+0.5*e01*e222+0.5*e01*e242+1.5*e01*e212+0.5*e01*e202; -A[3 + 10*8]=-0.5*e11*e282-0.5*e11*e252-e21*e26*e16+e27*e12*e28-e21*e25*e15-e21*e23*e13-e21*e28*e18+e17*e20*e26+e17*e21*e27+e17*e22*e28+e27*e20*e16+e27*e10*e26+e27*e22*e18+0.5*e11*e242+0.5*e11*e272-0.5*e11*e232-0.5*e11*e262+0.5*e11*e202+1.5*e11*e212+0.5*e11*e222+e21*e20*e10+e14*e20*e23+e14*e21*e24+e14*e22*e25+e24*e20*e13+e24*e10*e23+e24*e22*e15+e24*e12*e25+e21*e22*e12; -A[3 + 10*9]=0.5*e21*e202+0.5*e213+0.5*e21*e222+e24*e20*e23+0.5*e21*e242+e24*e22*e25+e27*e20*e26+0.5*e21*e272+e27*e22*e28-0.5*e21*e232-0.5*e21*e262-0.5*e21*e282-0.5*e21*e252; -A[3 + 10*10]=-0.5*e31*e032-0.5*e31*e052-0.5*e31*e062-0.5*e31*e082+e07*e30*e06+e07*e00*e36+e07*e01*e37+e07*e32*e08+e07*e02*e38+e37*e00*e06+e37*e02*e08-e01*e33*e03-e01*e36*e06-e01*e35*e05-e01*e38*e08+0.5*e31*e072+e04*e30*e03+e04*e00*e33+e04*e01*e34+e04*e32*e05+e04*e02*e35+e34*e00*e03+e34*e02*e05+0.5*e31*e002+0.5*e31*e022+0.5*e31*e042+e01*e30*e00+e01*e32*e02+1.5*e31*e012; -A[3 + 10*11]=e34*e12*e05+e34*e02*e15+e07*e10*e36+e07*e32*e18+e07*e12*e38+e07*e31*e17+e07*e11*e37+e17*e30*e06+e17*e00*e36+e17*e01*e37+e17*e32*e08+e17*e02*e38+e37*e10*e06+e37*e00*e16+e37*e12*e08+e37*e02*e18-e01*e36*e16-e01*e35*e15-e01*e33*e13-e01*e38*e18-e11*e33*e03-e11*e36*e06-e11*e35*e05+e01*e30*e10+e01*e32*e12+3*e01*e31*e11+e11*e30*e00+e11*e32*e02+e31*e10*e00+e31*e12*e02+e04*e30*e13+e04*e10*e33+e04*e32*e15+e04*e12*e35+e04*e31*e14+e04*e11*e34+e14*e30*e03+e14*e00*e33+e14*e01*e34+e14*e32*e05+e14*e02*e35+e34*e10*e03+e34*e00*e13+e07*e30*e16-e11*e38*e08-e31*e13*e03-e31*e16*e06-e31*e15*e05-e31*e18*e08; -A[3 + 10*12]=-e11*e33*e13-e11*e38*e18+0.5*e31*e142+0.5*e31*e172-0.5*e31*e162-0.5*e31*e152-0.5*e31*e132-0.5*e31*e182+0.5*e31*e122+0.5*e31*e102+e11*e30*e10+e11*e32*e12+e14*e30*e13+e14*e10*e33+e14*e32*e15+e14*e12*e35+e14*e11*e34+e34*e10*e13+e34*e12*e15+e17*e30*e16+e17*e10*e36+e17*e32*e18+e17*e12*e38+e17*e11*e37+e37*e10*e16+e37*e12*e18-e11*e36*e16-e11*e35*e15+1.5*e31*e112; -A[3 + 10*13]=-e21*e35*e05+e07*e32*e28+e01*e30*e20-e21*e33*e03-e21*e36*e06-e21*e38*e08-e31*e23*e03-e31*e26*e06-e31*e25*e05-e31*e28*e08+3*e01*e31*e21+e01*e32*e22+e21*e30*e00+e21*e32*e02+e31*e20*e00+e31*e22*e02+e04*e30*e23+e04*e20*e33+e04*e31*e24+e04*e21*e34+e04*e32*e25+e04*e22*e35+e24*e30*e03+e24*e00*e33+e24*e01*e34+e24*e32*e05+e24*e02*e35+e34*e20*e03+e34*e00*e23+e34*e22*e05+e34*e02*e25+e07*e30*e26+e07*e20*e36+e07*e31*e27+e07*e21*e37+e07*e22*e38+e27*e30*e06+e27*e00*e36+e27*e01*e37+e27*e32*e08+e27*e02*e38+e37*e00*e26+e37*e22*e08+e37*e02*e28-e01*e33*e23-e01*e36*e26-e01*e38*e28-e01*e35*e25+e37*e20*e06; -A[3 + 10*14]=e11*e32*e22+e34*e12*e25+e11*e30*e20+3*e11*e31*e21+e21*e30*e10+e21*e32*e12+e34*e10*e23+e34*e22*e15+e17*e30*e26+e17*e20*e36+e17*e31*e27+e17*e21*e37+e17*e32*e28+e17*e22*e38+e27*e30*e16+e27*e10*e36+e27*e32*e18+e27*e12*e38+e27*e11*e37+e37*e20*e16+e37*e10*e26+e37*e22*e18+e37*e12*e28-e11*e33*e23-e11*e36*e26-e11*e38*e28-e11*e35*e25-e21*e36*e16-e21*e35*e15-e21*e33*e13-e21*e38*e18-e31*e26*e16-e31*e25*e15-e31*e23*e13-e31*e28*e18+e31*e20*e10+e31*e22*e12+e14*e30*e23+e14*e20*e33+e14*e31*e24+e14*e21*e34+e14*e32*e25+e14*e22*e35+e24*e30*e13+e24*e10*e33+e24*e32*e15+e24*e12*e35+e24*e11*e34+e34*e20*e13; -A[3 + 10*15]=-e21*e36*e26+e37*e22*e28-e21*e33*e23-e21*e38*e28-e21*e35*e25+0.5*e31*e222+0.5*e31*e242+0.5*e31*e272-0.5*e31*e232-0.5*e31*e262-0.5*e31*e282-0.5*e31*e252+e21*e30*e20+e21*e32*e22+e24*e30*e23+e24*e20*e33+e24*e21*e34+e24*e32*e25+e24*e22*e35+e34*e20*e23+e34*e22*e25+e27*e30*e26+e27*e20*e36+e27*e21*e37+e27*e32*e28+e27*e22*e38+e37*e20*e26+1.5*e31*e212+0.5*e31*e202; -A[3 + 10*16]=e04*e32*e35+0.5*e01*e372-0.5*e01*e352-0.5*e01*e362-0.5*e01*e332-0.5*e01*e382+e04*e31*e34+e34*e30*e03+e34*e00*e33+e34*e32*e05+e34*e02*e35+e07*e30*e36+e07*e32*e38+e07*e31*e37+e37*e30*e06+e37*e00*e36+e37*e32*e08+e04*e30*e33+e37*e02*e38-e31*e33*e03-e31*e36*e06-e31*e35*e05-e31*e38*e08+0.5*e01*e302+0.5*e01*e322+1.5*e01*e312+0.5*e01*e342+e31*e30*e00+e31*e32*e02; -A[3 + 10*17]=e31*e32*e12+e14*e30*e33+e14*e32*e35+e14*e31*e34+e34*e30*e13+e34*e10*e33+e34*e32*e15+e34*e12*e35+e17*e30*e36+e17*e32*e38+e17*e31*e37+e37*e30*e16+e37*e10*e36+e37*e32*e18+e37*e12*e38-e31*e36*e16-e31*e35*e15+0.5*e11*e302+0.5*e11*e322+1.5*e11*e312-e31*e33*e13-e31*e38*e18+0.5*e11*e342+0.5*e11*e372+e31*e30*e10-0.5*e11*e352-0.5*e11*e362-0.5*e11*e332-0.5*e11*e382; -A[3 + 10*18]=e34*e32*e25+0.5*e21*e342+0.5*e21*e372-0.5*e21*e352-0.5*e21*e362-0.5*e21*e332-0.5*e21*e382+0.5*e21*e302+0.5*e21*e322+1.5*e21*e312+e31*e30*e20+e31*e32*e22+e24*e30*e33+e24*e32*e35+e24*e31*e34+e34*e30*e23+e34*e20*e33+e34*e22*e35+e27*e30*e36+e27*e32*e38+e27*e31*e37+e37*e30*e26+e37*e20*e36+e37*e32*e28+e37*e22*e38-e31*e33*e23-e31*e36*e26-e31*e38*e28-e31*e35*e25; -A[3 + 10*19]=0.5*e31*e302+0.5*e31*e322+0.5*e313+e34*e30*e33+e34*e32*e35+0.5*e31*e342+e37*e30*e36+e37*e32*e38+0.5*e31*e372-0.5*e31*e352-0.5*e31*e362-0.5*e31*e332-0.5*e31*e382; -A[4 + 10*0]=e01*e00*e03+0.5*e012*e04+e01*e02*e05+0.5*e04*e032+0.5*e043+0.5*e04*e052+e07*e03*e06+0.5*e04*e072+e07*e05*e08-0.5*e04*e002-0.5*e04*e022-0.5*e04*e062-0.5*e04*e082; -A[4 + 10*1]=e07*e13*e06+e01*e10*e03-0.5*e14*e002-0.5*e14*e022-0.5*e14*e062-0.5*e14*e082+e01*e00*e13+e01*e11*e04+e01*e12*e05+e01*e02*e15+e11*e00*e03+e11*e02*e05+e04*e13*e03+e04*e15*e05+e07*e03*e16+e07*e04*e17+e07*e15*e08+e07*e05*e18+e17*e03*e06+e17*e05*e08-e04*e10*e00-e04*e12*e02-e04*e16*e06-e04*e18*e08+0.5*e012*e14+1.5*e14*e042+0.5*e14*e032+0.5*e14*e052+0.5*e14*e072; -A[4 + 10*2]=e11*e10*e03+0.5*e112*e04+0.5*e04*e132+0.5*e04*e152+1.5*e04*e142+0.5*e04*e172-0.5*e04*e102-0.5*e04*e162-0.5*e04*e122-0.5*e04*e182+e01*e10*e13+e01*e12*e15+e01*e11*e14+e11*e00*e13+e11*e12*e05+e11*e02*e15+e14*e13*e03+e14*e15*e05+e07*e13*e16+e07*e15*e18+e07*e14*e17+e17*e13*e06+e17*e03*e16+e17*e15*e08+e17*e05*e18-e14*e10*e00-e14*e12*e02-e14*e16*e06-e14*e18*e08; -A[4 + 10*3]=e11*e10*e13+e11*e12*e15+0.5*e112*e14+0.5*e14*e132+0.5*e14*e152+0.5*e143+e17*e13*e16+e17*e15*e18+0.5*e14*e172-0.5*e14*e102-0.5*e14*e162-0.5*e14*e122-0.5*e14*e182; -A[4 + 10*4]=-e04*e28*e08-e04*e26*e06-e04*e22*e02-e04*e20*e00+e27*e05*e08+e27*e03*e06+e07*e05*e28+e07*e25*e08+e07*e04*e27+e07*e03*e26+e07*e23*e06+e04*e25*e05+e04*e23*e03+e21*e02*e05+e21*e00*e03+e01*e02*e25+e01*e22*e05+e01*e21*e04+e01*e00*e23+e01*e20*e03+0.5*e012*e24+0.5*e24*e072+0.5*e24*e052+0.5*e24*e032+1.5*e24*e042-0.5*e24*e022-0.5*e24*e002-0.5*e24*e082-0.5*e24*e062; -A[4 + 10*5]=e11*e02*e25+e11*e22*e05+e11*e21*e04-e24*e18*e08-e24*e16*e06-e24*e12*e02-e14*e28*e08-e14*e26*e06-e14*e22*e02-e14*e20*e00-e04*e28*e18-e04*e22*e12-e04*e26*e16-e04*e20*e10+e27*e05*e18+e27*e15*e08+e27*e03*e16+e27*e13*e06+e17*e05*e28+e17*e25*e08+e17*e04*e27+e17*e03*e26+e17*e23*e06+e07*e14*e27+e07*e24*e17+e07*e15*e28+e07*e25*e18+e07*e13*e26+e07*e23*e16+e24*e15*e05+e24*e13*e03+e14*e25*e05+e14*e23*e03+3*e04*e24*e14+e04*e25*e15+e04*e23*e13+e21*e02*e15+e21*e12*e05+e21*e00*e13+e21*e10*e03-e24*e10*e00+e01*e20*e13+e01*e10*e23+e01*e22*e15+e01*e12*e25+e01*e11*e24+e11*e20*e03+e11*e00*e23+e01*e21*e14; -A[4 + 10*6]=e11*e12*e25-0.5*e24*e182-0.5*e24*e102-0.5*e24*e162-0.5*e24*e122+e27*e13*e16+e27*e15*e18-e14*e20*e10-e14*e26*e16-e14*e22*e12-e14*e28*e18+e17*e15*e28+e17*e14*e27+1.5*e24*e142+0.5*e24*e132+0.5*e24*e152+0.5*e112*e24+0.5*e24*e172+e11*e21*e14+e11*e22*e15+e11*e10*e23+e11*e20*e13+e21*e10*e13+e21*e12*e15+e17*e13*e26+e17*e23*e16+e14*e25*e15+e14*e23*e13+e17*e25*e18; -A[4 + 10*7]=e27*e03*e26+e27*e25*e08+e27*e05*e28-e24*e20*e00-e24*e22*e02-e24*e26*e06-e24*e28*e08+0.5*e04*e232+1.5*e04*e242+0.5*e04*e252+0.5*e04*e272-0.5*e04*e202-0.5*e04*e222-0.5*e04*e262-0.5*e04*e282+e24*e23*e03+e24*e25*e05+e07*e23*e26+e07*e24*e27+e07*e25*e28+e27*e23*e06+e21*e20*e03+e21*e00*e23+e21*e22*e05+e21*e02*e25+0.5*e212*e04+e01*e20*e23+e01*e21*e24+e01*e22*e25; -A[4 + 10*8]=-e24*e22*e12-e24*e28*e18+0.5*e14*e272-0.5*e14*e202-0.5*e14*e222-0.5*e14*e262-0.5*e14*e282+e17*e23*e26+e17*e24*e27+e17*e25*e28+e27*e23*e16+e27*e13*e26+e27*e25*e18+e27*e15*e28-e24*e20*e10-e24*e26*e16+0.5*e14*e232+1.5*e14*e242+0.5*e14*e252+e21*e10*e23+e21*e22*e15+e21*e12*e25+e24*e23*e13+e24*e25*e15+e21*e20*e13+0.5*e212*e14+e11*e20*e23+e11*e22*e25+e11*e21*e24; -A[4 + 10*9]=e21*e20*e23+0.5*e212*e24+e21*e22*e25+0.5*e24*e232+0.5*e243+0.5*e24*e252+e27*e23*e26+0.5*e24*e272+e27*e25*e28-0.5*e24*e202-0.5*e24*e222-0.5*e24*e262-0.5*e24*e282; -A[4 + 10*10]=-e04*e38*e08-e04*e32*e02-e04*e36*e06-0.5*e34*e002-0.5*e34*e022-0.5*e34*e062-0.5*e34*e082+e37*e03*e06+e37*e05*e08-e04*e30*e00+0.5*e34*e032+0.5*e34*e052+0.5*e34*e072+1.5*e34*e042+e01*e30*e03+e01*e00*e33+e01*e31*e04+e01*e32*e05+e01*e02*e35+e31*e00*e03+e31*e02*e05+e04*e33*e03+e04*e35*e05+e07*e03*e36+e07*e04*e37+e07*e35*e08+e07*e05*e38+0.5*e012*e34+e07*e33*e06; -A[4 + 10*11]=e07*e13*e36+e01*e12*e35-e04*e30*e10+e17*e04*e37+e17*e35*e08+e17*e05*e38+e37*e13*e06+e37*e03*e16+e37*e15*e08+e37*e05*e18-e04*e36*e16+e17*e33*e06+e04*e33*e13+e04*e35*e15+3*e04*e34*e14+e14*e33*e03+e14*e35*e05+e34*e13*e03+e34*e15*e05+e07*e33*e16+e07*e35*e18+e07*e15*e38+e07*e34*e17+e07*e14*e37+e17*e03*e36+e31*e10*e03+e01*e30*e13+e01*e10*e33+e01*e32*e15+e01*e31*e14+e01*e11*e34+e11*e30*e03+e11*e00*e33+e11*e31*e04+e11*e32*e05+e11*e02*e35+e31*e00*e13+e31*e12*e05+e31*e02*e15-e34*e12*e02-e34*e16*e06-e34*e18*e08-e14*e32*e02-e14*e36*e06-e14*e38*e08-e34*e10*e00-e04*e32*e12-e04*e38*e18-e14*e30*e00; -A[4 + 10*12]=e11*e32*e15-0.5*e34*e102-0.5*e34*e162-0.5*e34*e122-0.5*e34*e182+e37*e13*e16+0.5*e112*e34+1.5*e34*e142+0.5*e34*e132+0.5*e34*e152+0.5*e34*e172+e11*e30*e13+e11*e10*e33+e11*e12*e35+e11*e31*e14+e31*e10*e13+e31*e12*e15+e14*e33*e13+e14*e35*e15+e17*e33*e16+e17*e13*e36+e17*e35*e18+e17*e15*e38+e17*e14*e37+e37*e15*e18-e14*e30*e10-e14*e36*e16-e14*e32*e12-e14*e38*e18; -A[4 + 10*13]=e01*e22*e35-e04*e30*e20-e04*e32*e22+e01*e31*e24+e01*e21*e34+e01*e32*e25+e21*e30*e03+e21*e00*e33+e21*e31*e04+e21*e32*e05+e21*e02*e35+e31*e20*e03+e31*e00*e23+e31*e22*e05+e31*e02*e25+e04*e33*e23+3*e04*e34*e24+e04*e35*e25+e24*e33*e03+e37*e05*e28-e04*e36*e26-e04*e38*e28-e24*e30*e00-e24*e32*e02-e24*e36*e06-e24*e38*e08+e24*e35*e05+e34*e23*e03+e34*e25*e05+e07*e33*e26+e07*e23*e36+e07*e34*e27+e07*e24*e37+e07*e35*e28+e07*e25*e38+e27*e33*e06+e27*e03*e36+e27*e04*e37+e27*e35*e08+e27*e05*e38+e37*e23*e06+e37*e03*e26+e37*e25*e08-e34*e20*e00-e34*e22*e02-e34*e26*e06-e34*e28*e08+e01*e30*e23+e01*e20*e33; -A[4 + 10*14]=e21*e10*e33+e11*e30*e23+e11*e20*e33+e11*e31*e24+e11*e21*e34+e11*e32*e25+e11*e22*e35+e21*e30*e13+e21*e32*e15+e21*e12*e35+e21*e31*e14+e31*e20*e13+e31*e10*e23+e31*e22*e15+e31*e12*e25+e14*e33*e23+3*e14*e34*e24+e14*e35*e25+e24*e33*e13+e24*e35*e15+e34*e23*e13+e34*e25*e15+e17*e33*e26+e17*e23*e36+e17*e34*e27+e17*e24*e37+e17*e35*e28+e17*e25*e38+e27*e33*e16+e27*e13*e36+e27*e35*e18+e27*e15*e38+e27*e14*e37+e37*e23*e16+e37*e13*e26+e37*e25*e18+e37*e15*e28-e34*e28*e18-e34*e22*e12-e14*e32*e22-e14*e36*e26-e14*e38*e28-e24*e30*e10-e24*e36*e16-e24*e32*e12-e24*e38*e18-e34*e20*e10-e34*e26*e16-e14*e30*e20; -A[4 + 10*15]=-0.5*e34*e202-0.5*e34*e222-0.5*e34*e262-0.5*e34*e282+e37*e25*e28-e24*e32*e22-e24*e36*e26-e24*e38*e28-e24*e30*e20+0.5*e212*e34+1.5*e34*e242+0.5*e34*e232+0.5*e34*e252+0.5*e34*e272+e21*e30*e23+e21*e20*e33+e21*e31*e24+e21*e32*e25+e21*e22*e35+e31*e20*e23+e31*e22*e25+e24*e33*e23+e24*e35*e25+e27*e33*e26+e27*e23*e36+e27*e24*e37+e27*e35*e28+e27*e25*e38+e37*e23*e26; -A[4 + 10*16]=e37*e33*e06+e01*e30*e33+e01*e31*e34+e31*e30*e03+e31*e02*e35+e34*e33*e03+e34*e35*e05+e07*e33*e36+e07*e35*e38+e07*e34*e37+e37*e03*e36+e37*e35*e08+e37*e05*e38-e34*e32*e02-e34*e36*e06-e34*e38*e08+e31*e32*e05+e31*e00*e33+0.5*e312*e04+0.5*e04*e332+0.5*e04*e352+1.5*e04*e342+0.5*e04*e372+e01*e32*e35-0.5*e04*e302-0.5*e04*e322-0.5*e04*e362-0.5*e04*e382-e34*e30*e00; -A[4 + 10*17]=0.5*e14*e372-0.5*e14*e302-0.5*e14*e322-0.5*e14*e362-0.5*e14*e382+0.5*e312*e14+0.5*e14*e332+0.5*e14*e352+1.5*e14*e342+e11*e30*e33+e11*e32*e35+e11*e31*e34+e31*e30*e13+e31*e10*e33+e31*e32*e15+e31*e12*e35+e34*e33*e13+e34*e35*e15+e17*e33*e36+e17*e35*e38+e17*e34*e37+e37*e33*e16+e37*e13*e36+e37*e35*e18+e37*e15*e38-e34*e30*e10-e34*e36*e16-e34*e32*e12-e34*e38*e18; -A[4 + 10*18]=-e34*e32*e22-e34*e36*e26-e34*e38*e28+0.5*e24*e332+0.5*e24*e352+1.5*e24*e342+0.5*e24*e372-0.5*e24*e302-0.5*e24*e322-0.5*e24*e362-0.5*e24*e382+e21*e30*e33+0.5*e312*e24+e21*e32*e35+e21*e31*e34+e31*e30*e23+e31*e20*e33+e31*e32*e25+e31*e22*e35+e34*e33*e23+e34*e35*e25+e27*e33*e36+e27*e35*e38+e27*e34*e37+e37*e33*e26+e37*e23*e36+e37*e35*e28+e37*e25*e38-e34*e30*e20; -A[4 + 10*19]=e31*e30*e33+e31*e32*e35+0.5*e312*e34+0.5*e34*e332+0.5*e34*e352+0.5*e343+e37*e33*e36+e37*e35*e38+0.5*e34*e372-0.5*e34*e302-0.5*e34*e322-0.5*e34*e362-0.5*e34*e382; -A[5 + 10*0]=e01*e00*e06+0.5*e012*e07+e01*e02*e08+e04*e03*e06+0.5*e042*e07+e04*e05*e08+0.5*e07*e062+0.5*e073+0.5*e07*e082-0.5*e07*e002-0.5*e07*e022-0.5*e07*e032-0.5*e07*e052; -A[5 + 10*1]=e04*e13*e06+0.5*e042*e17+1.5*e17*e072+0.5*e17*e062+0.5*e17*e082-0.5*e17*e002-0.5*e17*e022-0.5*e17*e032-0.5*e17*e052+e01*e10*e06+e07*e16*e06+e07*e18*e08-e07*e10*e00-e07*e12*e02-e07*e13*e03-e07*e15*e05+e01*e00*e16+e01*e11*e07+e01*e12*e08+e01*e02*e18+e11*e00*e06+e11*e02*e08+e04*e03*e16+e04*e14*e07+e04*e15*e08+e04*e05*e18+e14*e03*e06+e14*e05*e08+0.5*e012*e17; -A[5 + 10*2]=-e17*e10*e00+0.5*e112*e07+0.5*e142*e07+0.5*e07*e162+0.5*e07*e182+1.5*e07*e172-0.5*e07*e102-0.5*e07*e152-0.5*e07*e132-0.5*e07*e122+e01*e10*e16+e01*e12*e18+e01*e11*e17+e11*e10*e06+e11*e00*e16+e11*e12*e08+e11*e02*e18+e04*e13*e16+e04*e15*e18+e04*e14*e17+e14*e13*e06+e14*e03*e16+e14*e15*e08+e14*e05*e18+e17*e16*e06+e17*e18*e08-e17*e12*e02-e17*e13*e03-e17*e15*e05; -A[5 + 10*3]=e11*e10*e16+e11*e12*e18+0.5*e112*e17+e14*e13*e16+e14*e15*e18+0.5*e142*e17+0.5*e17*e162+0.5*e17*e182+0.5*e173-0.5*e17*e102-0.5*e17*e152-0.5*e17*e132-0.5*e17*e122; -A[5 + 10*4]=e01*e22*e08+e07*e28*e08-e07*e20*e00-e07*e23*e03-e07*e22*e02-e07*e25*e05+0.5*e012*e27+0.5*e042*e27+1.5*e27*e072+0.5*e27*e062+0.5*e27*e082-0.5*e27*e002-0.5*e27*e022-0.5*e27*e032-0.5*e27*e052+e07*e26*e06+e01*e20*e06+e01*e00*e26+e01*e21*e07+e01*e02*e28+e21*e00*e06+e21*e02*e08+e04*e23*e06+e04*e03*e26+e04*e24*e07+e04*e25*e08+e04*e05*e28+e24*e03*e06+e24*e05*e08; -A[5 + 10*5]=e14*e24*e07+e14*e03*e26+e14*e23*e06+e04*e14*e27+e04*e24*e17+e04*e15*e28+e04*e25*e18+e04*e13*e26+e04*e23*e16+e21*e02*e18+e21*e12*e08-e27*e15*e05-e27*e13*e03-e27*e12*e02-e27*e10*e00-e17*e25*e05-e17*e23*e03-e17*e22*e02-e17*e20*e00-e07*e22*e12-e07*e23*e13-e07*e25*e15-e07*e20*e10+e27*e18*e08+e27*e16*e06+e17*e28*e08+e17*e26*e06+3*e07*e27*e17+e07*e28*e18+e07*e26*e16+e24*e05*e18+e24*e15*e08+e24*e03*e16+e24*e13*e06+e14*e05*e28+e14*e25*e08+e01*e12*e28+e01*e20*e16+e01*e10*e26+e01*e22*e18+e01*e21*e17+e11*e20*e06+e01*e11*e27+e21*e00*e16+e21*e10*e06+e11*e21*e07+e11*e22*e08+e11*e02*e28+e11*e00*e26; -A[5 + 10*6]=-0.5*e27*e102-0.5*e27*e152-0.5*e27*e132-0.5*e27*e122+0.5*e142*e27+1.5*e27*e172+0.5*e27*e162+0.5*e27*e182+0.5*e112*e27+e11*e22*e18+e11*e10*e26+e11*e20*e16-e17*e22*e12-e17*e23*e13-e17*e25*e15-e17*e20*e10+e17*e28*e18+e17*e26*e16+e24*e15*e18+e24*e13*e16+e14*e24*e17+e14*e15*e28+e14*e25*e18+e14*e13*e26+e14*e23*e16+e21*e12*e18+e21*e10*e16+e11*e21*e17+e11*e12*e28; -A[5 + 10*7]=-0.5*e07*e252+e27*e26*e06+e27*e28*e08-e27*e20*e00-e27*e22*e02-e27*e23*e03-e27*e25*e05+1.5*e07*e272+0.5*e07*e282+e01*e22*e28+e21*e20*e06+e21*e00*e26+e21*e22*e08+e21*e02*e28+e04*e23*e26+e04*e24*e27+e04*e25*e28+e24*e23*e06+e24*e03*e26+e24*e25*e08+e24*e05*e28+0.5*e212*e07+0.5*e242*e07+0.5*e07*e262+e01*e20*e26+e01*e21*e27-0.5*e07*e202-0.5*e07*e232-0.5*e07*e222; -A[5 + 10*8]=-e27*e25*e15-e27*e23*e13-e27*e22*e12-0.5*e17*e252-0.5*e17*e202-0.5*e17*e222-0.5*e17*e232+0.5*e17*e262+1.5*e17*e272+0.5*e17*e282+e24*e23*e16+e24*e13*e26+e24*e25*e18+e24*e15*e28+e27*e26*e16+e27*e28*e18-e27*e20*e10+e14*e24*e27+e14*e25*e28+0.5*e212*e17+0.5*e242*e17+e11*e20*e26+e11*e21*e27+e11*e22*e28+e21*e20*e16+e21*e10*e26+e21*e22*e18+e21*e12*e28+e14*e23*e26; -A[5 + 10*9]=e21*e20*e26+0.5*e212*e27+e21*e22*e28+e24*e23*e26+0.5*e242*e27+e24*e25*e28+0.5*e27*e262+0.5*e273+0.5*e27*e282-0.5*e27*e202-0.5*e27*e222-0.5*e27*e232-0.5*e27*e252; -A[5 + 10*10]=e04*e05*e38+e01*e30*e06-0.5*e37*e002-0.5*e37*e022-0.5*e37*e032-0.5*e37*e052-e07*e32*e02-e07*e35*e05-e07*e33*e03+e07*e36*e06+e07*e38*e08-e07*e30*e00+1.5*e37*e072+0.5*e37*e062+0.5*e37*e082+e01*e02*e38+e31*e00*e06+e31*e02*e08+e04*e33*e06+e04*e03*e36+e04*e34*e07+e04*e35*e08+e34*e03*e06+e34*e05*e08+0.5*e012*e37+0.5*e042*e37+e01*e00*e36+e01*e31*e07+e01*e32*e08; -A[5 + 10*11]=e14*e33*e06+e11*e30*e06+e11*e00*e36+e11*e31*e07+e31*e10*e06+e11*e32*e08+e11*e02*e38+e31*e00*e16+e31*e12*e08+e31*e02*e18+e04*e33*e16+e04*e13*e36+e04*e35*e18+e04*e15*e38+e01*e10*e36+e01*e32*e18+e01*e12*e38+e01*e31*e17+e01*e11*e37+e01*e30*e16-e17*e35*e05-e37*e10*e00-e37*e12*e02-e37*e13*e03-e37*e15*e05+e37*e18*e08-e07*e30*e10-e07*e35*e15-e07*e33*e13-e07*e32*e12-e17*e30*e00-e17*e32*e02-e17*e33*e03+e07*e38*e18+3*e07*e37*e17+e17*e36*e06+e17*e38*e08+e37*e16*e06+e04*e34*e17+e04*e14*e37+e14*e03*e36+e14*e34*e07+e14*e35*e08+e14*e05*e38+e34*e13*e06+e34*e03*e16+e34*e15*e08+e34*e05*e18+e07*e36*e16; -A[5 + 10*12]=e11*e32*e18-0.5*e37*e102-0.5*e37*e152-0.5*e37*e132-0.5*e37*e122+0.5*e112*e37+0.5*e142*e37+1.5*e37*e172+0.5*e37*e162+0.5*e37*e182+e11*e10*e36+e11*e12*e38+e11*e31*e17+e31*e10*e16+e31*e12*e18+e14*e33*e16+e14*e13*e36+e14*e35*e18+e14*e15*e38+e14*e34*e17+e34*e13*e16+e34*e15*e18+e17*e36*e16+e17*e38*e18-e17*e30*e10-e17*e35*e15-e17*e33*e13-e17*e32*e12+e11*e30*e16; -A[5 + 10*13]=e01*e20*e36+e01*e31*e27+e01*e21*e37+e01*e32*e28+e01*e22*e38+e21*e30*e06+e21*e00*e36+e21*e31*e07+e21*e32*e08+e21*e02*e38+e01*e30*e26+e31*e20*e06+e31*e00*e26+e31*e22*e08+e31*e02*e28+e04*e33*e26+e04*e23*e36+e04*e34*e27+e04*e24*e37+e04*e35*e28+e04*e25*e38+e24*e33*e06+e24*e03*e36+e24*e34*e07+e24*e35*e08+e24*e05*e38+e34*e23*e06+e34*e03*e26+e34*e25*e08+e34*e05*e28+e07*e36*e26+3*e07*e37*e27+e07*e38*e28+e27*e36*e06+e27*e38*e08+e37*e26*e06+e37*e28*e08-e07*e30*e20-e07*e32*e22-e07*e33*e23-e07*e35*e25-e27*e30*e00-e27*e32*e02-e27*e33*e03-e27*e35*e05-e37*e20*e00-e37*e22*e02-e37*e23*e03-e37*e25*e05; -A[5 + 10*14]=e11*e30*e26+e11*e20*e36+e11*e31*e27+e11*e21*e37+e11*e32*e28+e11*e22*e38+e21*e10*e36+e21*e32*e18+e21*e12*e38+e21*e31*e17+e31*e20*e16+e31*e10*e26+e31*e22*e18+e31*e12*e28+e14*e33*e26+e14*e23*e36+e14*e34*e27+e14*e24*e37+e14*e35*e28+e14*e25*e38+e24*e33*e16+e24*e13*e36+e24*e35*e18+e24*e15*e38+e24*e34*e17+e34*e23*e16+e34*e13*e26+e34*e25*e18+e34*e15*e28+e17*e36*e26+3*e17*e37*e27+e17*e38*e28+e27*e36*e16+e27*e38*e18+e37*e26*e16+e37*e28*e18-e17*e30*e20-e17*e32*e22-e17*e33*e23-e17*e35*e25-e27*e30*e10-e27*e35*e15-e27*e33*e13-e27*e32*e12-e37*e20*e10-e37*e25*e15-e37*e23*e13-e37*e22*e12+e21*e30*e16; -A[5 + 10*15]=e21*e20*e36+e21*e31*e27+e21*e32*e28+e21*e22*e38+e31*e22*e28+e24*e33*e26+e24*e23*e36+e24*e34*e27+e24*e35*e28+e24*e25*e38+e34*e23*e26+e34*e25*e28+e27*e36*e26+e27*e38*e28-e27*e30*e20-e27*e32*e22-e27*e33*e23-e27*e35*e25+0.5*e242*e37+1.5*e37*e272+0.5*e37*e262+0.5*e37*e282+e31*e20*e26+e21*e30*e26+0.5*e212*e37-0.5*e37*e202-0.5*e37*e222-0.5*e37*e232-0.5*e37*e252; -A[5 + 10*16]=e01*e30*e36+e01*e32*e38+e01*e31*e37+e31*e30*e06+e31*e00*e36+e31*e32*e08+e31*e02*e38+e04*e33*e36+e04*e35*e38+e04*e34*e37+e34*e33*e06+e34*e03*e36+e34*e35*e08+e34*e05*e38+e37*e36*e06+e37*e38*e08-e37*e30*e00-e37*e32*e02-e37*e33*e03-e37*e35*e05+0.5*e312*e07+0.5*e342*e07+0.5*e07*e362+0.5*e07*e382+1.5*e07*e372-0.5*e07*e302-0.5*e07*e352-0.5*e07*e322-0.5*e07*e332; -A[5 + 10*17]=0.5*e312*e17+0.5*e342*e17+0.5*e17*e362+0.5*e17*e382+1.5*e17*e372-0.5*e17*e302-0.5*e17*e352-0.5*e17*e322-0.5*e17*e332-e37*e32*e12-e37*e33*e13+e11*e30*e36+e11*e32*e38+e11*e31*e37+e31*e30*e16+e31*e10*e36+e31*e32*e18+e31*e12*e38+e14*e33*e36+e14*e35*e38+e14*e34*e37+e34*e33*e16+e34*e13*e36+e34*e35*e18+e34*e15*e38+e37*e36*e16+e37*e38*e18-e37*e30*e10-e37*e35*e15; -A[5 + 10*18]=e21*e31*e37-0.5*e27*e332+e21*e30*e36+e21*e32*e38+e31*e30*e26+e31*e20*e36+e31*e32*e28+e31*e22*e38+e24*e33*e36+e24*e35*e38+e24*e34*e37+e34*e33*e26+e34*e23*e36+e34*e35*e28+e34*e25*e38+e37*e36*e26+e37*e38*e28-e37*e30*e20-e37*e32*e22-e37*e33*e23-e37*e35*e25+0.5*e312*e27+0.5*e342*e27+0.5*e27*e362+0.5*e27*e382+1.5*e27*e372-0.5*e27*e302-0.5*e27*e352-0.5*e27*e322; -A[5 + 10*19]=e31*e30*e36+e31*e32*e38+0.5*e312*e37+e34*e33*e36+e34*e35*e38+0.5*e342*e37+0.5*e37*e362+0.5*e37*e382+0.5*e373-0.5*e37*e302-0.5*e37*e352-0.5*e37*e322-0.5*e37*e332; -A[6 + 10*0]=0.5*e02*e002+0.5*e02*e012+0.5*e023+e05*e00*e03+e05*e01*e04+0.5*e02*e052+e08*e00*e06+e08*e01*e07+0.5*e02*e082-0.5*e02*e032-0.5*e02*e042-0.5*e02*e062-0.5*e02*e072; -A[6 + 10*1]=-0.5*e12*e042-0.5*e12*e062-0.5*e12*e072+0.5*e12*e082-0.5*e12*e032+1.5*e12*e022+0.5*e12*e002+0.5*e12*e012+0.5*e12*e052+e02*e10*e00+e02*e11*e01+e05*e10*e03+e05*e00*e13+e05*e11*e04+e05*e01*e14+e05*e02*e15+e15*e00*e03+e15*e01*e04+e08*e10*e06+e08*e00*e16+e08*e11*e07+e08*e01*e17+e08*e02*e18+e18*e00*e06+e18*e01*e07-e02*e13*e03-e02*e14*e04-e02*e16*e06-e02*e17*e07; -A[6 + 10*2]=0.5*e02*e102+1.5*e02*e122+0.5*e02*e112+0.5*e02*e152+0.5*e02*e182-0.5*e02*e162-0.5*e02*e172-0.5*e02*e132-0.5*e02*e142+e12*e10*e00+e12*e11*e01+e05*e10*e13+e05*e12*e15+e05*e11*e14+e15*e10*e03+e15*e00*e13+e15*e11*e04+e15*e01*e14+e08*e10*e16+e08*e12*e18+e08*e11*e17+e18*e10*e06+e18*e00*e16+e18*e11*e07+e18*e01*e17-e12*e13*e03-e12*e14*e04-e12*e16*e06-e12*e17*e07; -A[6 + 10*3]=0.5*e12*e102+0.5*e123+0.5*e12*e112+e15*e10*e13+0.5*e12*e152+e15*e11*e14+e18*e10*e16+0.5*e12*e182+e18*e11*e17-0.5*e12*e162-0.5*e12*e172-0.5*e12*e132-0.5*e12*e142; -A[6 + 10*4]=-0.5*e22*e032-0.5*e22*e042-0.5*e22*e062-0.5*e22*e072+0.5*e22*e082+1.5*e22*e022+0.5*e22*e002+0.5*e22*e012+0.5*e22*e052+e02*e20*e00+e02*e21*e01+e05*e20*e03+e05*e00*e23+e05*e21*e04+e05*e01*e24+e05*e02*e25+e25*e00*e03+e25*e01*e04+e08*e20*e06+e08*e00*e26+e08*e21*e07+e08*e01*e27+e08*e02*e28+e28*e00*e06+e28*e01*e07-e02*e27*e07-e02*e23*e03-e02*e24*e04-e02*e26*e06; -A[6 + 10*5]=-e22*e17*e07-e22*e16*e06-e22*e14*e04-e22*e13*e03-e12*e26*e06-e12*e24*e04-e12*e23*e03-e12*e27*e07-e02*e24*e14-e02*e23*e13-e02*e27*e17-e02*e26*e16+e28*e01*e17+e28*e11*e07+e28*e00*e16+e28*e10*e06+e18*e02*e28+e18*e01*e27+e18*e21*e07+e18*e00*e26+e18*e20*e06+e08*e11*e27+e08*e21*e17+e08*e12*e28+e08*e22*e18+e08*e10*e26+e25*e01*e14+e25*e11*e04+e25*e00*e13+e25*e10*e03+e15*e01*e24+e02*e21*e11+e12*e21*e01+e15*e02*e25+e15*e21*e04+e05*e22*e15+e05*e11*e24+e15*e20*e03+e15*e00*e23+e05*e10*e23+e05*e12*e25+e05*e21*e14+e22*e10*e00+e22*e11*e01+e02*e20*e10+3*e02*e22*e12+e12*e20*e00+e08*e20*e16+e05*e20*e13; -A[6 + 10*6]=-e12*e24*e14-e12*e23*e13-e12*e27*e17-e12*e26*e16+e28*e11*e17+e28*e10*e16+e18*e11*e27+e18*e21*e17+e18*e12*e28+e18*e10*e26+e18*e20*e16+e25*e11*e14+e25*e10*e13+e15*e11*e24+e15*e21*e14+e15*e12*e25+e15*e10*e23+e15*e20*e13+e12*e21*e11+0.5*e22*e182+0.5*e22*e152+1.5*e22*e122+0.5*e22*e102+e12*e20*e10+0.5*e22*e112-0.5*e22*e172-0.5*e22*e132-0.5*e22*e142-0.5*e22*e162; -A[6 + 10*7]=0.5*e02*e282+e28*e01*e27-e22*e27*e07-e22*e23*e03-e22*e24*e04-e22*e26*e06+0.5*e02*e252+e05*e20*e23+e05*e22*e25+e25*e20*e03+e25*e00*e23+e25*e21*e04+e25*e01*e24+e08*e20*e26+e08*e21*e27+e08*e22*e28+e28*e20*e06+e28*e00*e26+e28*e21*e07+e05*e21*e24+0.5*e02*e202+0.5*e02*e212+1.5*e02*e222+e22*e20*e00+e22*e21*e01-0.5*e02*e272-0.5*e02*e242-0.5*e02*e232-0.5*e02*e262; -A[6 + 10*8]=-e22*e27*e17-e22*e23*e13-e22*e24*e14-0.5*e12*e232-0.5*e12*e262-0.5*e12*e242-0.5*e12*e272+0.5*e12*e282+e18*e21*e27+e28*e20*e16+e28*e10*e26+e28*e21*e17+e28*e11*e27-e22*e26*e16+e18*e22*e28+0.5*e12*e252+0.5*e12*e202+0.5*e12*e212+1.5*e12*e222+e22*e20*e10+e15*e20*e23+e15*e21*e24+e15*e22*e25+e25*e20*e13+e25*e10*e23+e25*e21*e14+e25*e11*e24+e18*e20*e26+e22*e21*e11; -A[6 + 10*9]=0.5*e22*e202+0.5*e22*e212+0.5*e223+e25*e20*e23+e25*e21*e24+0.5*e22*e252+e28*e20*e26+e28*e21*e27+0.5*e22*e282-0.5*e22*e232-0.5*e22*e262-0.5*e22*e242-0.5*e22*e272; -A[6 + 10*10]=e08*e31*e07-0.5*e32*e032-e02*e33*e03-e02*e34*e04-e02*e36*e06-0.5*e32*e042-0.5*e32*e062-0.5*e32*e072+e38*e01*e07+e38*e00*e06-e02*e37*e07+e05*e31*e04+e05*e01*e34+e05*e02*e35+e35*e01*e04+e35*e00*e03+e08*e30*e06+e08*e00*e36+e08*e01*e37+e08*e02*e38+0.5*e32*e052+e02*e30*e00+e02*e31*e01+e05*e30*e03+e05*e00*e33+1.5*e32*e022+0.5*e32*e012+0.5*e32*e002+0.5*e32*e082; -A[6 + 10*11]=e05*e32*e15+e32*e11*e01+e38*e10*e06+e08*e12*e38-e32*e14*e04-e32*e16*e06-e32*e17*e07-e12*e36*e06-e32*e13*e03-e02*e34*e14-e12*e37*e07-e12*e33*e03-e12*e34*e04-e02*e37*e17-e02*e33*e13+e38*e01*e17-e02*e36*e16+e18*e01*e37+e18*e02*e38+e38*e00*e16+e38*e11*e07+e08*e30*e16+e08*e10*e36+e08*e32*e18+e08*e31*e17+e08*e11*e37+e18*e30*e06+e18*e00*e36+e18*e31*e07+e35*e10*e03+e35*e00*e13+e35*e11*e04+e35*e01*e14+e15*e02*e35+e05*e10*e33+e05*e12*e35+e05*e31*e14+e05*e11*e34+e15*e30*e03+e15*e00*e33+e15*e31*e04+e15*e01*e34+e05*e30*e13+e02*e30*e10+e02*e31*e11+3*e02*e32*e12+e12*e30*e00+e12*e31*e01+e32*e10*e00; -A[6 + 10*12]=0.5*e32*e102+0.5*e32*e112+e12*e30*e10+1.5*e32*e122+e12*e31*e11+e15*e30*e13+e15*e10*e33+e15*e12*e35+e15*e31*e14+e15*e11*e34+e35*e10*e13-0.5*e32*e162-0.5*e32*e172-0.5*e32*e132-0.5*e32*e142-e12*e37*e17-e12*e33*e13-e12*e34*e14+0.5*e32*e182+0.5*e32*e152+e35*e11*e14+e18*e30*e16+e18*e10*e36+e18*e12*e38+e18*e31*e17+e18*e11*e37+e38*e10*e16+e38*e11*e17-e12*e36*e16; -A[6 + 10*13]=3*e02*e32*e22+e05*e31*e24+e08*e22*e38+e02*e31*e21+e22*e30*e00+e22*e31*e01+e32*e20*e00+e32*e21*e01+e05*e30*e23+e05*e20*e33+e05*e21*e34-e22*e37*e07-e22*e33*e03-e22*e34*e04-e22*e36*e06-e32*e27*e07-e32*e23*e03-e32*e24*e04-e32*e26*e06+e05*e32*e25+e25*e30*e03+e25*e00*e33+e25*e31*e04+e25*e01*e34+e25*e02*e35+e35*e20*e03+e35*e00*e23+e35*e21*e04+e35*e01*e24+e08*e30*e26+e08*e20*e36+e08*e31*e27+e08*e21*e37+e08*e32*e28+e28*e30*e06+e28*e00*e36+e28*e31*e07+e28*e01*e37+e28*e02*e38+e38*e20*e06+e38*e00*e26+e38*e21*e07+e38*e01*e27-e02*e33*e23-e02*e36*e26-e02*e34*e24-e02*e37*e27+e05*e22*e35+e02*e30*e20; -A[6 + 10*14]=e18*e22*e38+e12*e31*e21+3*e12*e32*e22+e22*e30*e10+e22*e31*e11+e32*e20*e10+e32*e21*e11+e15*e30*e23+e15*e20*e33+e15*e31*e24+e15*e21*e34+e15*e32*e25+e15*e22*e35+e25*e30*e13+e25*e10*e33+e25*e12*e35+e25*e31*e14+e25*e11*e34+e35*e20*e13+e35*e10*e23+e35*e21*e14+e35*e11*e24+e18*e30*e26+e18*e20*e36+e18*e31*e27+e18*e21*e37+e18*e32*e28+e28*e30*e16+e28*e10*e36+e28*e12*e38+e28*e31*e17+e28*e11*e37+e38*e20*e16+e38*e10*e26+e12*e30*e20-e22*e37*e17-e22*e33*e13-e22*e34*e14-e32*e26*e16-e32*e27*e17-e32*e23*e13-e32*e24*e14-e22*e36*e16+e38*e21*e17+e38*e11*e27-e12*e33*e23-e12*e36*e26-e12*e34*e24-e12*e37*e27; -A[6 + 10*15]=e25*e30*e23+e22*e30*e20+e22*e31*e21+e25*e20*e33+e25*e31*e24+e25*e21*e34+e25*e22*e35+e35*e20*e23+e35*e21*e24+e28*e30*e26+e28*e20*e36+e28*e31*e27+e28*e21*e37+e28*e22*e38+e38*e20*e26+e38*e21*e27-e22*e33*e23-e22*e36*e26-e22*e34*e24-e22*e37*e27+0.5*e32*e212+0.5*e32*e252+1.5*e32*e222+0.5*e32*e202+0.5*e32*e282-0.5*e32*e232-0.5*e32*e262-0.5*e32*e242-0.5*e32*e272; -A[6 + 10*16]=0.5*e02*e302+1.5*e02*e322+0.5*e02*e312+0.5*e02*e352+0.5*e02*e382-0.5*e02*e342-0.5*e02*e362-0.5*e02*e332-0.5*e02*e372+e38*e30*e06+e32*e30*e00+e32*e31*e01+e05*e30*e33+e05*e32*e35+e05*e31*e34+e35*e30*e03+e35*e00*e33+e35*e31*e04+e35*e01*e34+e08*e30*e36+e08*e32*e38+e08*e31*e37+e38*e00*e36+e38*e31*e07+e38*e01*e37-e32*e37*e07-e32*e33*e03-e32*e34*e04-e32*e36*e06; -A[6 + 10*17]=e32*e30*e10+e32*e31*e11+e15*e30*e33+e15*e32*e35+e15*e31*e34+e35*e30*e13+e35*e10*e33+e35*e31*e14+e35*e11*e34+e18*e30*e36+e18*e32*e38+e18*e31*e37+e38*e30*e16+e38*e10*e36+e38*e31*e17+e38*e11*e37-e32*e36*e16-e32*e37*e17-e32*e33*e13-e32*e34*e14+0.5*e12*e382-0.5*e12*e342-0.5*e12*e362-0.5*e12*e332-0.5*e12*e372+0.5*e12*e352+1.5*e12*e322+0.5*e12*e312+0.5*e12*e302; -A[6 + 10*18]=0.5*e22*e302+0.5*e22*e312+0.5*e22*e352+0.5*e22*e382-0.5*e22*e342-0.5*e22*e362-0.5*e22*e332-0.5*e22*e372+1.5*e22*e322+e32*e30*e20+e32*e31*e21+e25*e30*e33+e25*e32*e35+e25*e31*e34+e35*e30*e23+e35*e20*e33+e35*e31*e24+e35*e21*e34+e28*e30*e36+e28*e32*e38+e28*e31*e37+e38*e30*e26+e38*e20*e36+e38*e31*e27+e38*e21*e37-e32*e33*e23-e32*e36*e26-e32*e34*e24-e32*e37*e27; -A[6 + 10*19]=0.5*e32*e302+0.5*e323+0.5*e32*e312+e35*e30*e33+0.5*e32*e352+e35*e31*e34+e38*e30*e36+0.5*e32*e382+e38*e31*e37-0.5*e32*e342-0.5*e32*e362-0.5*e32*e332-0.5*e32*e372; -A[7 + 10*0]=e02*e01*e04+e02*e00*e03+0.5*e022*e05+0.5*e05*e032+0.5*e05*e042+0.5*e053+e08*e03*e06+e08*e04*e07+0.5*e05*e082-0.5*e05*e002-0.5*e05*e062-0.5*e05*e012-0.5*e05*e072; -A[7 + 10*1]=e08*e13*e06+e02*e10*e03+e02*e00*e13+e02*e11*e04+e02*e01*e14+e02*e12*e05+e12*e01*e04+e12*e00*e03+e05*e13*e03+e05*e14*e04+e08*e03*e16+e08*e14*e07+e08*e04*e17+e08*e05*e18+e18*e03*e06+e18*e04*e07-e05*e10*e00-e05*e11*e01-e05*e16*e06-e05*e17*e07+0.5*e022*e15+1.5*e15*e052+0.5*e15*e032+0.5*e15*e042+0.5*e15*e082-0.5*e15*e002-0.5*e15*e062-0.5*e15*e012-0.5*e15*e072; -A[7 + 10*2]=0.5*e122*e05+0.5*e05*e132+1.5*e05*e152+0.5*e05*e142+0.5*e05*e182-0.5*e05*e102-0.5*e05*e162-0.5*e05*e112-0.5*e05*e172+e02*e10*e13+e02*e12*e15+e02*e11*e14+e12*e10*e03+e12*e00*e13+e12*e11*e04+e12*e01*e14+e15*e13*e03+e15*e14*e04+e08*e13*e16+e08*e15*e18+e08*e14*e17+e18*e13*e06+e18*e03*e16+e18*e14*e07+e18*e04*e17-e15*e11*e01-e15*e16*e06-e15*e17*e07-e15*e10*e00; -A[7 + 10*3]=e12*e10*e13+0.5*e122*e15+e12*e11*e14+0.5*e15*e132+0.5*e153+0.5*e15*e142+e18*e13*e16+0.5*e15*e182+e18*e14*e17-0.5*e15*e102-0.5*e15*e162-0.5*e15*e112-0.5*e15*e172; -A[7 + 10*4]=0.5*e25*e082-0.5*e25*e002-0.5*e25*e062-0.5*e25*e012-0.5*e25*e072+e02*e20*e03+e02*e00*e23+e02*e21*e04+e02*e01*e24+e02*e22*e05+e22*e01*e04+e22*e00*e03+e05*e23*e03+e05*e24*e04+e08*e23*e06+e08*e03*e26+e08*e24*e07+e08*e04*e27+e08*e05*e28+e28*e03*e06+e28*e04*e07-e05*e20*e00-e05*e27*e07-e05*e21*e01-e05*e26*e06+0.5*e022*e25+1.5*e25*e052+0.5*e25*e032+0.5*e25*e042; -A[7 + 10*5]=-e25*e17*e07-e25*e16*e06-e25*e11*e01-e25*e10*e00-e15*e26*e06-e15*e21*e01-e15*e27*e07-e15*e20*e00-e05*e27*e17-e05*e21*e11-e05*e26*e16-e05*e20*e10+e28*e04*e17+e28*e14*e07+e28*e03*e16+e28*e13*e06+e18*e05*e28+e18*e04*e27+e18*e24*e07+e18*e03*e26+e18*e23*e06+e08*e14*e27+e08*e24*e17+e08*e15*e28+e08*e25*e18+e08*e13*e26+e08*e23*e16+e25*e14*e04+e25*e13*e03+e15*e24*e04+e15*e23*e03+e05*e24*e14+3*e05*e25*e15+e05*e23*e13+e22*e01*e14+e22*e11*e04+e22*e00*e13+e22*e10*e03+e12*e22*e05+e12*e01*e24+e12*e21*e04+e12*e00*e23+e12*e20*e03+e02*e11*e24+e02*e21*e14+e02*e12*e25+e02*e22*e15+e02*e10*e23+e02*e20*e13; -A[7 + 10*6]=-e15*e27*e17-e15*e21*e11-e15*e26*e16+e28*e14*e17+e28*e13*e16+e18*e14*e27+e18*e24*e17+e18*e15*e28+e18*e13*e26+e15*e24*e14+e15*e23*e13+e22*e11*e14+e22*e10*e13+e12*e11*e24+e12*e21*e14+e12*e22*e15+e12*e10*e23+e18*e23*e16+0.5*e25*e142+0.5*e25*e182+1.5*e25*e152+0.5*e25*e132+0.5*e122*e25+e12*e20*e13-0.5*e25*e172-0.5*e25*e162-0.5*e25*e112-0.5*e25*e102-e15*e20*e10; -A[7 + 10*7]=e28*e24*e07-0.5*e05*e272-0.5*e05*e262-0.5*e05*e212+0.5*e05*e282-0.5*e05*e202+e28*e23*e06+e08*e23*e26+e08*e25*e28+e08*e24*e27+e28*e03*e26+e28*e04*e27-e25*e27*e07-e25*e21*e01-e25*e26*e06+e02*e20*e23+e02*e22*e25+e02*e21*e24+e22*e20*e03+e22*e00*e23+e22*e21*e04+e22*e01*e24+e25*e23*e03+e25*e24*e04+0.5*e222*e05+0.5*e05*e232+1.5*e05*e252+0.5*e05*e242-e25*e20*e00; -A[7 + 10*8]=-0.5*e15*e202-0.5*e15*e262-0.5*e15*e212-0.5*e15*e272+e18*e23*e26+e18*e25*e28+e18*e24*e27+e28*e23*e16+e28*e13*e26+e28*e24*e17+e28*e14*e27-e25*e20*e10-e25*e26*e16-e25*e21*e11-e25*e27*e17+0.5*e15*e282+0.5*e15*e232+1.5*e15*e252+0.5*e15*e242+0.5*e222*e15+e12*e21*e24+e22*e20*e13+e22*e10*e23+e22*e21*e14+e22*e11*e24+e25*e23*e13+e25*e24*e14+e12*e20*e23+e12*e22*e25; -A[7 + 10*9]=e22*e20*e23+0.5*e222*e25+e22*e21*e24+0.5*e25*e232+0.5*e253+0.5*e25*e242+e28*e23*e26+0.5*e25*e282+e28*e24*e27-0.5*e25*e202-0.5*e25*e262-0.5*e25*e212-0.5*e25*e272; -A[7 + 10*10]=-0.5*e35*e062-0.5*e35*e012-0.5*e35*e072-e05*e30*e00-e05*e31*e01-e05*e36*e06-e05*e37*e07-0.5*e35*e002+0.5*e35*e082+e05*e34*e04+e08*e33*e06+e08*e03*e36+e08*e34*e07+e08*e04*e37+e08*e05*e38+e38*e04*e07+e38*e03*e06+0.5*e022*e35+1.5*e35*e052+0.5*e35*e042+0.5*e35*e032+e02*e30*e03+e02*e00*e33+e02*e31*e04+e02*e01*e34+e02*e32*e05+e32*e01*e04+e32*e00*e03+e05*e33*e03; -A[7 + 10*11]=e08*e33*e16-e35*e16*e06-e35*e17*e07-e15*e30*e00-e15*e37*e07-e15*e31*e01-e15*e36*e06-e35*e10*e00-e35*e11*e01-e05*e37*e17-e05*e31*e11+e38*e04*e17-e05*e30*e10-e05*e36*e16+e18*e33*e06+e18*e03*e36+e18*e34*e07+e18*e04*e37+e18*e05*e38+e38*e13*e06+e38*e03*e16+e38*e14*e07+e35*e14*e04+e08*e13*e36+e08*e35*e18+e08*e15*e38+e08*e34*e17+e08*e14*e37+e35*e13*e03+e05*e33*e13+3*e05*e35*e15+e05*e34*e14+e15*e33*e03+e15*e34*e04+e12*e01*e34+e12*e32*e05+e32*e10*e03+e32*e00*e13+e32*e11*e04+e32*e01*e14+e12*e30*e03+e02*e30*e13+e02*e32*e15+e02*e10*e33+e02*e12*e35+e12*e00*e33+e02*e31*e14+e02*e11*e34+e12*e31*e04; -A[7 + 10*12]=-0.5*e35*e162-0.5*e35*e172-e15*e36*e16-e15*e31*e11-e15*e37*e17-0.5*e35*e102-0.5*e35*e112-e15*e30*e10+e18*e13*e36+e18*e15*e38+e18*e34*e17+e18*e14*e37+e38*e13*e16+e38*e14*e17+e18*e33*e16+1.5*e35*e152+0.5*e35*e132+0.5*e35*e142+0.5*e35*e182+0.5*e122*e35+e32*e10*e13+e32*e11*e14+e15*e33*e13+e15*e34*e14+e12*e10*e33+e12*e32*e15+e12*e31*e14+e12*e11*e34+e12*e30*e13; -A[7 + 10*13]=e05*e33*e23+3*e05*e35*e25+e05*e34*e24+e25*e33*e03+e25*e34*e04+e35*e23*e03+e35*e24*e04+e08*e33*e26+e08*e23*e36+e08*e35*e28+e02*e20*e33+e02*e32*e25+e02*e22*e35+e02*e31*e24+e02*e21*e34+e22*e30*e03+e22*e00*e33+e22*e31*e04+e22*e01*e34+e22*e32*e05+e32*e20*e03+e32*e00*e23+e32*e21*e04+e32*e01*e24+e02*e30*e23-e35*e27*e07-e35*e21*e01-e35*e26*e06+e08*e25*e38+e08*e34*e27+e08*e24*e37+e28*e33*e06+e28*e03*e36+e28*e34*e07+e28*e04*e37+e28*e05*e38+e38*e23*e06+e38*e03*e26+e38*e24*e07+e38*e04*e27-e05*e30*e20-e05*e36*e26-e05*e31*e21-e05*e37*e27-e25*e30*e00-e25*e37*e07-e25*e31*e01-e25*e36*e06-e35*e20*e00; -A[7 + 10*14]=e12*e21*e34+e18*e25*e38+e12*e30*e23+e12*e20*e33+e12*e32*e25+e12*e22*e35+e12*e31*e24+e22*e30*e13+e22*e10*e33+e22*e32*e15+e22*e31*e14+e22*e11*e34+e32*e20*e13+e32*e10*e23+e32*e21*e14-e25*e30*e10-e25*e36*e16-e25*e31*e11-e25*e37*e17-e35*e20*e10-e35*e26*e16-e35*e21*e11-e35*e27*e17+e15*e33*e23+3*e15*e35*e25+e15*e34*e24+e25*e33*e13+e25*e34*e14+e35*e23*e13+e35*e24*e14+e18*e33*e26+e18*e23*e36+e18*e35*e28+e18*e34*e27+e18*e24*e37+e28*e33*e16+e28*e13*e36+e28*e15*e38+e28*e34*e17+e28*e14*e37+e38*e23*e16+e38*e13*e26+e38*e24*e17+e38*e14*e27-e15*e30*e20-e15*e36*e26-e15*e31*e21-e15*e37*e27+e32*e11*e24; -A[7 + 10*15]=-0.5*e35*e202-0.5*e35*e262-0.5*e35*e212-0.5*e35*e272+e25*e34*e24+e28*e23*e36+e28*e25*e38+e28*e34*e27+e28*e24*e37+e38*e23*e26+e38*e24*e27-e25*e30*e20-e25*e36*e26-e25*e31*e21-e25*e37*e27+e25*e33*e23+0.5*e222*e35+1.5*e35*e252+0.5*e35*e232+0.5*e35*e242+0.5*e35*e282+e22*e30*e23+e22*e20*e33+e22*e32*e25+e22*e31*e24+e22*e21*e34+e32*e20*e23+e32*e21*e24+e28*e33*e26; -A[7 + 10*16]=-e35*e30*e00-e35*e31*e01-e35*e36*e06-e35*e37*e07+0.5*e322*e05+0.5*e05*e332+0.5*e05*e342+1.5*e05*e352+0.5*e05*e382-0.5*e05*e302-0.5*e05*e362-0.5*e05*e312-0.5*e05*e372+e02*e30*e33+e02*e31*e34+e02*e32*e35+e32*e30*e03+e32*e00*e33+e32*e31*e04+e32*e01*e34+e35*e33*e03+e35*e34*e04+e08*e33*e36+e08*e34*e37+e08*e35*e38+e38*e33*e06+e38*e03*e36+e38*e34*e07+e38*e04*e37; -A[7 + 10*17]=-e35*e30*e10+e12*e32*e35-0.5*e15*e362-0.5*e15*e312-0.5*e15*e372-e35*e36*e16+0.5*e322*e15+0.5*e15*e332+0.5*e15*e342+1.5*e15*e352+0.5*e15*e382-0.5*e15*e302+e12*e30*e33+e12*e31*e34+e32*e30*e13+e32*e10*e33+e32*e31*e14+e32*e11*e34+e35*e33*e13+e35*e34*e14+e18*e33*e36+e18*e34*e37+e18*e35*e38+e38*e33*e16+e38*e13*e36+e38*e34*e17+e38*e14*e37-e35*e31*e11-e35*e37*e17; -A[7 + 10*18]=-0.5*e25*e302-0.5*e25*e362-0.5*e25*e312-0.5*e25*e372+0.5*e322*e25+0.5*e25*e332+0.5*e25*e342+1.5*e25*e352+0.5*e25*e382+e22*e30*e33+e22*e31*e34+e22*e32*e35+e32*e30*e23+e32*e20*e33+e32*e31*e24+e32*e21*e34+e35*e33*e23+e35*e34*e24+e28*e33*e36+e28*e34*e37+e28*e35*e38+e38*e33*e26+e38*e23*e36+e38*e34*e27+e38*e24*e37-e35*e30*e20-e35*e36*e26-e35*e31*e21-e35*e37*e27; -A[7 + 10*19]=e32*e30*e33+e32*e31*e34+0.5*e322*e35+0.5*e35*e332+0.5*e35*e342+0.5*e353+e38*e33*e36+e38*e34*e37+0.5*e35*e382-0.5*e35*e302-0.5*e35*e362-0.5*e35*e312-0.5*e35*e372; -A[8 + 10*0]=e02*e00*e06+e02*e01*e07+0.5*e022*e08+e05*e04*e07+e05*e03*e06+0.5*e052*e08+0.5*e08*e062+0.5*e08*e072+0.5*e083-0.5*e08*e042-0.5*e08*e002-0.5*e08*e012-0.5*e08*e032; -A[8 + 10*1]=e02*e10*e06+e02*e00*e16+e02*e11*e07+e02*e01*e17+e02*e12*e08+e12*e00*e06+e12*e01*e07+e05*e13*e06+e05*e03*e16+e05*e14*e07+e05*e04*e17+e05*e15*e08+e15*e04*e07+e15*e03*e06+e08*e16*e06+e08*e17*e07-e08*e10*e00-e08*e11*e01-e08*e13*e03-e08*e14*e04+0.5*e022*e18+0.5*e052*e18+1.5*e18*e082+0.5*e18*e062+0.5*e18*e072-0.5*e18*e042-0.5*e18*e002-0.5*e18*e012-0.5*e18*e032; -A[8 + 10*2]=e12*e01*e17+0.5*e152*e08+0.5*e08*e162+1.5*e08*e182+0.5*e08*e172-0.5*e08*e102-0.5*e08*e112-0.5*e08*e132-0.5*e08*e142+e05*e13*e16+e05*e14*e17+e05*e15*e18+e15*e13*e06+e15*e03*e16+e15*e14*e07+e15*e04*e17+e18*e16*e06+e18*e17*e07-e18*e10*e00-e18*e11*e01-e18*e13*e03-e18*e14*e04+0.5*e122*e08+e02*e10*e16+e02*e12*e18+e02*e11*e17+e12*e10*e06+e12*e00*e16+e12*e11*e07; -A[8 + 10*3]=e12*e10*e16+0.5*e122*e18+e12*e11*e17+e15*e13*e16+e15*e14*e17+0.5*e152*e18+0.5*e18*e162+0.5*e183+0.5*e18*e172-0.5*e18*e102-0.5*e18*e112-0.5*e18*e132-0.5*e18*e142; -A[8 + 10*4]=-e08*e20*e00+e08*e27*e07-e08*e21*e01-e08*e23*e03-e08*e24*e04+e02*e20*e06+e02*e00*e26+e02*e21*e07+e02*e01*e27+e02*e22*e08+e22*e00*e06+e22*e01*e07+e05*e23*e06+e05*e03*e26+e05*e24*e07+e05*e04*e27+e05*e25*e08+e25*e04*e07+e25*e03*e06+e08*e26*e06+0.5*e022*e28+0.5*e052*e28+1.5*e28*e082+0.5*e28*e062+0.5*e28*e072-0.5*e28*e042-0.5*e28*e002-0.5*e28*e012-0.5*e28*e032; -A[8 + 10*5]=e22*e10*e06+e22*e11*e07+e22*e01*e17+e05*e23*e16+e05*e13*e26+e05*e25*e18+e05*e15*e28+e05*e24*e17+e05*e14*e27+e15*e23*e06+e15*e03*e26+e15*e24*e07+e15*e04*e27+e15*e25*e08+e25*e13*e06+e25*e03*e16+e25*e14*e07+e25*e04*e17+e08*e26*e16+3*e08*e28*e18+e08*e27*e17+e18*e26*e06+e18*e27*e07+e22*e00*e16+e28*e16*e06+e28*e17*e07-e08*e20*e10-e08*e21*e11-e08*e23*e13-e08*e24*e14-e18*e20*e00-e18*e21*e01-e18*e23*e03-e18*e24*e04-e28*e10*e00-e28*e11*e01-e28*e13*e03-e28*e14*e04+e02*e20*e16+e02*e10*e26+e02*e22*e18+e02*e12*e28+e02*e21*e17+e02*e11*e27+e12*e20*e06+e12*e00*e26+e12*e21*e07+e12*e01*e27+e12*e22*e08; -A[8 + 10*6]=-e18*e24*e14-e18*e21*e11-e18*e23*e13-e18*e20*e10+e18*e27*e17+e18*e26*e16+e25*e14*e17+e25*e13*e16+e15*e25*e18+e15*e14*e27+e15*e24*e17+e15*e13*e26+e15*e23*e16+e22*e11*e17+e22*e10*e16+e12*e11*e27+e12*e21*e17+e12*e22*e18+e12*e10*e26+e12*e20*e16+0.5*e28*e162+0.5*e28*e172+1.5*e28*e182+0.5*e152*e28-0.5*e28*e142-0.5*e28*e112-0.5*e28*e132-0.5*e28*e102+0.5*e122*e28; -A[8 + 10*7]=-e28*e24*e04-e28*e21*e01-e28*e23*e03-e28*e20*e00+e28*e27*e07+e28*e26*e06+e25*e04*e27+e25*e24*e07+e25*e03*e26+e05*e24*e27+e05*e25*e28+e05*e23*e26+e22*e01*e27+e22*e21*e07+e22*e00*e26+e22*e20*e06+e02*e22*e28+e02*e20*e26+e02*e21*e27+0.5*e222*e08-0.5*e08*e242-0.5*e08*e212-0.5*e08*e232-0.5*e08*e202+0.5*e08*e262+0.5*e08*e272+1.5*e08*e282+0.5*e252*e08+e25*e23*e06; -A[8 + 10*8]=e25*e24*e17+e25*e14*e27+e28*e26*e16+e28*e27*e17-e28*e21*e11-e28*e24*e14+e12*e22*e28+e22*e10*e26+e22*e21*e17+e22*e11*e27+e15*e23*e26+e15*e25*e28+e15*e24*e27+e25*e23*e16+e25*e13*e26+e22*e20*e16+0.5*e222*e18+0.5*e252*e18+0.5*e18*e262+0.5*e18*e272+e12*e20*e26+e12*e21*e27-e28*e20*e10-0.5*e18*e232-0.5*e18*e242-e28*e23*e13-0.5*e18*e212+1.5*e18*e282-0.5*e18*e202; -A[8 + 10*9]=e22*e20*e26+e22*e21*e27+0.5*e222*e28+e25*e23*e26+0.5*e252*e28+e25*e24*e27+0.5*e28*e262+0.5*e28*e272+0.5*e283-0.5*e28*e202-0.5*e28*e212-0.5*e28*e232-0.5*e28*e242; -A[8 + 10*10]=-e08*e30*e00-0.5*e38*e042-0.5*e38*e002-0.5*e38*e012-0.5*e38*e032+1.5*e38*e082+0.5*e38*e062+0.5*e38*e072+e32*e01*e07+e05*e33*e06+e05*e03*e36+e05*e34*e07+e05*e04*e37+e05*e35*e08+e35*e04*e07+e35*e03*e06+e08*e36*e06+e08*e37*e07+0.5*e052*e38+e32*e00*e06+e02*e30*e06+e02*e00*e36+e02*e31*e07+e02*e01*e37+e02*e32*e08+0.5*e022*e38-e08*e33*e03-e08*e31*e01-e08*e34*e04; -A[8 + 10*11]=-e38*e11*e01-e38*e14*e04-e38*e10*e00-e38*e13*e03-e18*e30*e00-e18*e33*e03-e18*e31*e01-e18*e34*e04-e08*e30*e10-e08*e33*e13-e08*e31*e11-e08*e34*e14+3*e08*e38*e18+e08*e37*e17+e18*e36*e06+e18*e37*e07+e38*e16*e06+e38*e17*e07+e15*e35*e08+e35*e13*e06+e35*e03*e16+e35*e14*e07+e35*e04*e17+e08*e36*e16+e05*e35*e18+e05*e15*e38+e15*e33*e06+e15*e03*e36+e15*e34*e07+e15*e04*e37+e05*e14*e37+e12*e30*e06+e12*e31*e07+e12*e01*e37+e12*e00*e36+e12*e32*e08+e32*e10*e06+e32*e00*e16+e32*e11*e07+e32*e01*e17+e05*e33*e16+e05*e13*e36+e05*e34*e17+e02*e30*e16+e02*e10*e36+e02*e32*e18+e02*e12*e38+e02*e31*e17+e02*e11*e37; -A[8 + 10*12]=e12*e30*e16+e12*e10*e36+e12*e32*e18+e12*e31*e17+e12*e11*e37+e32*e10*e16+e32*e11*e17+e15*e33*e16+e15*e13*e36-0.5*e38*e102-0.5*e38*e112-0.5*e38*e132-0.5*e38*e142+0.5*e38*e162+0.5*e38*e172+e15*e34*e17+e15*e14*e37+e15*e35*e18+e35*e13*e16+e35*e14*e17+e18*e36*e16+e18*e37*e17-e18*e30*e10-e18*e33*e13-e18*e31*e11-e18*e34*e14+0.5*e122*e38+0.5*e152*e38+1.5*e38*e182; -A[8 + 10*13]=e22*e30*e06-e28*e34*e04+e05*e35*e28+e02*e22*e38+e22*e00*e36+e22*e31*e07+e22*e01*e37+e02*e32*e28+e02*e21*e37-e38*e20*e00-e28*e31*e01-e38*e23*e03-e38*e21*e01-e38*e24*e04-e28*e30*e00-e08*e30*e20-e08*e31*e21-e08*e33*e23-e08*e34*e24-e28*e33*e03+e35*e24*e07+e35*e04*e27+e08*e36*e26+e08*e37*e27+3*e08*e38*e28+e28*e36*e06+e28*e37*e07+e38*e26*e06+e38*e27*e07+e25*e04*e37+e25*e35*e08+e35*e23*e06+e35*e03*e26+e05*e23*e36+e05*e25*e38+e05*e34*e27+e05*e24*e37+e25*e33*e06+e25*e03*e36+e25*e34*e07+e05*e33*e26+e32*e21*e07+e32*e01*e27+e22*e32*e08+e32*e20*e06+e32*e00*e26+e02*e30*e26+e02*e20*e36+e02*e31*e27; -A[8 + 10*14]=e35*e13*e26-e38*e21*e11-e38*e24*e14+e35*e24*e17+e35*e14*e27+e18*e36*e26+e18*e37*e27+3*e18*e38*e28+e28*e36*e16+e28*e37*e17+e38*e26*e16+e38*e27*e17-e18*e30*e20-e18*e31*e21-e18*e33*e23-e18*e34*e24-e28*e30*e10-e28*e33*e13-e28*e31*e11-e28*e34*e14-e38*e20*e10-e38*e23*e13+e35*e23*e16+e12*e20*e36+e12*e30*e26+e12*e31*e27+e12*e21*e37+e12*e32*e28+e12*e22*e38+e22*e30*e16+e22*e10*e36+e22*e32*e18+e22*e31*e17+e22*e11*e37+e32*e20*e16+e32*e10*e26+e32*e21*e17+e32*e11*e27+e15*e33*e26+e15*e23*e36+e15*e35*e28+e15*e25*e38+e15*e34*e27+e15*e24*e37+e25*e33*e16+e25*e13*e36+e25*e34*e17+e25*e14*e37+e25*e35*e18; -A[8 + 10*15]=-e28*e30*e20+e22*e30*e26+e22*e20*e36+e22*e31*e27+e22*e21*e37+e22*e32*e28+e32*e20*e26+e32*e21*e27+e25*e33*e26+e25*e23*e36+e25*e35*e28+e25*e34*e27+e25*e24*e37+e35*e23*e26+e35*e24*e27+e28*e36*e26+e28*e37*e27-e28*e31*e21-e28*e33*e23-e28*e34*e24-0.5*e38*e242+0.5*e252*e38+1.5*e38*e282+0.5*e38*e262+0.5*e38*e272-0.5*e38*e202-0.5*e38*e212-0.5*e38*e232+0.5*e222*e38; -A[8 + 10*16]=-0.5*e08*e312-0.5*e08*e342+0.5*e352*e08+0.5*e08*e362+1.5*e08*e382+0.5*e08*e372-0.5*e08*e302-0.5*e08*e332+e02*e30*e36+e02*e32*e38+e02*e31*e37+e32*e30*e06+e32*e00*e36+e32*e31*e07+e32*e01*e37+e05*e33*e36+e05*e34*e37+e05*e35*e38+e35*e33*e06+e35*e03*e36+e35*e34*e07+e35*e04*e37+0.5*e322*e08+e38*e36*e06+e38*e37*e07-e38*e30*e00-e38*e33*e03-e38*e31*e01-e38*e34*e04; -A[8 + 10*17]=-e38*e30*e10+e38*e36*e16+e38*e37*e17-e38*e33*e13-e38*e31*e11-e38*e34*e14+0.5*e18*e362+e12*e30*e36+e12*e32*e38+e12*e31*e37+e32*e30*e16+e32*e10*e36+e32*e31*e17+e32*e11*e37+e15*e33*e36+e15*e34*e37+e15*e35*e38+e35*e33*e16+e35*e13*e36+e35*e34*e17+e35*e14*e37+0.5*e322*e18+0.5*e352*e18+1.5*e18*e382+0.5*e18*e372-0.5*e18*e302-0.5*e18*e332-0.5*e18*e312-0.5*e18*e342; -A[8 + 10*18]=-e38*e30*e20+e25*e35*e38+e22*e30*e36+e22*e32*e38+e22*e31*e37+e32*e30*e26+e32*e20*e36+e32*e31*e27+e32*e21*e37+e25*e33*e36+e25*e34*e37+e35*e33*e26+e35*e23*e36+e35*e34*e27+e35*e24*e37+e38*e36*e26+e38*e37*e27-e38*e31*e21-e38*e33*e23-e38*e34*e24-0.5*e28*e332-0.5*e28*e312-0.5*e28*e342+0.5*e322*e28+0.5*e352*e28+0.5*e28*e362+1.5*e28*e382+0.5*e28*e372-0.5*e28*e302; -A[8 + 10*19]=e32*e30*e36+0.5*e322*e38+e32*e31*e37+e35*e33*e36+e35*e34*e37+0.5*e352*e38+0.5*e38*e362+0.5*e383+0.5*e38*e372-0.5*e38*e302-0.5*e38*e332-0.5*e38*e312-0.5*e38*e342; -A[9 + 10*0]=e00*e04*e08-e00*e05*e07+e03*e02*e07-e03*e01*e08-e06*e02*e04+e06*e01*e05; -A[9 + 10*1]=e06*e01*e15-e16*e02*e04+e16*e01*e05+e03*e02*e17-e13*e01*e08+e06*e11*e05+e13*e02*e07+e00*e04*e18+e00*e14*e08-e00*e05*e17-e10*e05*e07-e00*e15*e07-e06*e12*e04-e06*e02*e14-e03*e01*e18-e03*e11*e08+e10*e04*e08+e03*e12*e07; -A[9 + 10*2]=-e13*e01*e18-e13*e11*e08+e13*e12*e07+e13*e02*e17+e03*e12*e17-e10*e15*e07+e10*e04*e18+e10*e14*e08-e10*e05*e17-e00*e15*e17+e00*e14*e18+e16*e01*e15+e06*e11*e15-e06*e12*e14-e16*e12*e04-e16*e02*e14+e16*e11*e05-e03*e11*e18; -A[9 + 10*3]=e10*e14*e18-e10*e15*e17-e13*e11*e18+e13*e12*e17+e16*e11*e15-e16*e12*e14; -A[9 + 10*4]=-e20*e05*e07+e03*e22*e07+e06*e21*e05+e06*e01*e25-e23*e01*e08+e23*e02*e07+e00*e24*e08-e00*e25*e07-e00*e05*e27+e00*e04*e28-e06*e22*e04-e06*e02*e24-e03*e21*e08-e03*e01*e28-e26*e02*e04+e26*e01*e05+e03*e02*e27+e20*e04*e08; -A[9 + 10*5]=e23*e12*e07-e26*e02*e14+e16*e21*e05-e23*e11*e08+e10*e24*e08-e20*e05*e17+e26*e11*e05+e26*e01*e15+e10*e04*e28+e00*e24*e18-e00*e15*e27+e03*e22*e17-e13*e01*e28+e23*e02*e17+e16*e01*e25+e20*e04*e18+e06*e11*e25+e13*e02*e27-e23*e01*e18-e20*e15*e07-e10*e25*e07+e13*e22*e07-e06*e22*e14-e26*e12*e04-e03*e11*e28-e03*e21*e18-e16*e22*e04-e16*e02*e24-e06*e12*e24+e06*e21*e15+e00*e14*e28-e00*e25*e17+e20*e14*e08-e13*e21*e08-e10*e05*e27+e03*e12*e27; -A[9 + 10*6]=-e13*e11*e28+e13*e12*e27+e13*e22*e17+e16*e11*e25+e10*e14*e28-e13*e21*e18-e23*e11*e18+e23*e12*e17+e20*e14*e18-e20*e15*e17+e26*e11*e15-e10*e15*e27-e10*e25*e17-e16*e22*e14-e16*e12*e24+e16*e21*e15-e26*e12*e14+e10*e24*e18; -A[9 + 10*7]=e26*e21*e05+e26*e01*e25+e20*e04*e28+e20*e24*e08-e20*e25*e07+e23*e22*e07+e03*e22*e27-e03*e21*e28-e26*e22*e04-e20*e05*e27-e00*e25*e27+e06*e21*e25-e06*e22*e24+e00*e24*e28-e26*e02*e24-e23*e21*e08-e23*e01*e28+e23*e02*e27; -A[9 + 10*8]=-e10*e25*e27+e10*e24*e28-e20*e15*e27-e20*e25*e17+e20*e14*e28+e20*e24*e18+e26*e11*e25+e23*e22*e17-e23*e11*e28+e23*e12*e27-e23*e21*e18-e13*e21*e28+e13*e22*e27-e26*e12*e24+e26*e21*e15-e16*e22*e24+e16*e21*e25-e26*e22*e14; -A[9 + 10*9]=-e20*e25*e27+e20*e24*e28-e23*e21*e28+e23*e22*e27-e26*e22*e24+e26*e21*e25; -A[9 + 10*10]=e03*e02*e37-e03*e31*e08-e03*e01*e38+e03*e32*e07-e00*e35*e07+e30*e04*e08+e06*e31*e05-e36*e02*e04+e36*e01*e05-e06*e32*e04-e06*e02*e34+e06*e01*e35+e00*e04*e38-e00*e05*e37+e33*e02*e07-e33*e01*e08-e30*e05*e07+e00*e34*e08; -A[9 + 10*11]=-e36*e12*e04+e30*e04*e18-e30*e15*e07-e36*e02*e14-e30*e05*e17+e30*e14*e08-e00*e35*e17-e00*e15*e37+e33*e02*e17-e06*e32*e14-e06*e12*e34-e16*e32*e04+e06*e31*e15+e06*e11*e35+e00*e34*e18-e10*e35*e07-e33*e11*e08-e33*e01*e18+e16*e01*e35-e16*e02*e34+e16*e31*e05-e03*e31*e18-e03*e11*e38+e03*e32*e17+e13*e02*e37-e13*e31*e08-e13*e01*e38+e10*e34*e08+e00*e14*e38+e36*e11*e05+e36*e01*e15+e03*e12*e37-e10*e05*e37+e10*e04*e38+e33*e12*e07+e13*e32*e07; -A[9 + 10*12]=-e36*e12*e14-e30*e15*e17+e13*e32*e17-e13*e31*e18-e33*e11*e18+e33*e12*e17+e10*e14*e38+e30*e14*e18-e13*e11*e38+e13*e12*e37-e10*e35*e17+e10*e34*e18-e16*e12*e34-e16*e32*e14+e16*e11*e35+e16*e31*e15+e36*e11*e15-e10*e15*e37; -A[9 + 10*13]=-e06*e22*e34-e06*e32*e24-e00*e25*e37-e00*e35*e27+e23*e02*e37+e00*e24*e38-e23*e01*e38-e03*e31*e28-e33*e01*e28+e03*e22*e37+e03*e32*e27+e33*e02*e27-e03*e21*e38-e26*e32*e04-e33*e21*e08+e36*e01*e25+e36*e21*e05-e20*e05*e37+e20*e04*e38+e30*e04*e28-e20*e35*e07+e33*e22*e07+e30*e24*e08-e30*e25*e07-e23*e31*e08+e23*e32*e07+e00*e34*e28+e06*e21*e35+e06*e31*e25-e36*e02*e24+e26*e01*e35-e36*e22*e04+e26*e31*e05-e26*e02*e34+e20*e34*e08-e30*e05*e27; -A[9 + 10*14]=e33*e22*e17+e33*e12*e27+e16*e21*e35-e16*e22*e34-e16*e32*e24+e23*e32*e17-e23*e11*e38-e23*e31*e18+e23*e12*e37-e13*e21*e38-e13*e31*e28+e13*e22*e37+e36*e21*e15-e36*e12*e24+e36*e11*e25-e26*e12*e34-e20*e35*e17+e20*e14*e38+e20*e34*e18+e30*e24*e18-e30*e15*e27-e30*e25*e17+e30*e14*e28-e33*e21*e18+e10*e34*e28+e10*e24*e38-e10*e35*e27-e10*e25*e37-e20*e15*e37-e26*e32*e14+e26*e11*e35+e26*e31*e15-e36*e22*e14+e13*e32*e27+e16*e31*e25-e33*e11*e28; -A[9 + 10*15]=-e20*e35*e27-e20*e25*e37+e20*e34*e28+e20*e24*e38+e30*e24*e28-e30*e25*e27+e23*e32*e27+e23*e22*e37-e23*e31*e28-e23*e21*e38+e33*e22*e27-e26*e22*e34-e26*e32*e24+e26*e21*e35+e26*e31*e25-e36*e22*e24+e36*e21*e25-e33*e21*e28; -A[9 + 10*16]=-e33*e01*e38-e03*e31*e38+e00*e34*e38+e33*e32*e07+e03*e32*e37+e06*e31*e35-e00*e35*e37-e36*e32*e04-e06*e32*e34-e36*e02*e34+e36*e01*e35+e36*e31*e05+e30*e04*e38+e30*e34*e08-e33*e31*e08+e33*e02*e37-e30*e05*e37-e30*e35*e07; -A[9 + 10*17]=-e33*e31*e18-e33*e11*e38+e10*e34*e38+e30*e14*e38-e10*e35*e37-e30*e15*e37-e13*e31*e38+e13*e32*e37-e30*e35*e17+e33*e12*e37+e30*e34*e18+e33*e32*e17+e16*e31*e35-e16*e32*e34-e36*e12*e34-e36*e32*e14+e36*e11*e35+e36*e31*e15; -A[9 + 10*18]=-e20*e35*e37+e20*e34*e38+e30*e24*e38-e30*e35*e27-e30*e25*e37+e30*e34*e28+e23*e32*e37-e23*e31*e38-e33*e21*e38-e33*e31*e28+e33*e22*e37+e33*e32*e27+e26*e31*e35-e26*e32*e34-e36*e22*e34-e36*e32*e24+e36*e21*e35+e36*e31*e25; -A[9 + 10*19]=-e33*e31*e38-e30*e35*e37+e36*e31*e35+e33*e32*e37+e30*e34*e38-e36*e32*e34; - -} diff --git a/visionary/coniccamera.m b/visionary/coniccamera.m deleted file mode 100644 index fd8da1d..0000000 --- a/visionary/coniccamera.m +++ /dev/null @@ -1,68 +0,0 @@ -function pconic = coniccamera(p); -% CONICCAMERA(P) from 3x4 camera matrix to linear "conic camera" 6x10 -p11=p(1,1);p12=p(1,2);p13=p(1,3);p14=p(1,4); -p21=p(2,1);p22=p(2,2);p23=p(2,3);p24=p(2,4); -p31=p(3,1);p32=p(3,2);p33=p(3,3);p34=p(3,4); - -pconic = ones(6,10); - -pconic(1,1) = p11*p11; -pconic(1,2) = 2.0*p11*p12; -pconic(1,3) = p12*p12; -pconic(1,4) = 2.0*p11*p13; -pconic(1,5) = 2.0*p12*p13; -pconic(1,6) = p13*p13; -pconic(1,7) = 2.0*p11*p14; -pconic(1,8) = 2.0*p12*p14; -pconic(1,9) = 2.0*p13*p14; -pconic(1,10) = p14*p14; -pconic(2,1) = p21*p11; -pconic(2,2) = p21*p12+p22*p11; -pconic(2,3) = p22*p12; -pconic(2,4) = p21*p13+p23*p11; -pconic(2,5) = p22*p13+p23*p12; -pconic(2,6) = p23*p13; -pconic(2,7) = p21*p14+p24*p11; -pconic(2,8) = p22*p14+p24*p12; -pconic(2,9) = p23*p14+p24*p13; -pconic(2,10) = p24*p14; -pconic(3,1) = p21*p21; -pconic(3,2) = 2.0*p21*p22; -pconic(3,3) = p22*p22; -pconic(3,4) = 2.0*p21*p23; -pconic(3,5) = 2.0*p22*p23; -pconic(3,6) = p23*p23; -pconic(3,7) = 2.0*p21*p24; -pconic(3,8) = 2.0*p22*p24; -pconic(3,9) = 2.0*p23*p24; -pconic(3,10) = p24*p24; -pconic(4,1) = p31*p11; -pconic(4,2) = p31*p12+p32*p11; -pconic(4,3) = p32*p12; -pconic(4,4) = p31*p13+p33*p11; -pconic(4,5) = p32*p13+p33*p12; -pconic(4,6) = p33*p13; -pconic(4,7) = p31*p14+p34*p11; -pconic(4,8) = p32*p14+p34*p12; -pconic(4,9) = p33*p14+p34*p13; -pconic(4,10) = p34*p14; -pconic(5,1) = p31*p21; -pconic(5,2) = p31*p22+p32*p21; -pconic(5,3) = p32*p22; -pconic(5,4) = p31*p23+p33*p21; -pconic(5,5) = p32*p23+p33*p22; -pconic(5,6) = p33*p23; -pconic(5,7) = p31*p24+p34*p21; -pconic(5,8) = p32*p24+p34*p22; -pconic(5,9) = p33*p24+p34*p23; -pconic(5,10) = p34*p24; -pconic(6,1) = p31*p31; -pconic(6,2) = 2.0*p31*p32; -pconic(6,3) = p32*p32; -pconic(6,4) = 2.0*p31*p33; -pconic(6,5) = 2.0*p32*p33; -pconic(6,6) = p33*p33; -pconic(6,7) = 2.0*p31*p34; -pconic(6,8) = 2.0*p32*p34; -pconic(6,9) = 2.0*p33*p34; -pconic(6,10) = p34*p34; diff --git a/visionary/crossProductMatrix.m b/visionary/crossProductMatrix.m deleted file mode 100644 index 3ff2adc..0000000 --- a/visionary/crossProductMatrix.m +++ /dev/null @@ -1,9 +0,0 @@ -% A = crossProductMatrix(a) -% -% Takes as input a vector a in R^3 and outputs the corresponding cross -% product matrix, i.e., the matrix A such that A*u = a x u for all vectors -% u. -% -function A = crossProductMatrix(a) - A = [0, -a(3), a(2); a(3), 0, -a(1); -a(2), a(1), 0]; -end diff --git a/visionary/datastructures.m b/visionary/datastructures.m deleted file mode 100644 index 33c4d7d..0000000 --- a/visionary/datastructures.m +++ /dev/null @@ -1,68 +0,0 @@ -% VISIONARY/DATASTRUCTURES & CLASSES -% -% There are four main classes in Visionary. -% -% 1. imagedata - an image and its features, i.e. 2D data -% 2. imagesequence - a set of imagedata objects -% 3. structure - 3D data of scene -% 4. motion - camera motion rel. the scene and calibration -% -% The folling methods exist in these classes. For more information, -% type HELP CLASS/METHOD. -% -% 1. IMAGEDATA -% -% imagedata - create an IMAGEDATA object -% plot - plot imagedata -% plus - add all features of an imagedata object to another -% addpoints -% addlines -% addconics -% getfilename - return filename of image -% getpoints - return points in homogeneous coordinates -% getlines - return lines in stored format -% gethomogeneouslines - return lines in homogeneous coordinates -% getconics - return conics in dual coordinates -% clearpoints - clear points -% clearlines -% clearconics -% getimage - return image matrix -% loadimage - load image into memory with imread -% clearimage - clear current image in memory -% size - return number of points,lines & conics -% changecsystem - change coordinate system -% getnormtrans - return a "good" image transformation to better numerics -% -% 2. IMAGESEQUENCE -% Not implemented -% getcommonfeatures - return features present in all images -% -% 3. STRUCTURE -% -% structure - create a STRUCTURE object -% plot - plot structure in 3D -% plus - add all features of a structure object to another -% addpoints -% addlines -% addquadrics -% getpoints - return points in homogeneous coordinates -% getlines -% getpluckerlines - plucker coordinates of lines -% getquadrics -% clearpoints - clear points -% clearlines -% clearquadrics -% size - return number of points,lines & quadrics -% changecsystem - change coordinate system -% -% 4. MOTION -% -% motion - create a MOTION object -% plot - plot motion orbit -% addcameras - add cameras -% getcameras - return selected cameras -% size - return number of cameras -% changecsystem - change coordinate system -% focalpoints - calculate camera positions from camera matrices. -help datastructures - diff --git a/visionary/dist_radial.m b/visionary/dist_radial.m deleted file mode 100644 index 60298dd..0000000 --- a/visionary/dist_radial.m +++ /dev/null @@ -1,30 +0,0 @@ -function imseq2 = dist_radial(imseq1,f,k) -%distort all points in an image sequence -% INPUT: imseq1 - cell of imagedata objects OR cell of 2xN matrices -% f - vector of focal lengths -% k - 2xn matrix of radial distortion parameter -% OUTPUT: imseq2 - cell of imagedata objects -% -% MODEL: (x',y')=r(x,y)*(x,y) where r(x,y)=1+k(1)*(x^2+y^2)+k(2)*(x^2+y^2)^2 - -n=length(imseq1); -for ii=1:n, - if isa(imseq1{ii},'imagedata'), - tmp=pflat(getpoints(imseq1{ii}));tmp(3,:)=[]; - else - tmp=imseq1{ii}; - end - for jj=find(~isnan(tmp(1,:))); - tmp(:,jj)=calibrate(tmp(:,jj),f(ii),k(:,ii)); - end - imseq2{ii}=imagedata([],tmp); -end - - -function xc = calibrate(loc,f,k) - -% loc is the point location, f is the focal length, k are the radial distortion parameters. - -rdist = norm(loc); -xc=(1+rdist^2*k(1)+rdist^4*k(2))*loc*f; - diff --git a/visionary/epipolarMatches.m b/visionary/epipolarMatches.m deleted file mode 100644 index 5ecb765..0000000 --- a/visionary/epipolarMatches.m +++ /dev/null @@ -1,56 +0,0 @@ -% numMatches = epipolarMatches(image1, image2, KK, kc) -% -% Calculates the number of inliers for the epipolar geometry defined by the -% two input pictures. SIFT features are extracted from both images and -% matched, and then the number of inliers are found by performing 5pt -% RANSAC followed by bundle adjustment to calculate the essential matrix. -% Both images are plotted together with lines connecting the found matches -function numMatches = epipolarMatches(image1, image2, KK, kc) - % Extract SIFT features - if (ndims(image1) == 3) - image1 = single(rgb2gray(image1)); - else - image1 = single(image1); - end - %[f1,d1] = vl_sift(image1, 'PeakThresh', 1, 'EdgeThresh', 5); - [f1,d1] = vl_dsift(image1, 'size', 8, 'step', 2); - locs1 = f1([1,2],:); - - if (ndims(image2) == 3) - image2 = single(rgb2gray(image2)); - else - image2 = single(image2); - end - %[f2,d2] = vl_sift(image2, 'PeakThresh', 1, 'EdgeThresh', 5); - [f2,d2] = vl_dsift(image2, 'size', 8, 'step', 2); - locs2 = f2([1,2],:); - - % Match SIFT features between the two pictures - [ind1, ind2] = matchsiftvectors(d1,d2,0.8); - - % Find matches in normalized image coordinates - fc = KK([1 5]); - cc = KK(1:2,3); - alpha_c = KK(1,2)/fc(1); - pp1 = pextend(normalize(locs1(:,ind1),fc,cc,kc,alpha_c)); - pp2 = pextend(normalize(locs2(:,ind2),fc,cc,kc,alpha_c)); - - [inlierList,Pmax] = RANSAC_Essential(pp1, pp2, 0.004); - numMatches = nnz(inlierList); - - %%%%%%%%%%% PLOT THE RESULTS AS WELL %%%%%%%%%%%% - imSize = size(image1); - imbig = [image1, image2(1:imSize(1), 1:imSize(2))]; - figure; - imshow(uint8(imbig)); - im1Ind = ind1(find(inlierList)); - im2Ind = ind2(find(inlierList)); - for i = 1:length(im1Ind) - line([locs1(1,im1Ind(i));locs2(1,im2Ind(i))+size(image1,2)], [locs1(2,im1Ind(i)); locs2(2,im2Ind(i))]); - pause; - end - - figure; - plot(motion(Pmax)); -end - diff --git a/visionary/findaff.m b/visionary/findaff.m deleted file mode 100644 index b59087c..0000000 --- a/visionary/findaff.m +++ /dev/null @@ -1,23 +0,0 @@ -function a=findaff(data1,data2); -% a=FINDAFF(data1,data2) - finds affine transformation 'a' using points -% "a(data1) = data2" -% data1, data2 : structure or imagedata objects -% See also: changecsystem - -x = pflat(getpoints(data1)); -y = pflat(getpoints(data2)); - -m = size(x,1)-1; -x = x(1:m,:); -y = y(1:m,:); - -xmean=mean(x')'; -ymean=mean(y')'; - -dx = x-xmean*ones(1,size(x,2)); -dy = y-ymean*ones(1,size(y,2)); - -A = dy/dx; - -a = [A,ymean-A*xmean;zeros(size(xmean))',1]; - diff --git a/visionary/findeucl.m b/visionary/findeucl.m deleted file mode 100644 index 2ec2d32..0000000 --- a/visionary/findeucl.m +++ /dev/null @@ -1,53 +0,0 @@ -function e=findeucl(data1,data2,fixscale); -% e=FINDEUCL(data1,data2) - finds euclidean transformation 'e' using points -% "e(data1) = data2" -% data1, data2 : structure or imagedata objects -% See also: changecsystem - -if nargin<3, - fixscale=0; -end - - x = pflat(getpoints(data1)); - y = pflat(getpoints(data2)); - - m = size(x,1)-1; - x = x(1:m,:); - y = y(1:m,:); - - n = size(x,2); - X1= y; X2 = x; - - m1 = mean(X1'); - m2 = mean(X2'); - - X1 = X1-m1'*ones(1,n); %determining centroid - X2 = X2-m2'*ones(1,n); -if fixscale, - s1=1; - s2=1; -else - s1 = sum(sum(X1.^2).^.5); %determining scale - s2 = sum(sum(X2.^2).^.5); -end - - X1 = X1/s1; - X2 = X2/s2; - - - M = zeros(size(X1,1)); %determining rotation - for i =1:n - M = M+X1(:,i)*X2(:,i)'; - end - - [u,d,v] = svd(M); - - - - R = u*v'; - - if det(R)<0, - eye(m); ans(m,m) = -1; - R = u*ans*v'; - end - e = [s1/s2*R, -s1/s2*R*(m2')+m1'; zeros(1,m) 1]; diff --git a/visionary/findmorematches.m b/visionary/findmorematches.m deleted file mode 100644 index f1ddc27..0000000 --- a/visionary/findmorematches.m +++ /dev/null @@ -1,153 +0,0 @@ -function newimseq=findmorematches(images,imh,F12,F23,F13,threshold,intensitythreshold) -% blubb, -patchsize=5; -stdev=1.5; -borderignore=12+patchsize; - -nbrcorners=size(imh,1); -[imsz1,imsz2]=size(images{1}); -newpts1=[];newpts2=[];newpts3=[]; -for i=1:nbrcorners, - pt=getpoints(imh,i); -%figure(2);clf;plot(imagedata(images{2},pt)); -%figure(1);clf;plot(imagedata(images{1})); - - epiline1=F12*pt; - epiline1=epiline1/norm(epiline1(1:2)); - - horizontal=abs(epiline1(1))borderignore & uu(1)borderignore & uu(2)borderignore & destpos3(1)borderignore & destpos3(2)borderignore & uu(1)borderignore & uu(2)search - end %image 3 search along epipolar -end - -newimseq={imagedata(sparse(imsz1,imsz2),newpts1),imagedata(sparse(imsz1,imsz2),newpts2),imagedata(sparse(imsz1,imsz2),newpts3)}; - -%qq=1;figure(qq);clf;plot(imagedata(double(images{qq}),destpos1,epiline1));zoom on;hold on; -%qq=2;figure(qq);plot(imagedata(double(images{qq}),pt));zoom on; -%qq=3;figure(qq);plot(imagedata(double(images{qq}),destpos3,[epiline3,epiline3b]));zoom on; diff --git a/visionary/findpatch.m b/visionary/findpatch.m deleted file mode 100644 index 9dbe1f1..0000000 --- a/visionary/findpatch.m +++ /dev/null @@ -1,263 +0,0 @@ -function [pos2,residual]=findpatch(im1,im2,pos1,predpos,precalc) -% pos2=findpatch(im1,im2,pos1,predpos,precalc) -% Finds a patch in image im2, -% using a patch around pos1 in image im1 -% by means of affine subpixel correlation -% Input: -% im1,im2 - image matrices -% pos1 - position in image im1 -% predpos - predicted position in im2 (optional) -% precalc - precalculated data (optional) -% This used if findpatch is called several times. -% Output: -% pos2 - position in image im2 -% residual - intensity residual between patches -% See also: findpatch2 - -patchsize=5; -searchsize=100; -stdev=1.5; - - -if nargin<4 - predpos=pos1; -end; -if sum(pos1(1:2)0 | ... - pos1(1)>size(im1,2)-patchsize-1 | ... - pos1(2)>size(im1,1)-patchsize-1, - residual=Inf; - pos2=predpos; -else -%%%%%%%%%%%%%%%%%%% - - -patch=im1(round(pos1(2))+(-patchsize:patchsize),round(pos1(1))+(-patchsize:patchsize)); - -index1=max(1,round(predpos(2))-searchsize):min(size(im2,1),round(predpos(2))+searchsize); -index2=max(1,round(predpos(1))-searchsize):min(size(im2,2),round(predpos(1))+searchsize); -searchpatch=im2(index1,index2); - -if nargin<5 - A2conv = conv2(searchpatch.*searchpatch,ones(2*patchsize+1),'valid'); -else - A2conv=im2conv(index1(1+patchsize:end-patchsize)-patchsize,... - index2(1+patchsize:end-patchsize)-patchsize); -end - -patch = flipud(fliplr(patch)); -%B2 = sum(sum(patch.*patch)); -C = conv2(searchpatch,patch,'valid'); -%disp(['Calculating correlation']); -%res = B2 .* ones(size(C)) + A2conv - 2.*C; -res = A2conv - 2.*C; -%disp(['Finding minimum']); - - - -[posxx,posyy]=find(res==min(res(:))); -posxx=posxx(1)-1+index1(1)+patchsize; -posyy=posyy(1)-1+index2(1)+patchsize; - -%disp(min(res(:))+sum(sum(patch.*patch))); -%figure(2);plot(posyy,posxx,'ro'); - -startpos=pos1(1:2)'; -destpos=[posyy,posxx]; - -Astart = calcAstartC(startpos,destpos); - -[u, Afirst1] = affineoptC(im1, im2, startpos, Astart, patchsize, stdev); - -residual=affineresC(im1,im2,u,Afirst1,stdev); - - -um = mean(u'); -A00 = [1 0 -um(1) ; ... - 0 1 -um(2) ; ... - 0 0 1 ]; -u1 = A00*u; -u1m = std(u1'); -A01 = [ 1/u1m(1) 0 0 ; 0 1/u1m(2) 0 ; 0 0 1]; -A0 = A01*A00; -Aopt= inv(A0)*Afirst1*A0; - -pos2=Aopt*[startpos,1]'; - -%%%%%%%%%%%%%%%%%%% -end % if pos outside image - -%%%%%%%%%%%%%%%%%%% -function [patch, A] = affineoptC(bild1, bild2, pos1, A, patchsize, a); -patch = createpatch(pos1,patchsize); - -[f0, Idiff, Idiffgrad] = affineresC(bild1, bild2,patch,A,a); - -dA= inf; -counter=0; -while log(norm(dA)+eps)/log(10)>-6 & counter<30; -% log(norm(dA)+eps)/log(10) - S = Idiffgrad'*Idiffgrad; - dA=inv(S+0.001*eye(size(S,1)))*Idiffgrad'*Idiff; - Anew=A+reshape([dA ; 0 ; 0 ; 0],size(A))'; - [f1, Idiff, Idiffgrad] = affineresC(bild1, bild2,patch,Anew,a); - while f1>f0 & counter<30, -% f0 -% f1 -% disp('Det har tar vi en gang till'); - dA=dA/2; - Anew=A+reshape([dA ; 0 ; 0 ; 0],size(A))'; - [f1, Idiff, Idiffgrad] = affineresC(bild1, bild2,patch,Anew,a); - counter=counter+1; - end; - - if f1<=f0, - f0=f1; - A = Anew; - end; - counter=counter+1; -end; - - -function [f,intendiff, intendiffgrad, i1, i2]=affineresC(image1,image2,points1,A,a); - -% First calculate A0 - -um = mean(points1'); - -A00 = [1 0 -um(1) ; ... - 0 1 -um(2) ; ... - 0 0 1 ]; - -u1 = A00*points1; -u1m = std(u1'); -A01 = [ 1/u1m(1) 0 0 ; 0 1/u1m(2) 0 ; 0 0 1]; -A0 = A01*A00; - -points2 = inv(A0)*A*A0*points1; - -if max(points2(1,:))>(size(image2,2)-5) | ... - max(points2(2,:))>(size(image2,1)-5) | ... - min(points2(1,:))<6 | ... - min(points2(2,:))<6, - - f=Inf; - intendiff=Inf; - intendiffgrad=Inf*ones(1,6); - i1=Inf; - i2=Inf; -else - -%%%%%%% - - - -[i1, dummy] = measure(image1,points1,a); -[i2, i2grad] = measure(image2,points2,a); -intendiff = (i1-i2)'; -M = eye([9 9]); -d1 = inv(A0)*reshape(M(1,1:9),3,3)'*A0; -d2 = inv(A0)*reshape(M(2,1:9),3,3)'*A0; -d3 = inv(A0)*reshape(M(3,1:9),3,3)'*A0; -d4 = inv(A0)*reshape(M(4,1:9),3,3)'*A0; -d5 = inv(A0)*reshape(M(5,1:9),3,3)'*A0; -d6 = inv(A0)*reshape(M(6,1:9),3,3)'*A0; - -dAx1 = d1 * points1; -dAx2 = d2 * points1; -dAx3 = d3 * points1; -dAx4 = d4 * points1; -dAx5 = d5 * points1; -dAx6 = d6 * points1; - -dfel1 = sum(i2grad .* dAx1(1:2,:)); -dfel2 = sum(i2grad .* dAx2(1:2,:)); -dfel3 = sum(i2grad .* dAx3(1:2,:)); -dfel4 = sum(i2grad .* dAx4(1:2,:)); -dfel5 = sum(i2grad .* dAx5(1:2,:)); -dfel6 = sum(i2grad .* dAx6(1:2,:)); - - -intendiffgrad = [ dfel1 ; dfel2 ; dfel3 ; dfel4 ; dfel5 ; dfel6]'; - -f = intendiff'*intendiff; - - -end; - - - -function u = createpatch(pos,s); -% Creates a list of points that make up a patch. -% (Rectangular shape) - -i=1; -for x=-s:s - for y=-s:s - u(1:3,i)=[pos(1)+x pos(2)+y 1]'; - i=i+1; - end; -end; - -function [inten, intengrad]=measure(image,points,a); - -inten=zeros(1,size(points,2)); -intengrad=zeros(2,size(points,2)); - -[m,n]=size(image); -NN=round(a*3); - - -for i=1:size(points,2) - - -%%%%%%%% [In, Igrad] = measurepoint(image,points(1:2,i),a); -%measurepoint -%function [inten, intengrad]=measurepoint(bild,punkt,a); - - -x0=points(1,i); -y0=points(2,i); - - -cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -cutbild=image(cuty,cutx); - -[x,y]=meshgrid(cutx,cuty); - -filter=exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^2*pi); -filtx=2*(x-x0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -filty=2*(y-y0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -%if size(filtx,1)~=size(cutbild,1) | size(filtx,2)~=size(cutbild,2), -% keyboard; -%end -Igrad(1,1)=sum(sum(filtx.*cutbild)); -Igrad(2,1)=sum(sum(filty.*cutbild)); -In=sum(sum(filter.*cutbild)); - -%measurepoint -%%%%%%%%%%%%%%%%%%%% - - - inten(i) = In; - intengrad(1:2,i) = Igrad; -end; - - -function Astart = calcAstartC(pos1, pos2); - -delta = pos2-pos1; -A = eye(3); -A(1,3)=delta(1); -A(2,3)=delta(2); - -um=pos1; - -A00 = [1 0 -um(1) ; 0 1 -um(2) ; 0 0 1 ]; -A01 = [ 1/3.1754 0 0 ; 0 1/3.1754 0 ; 0 0 1]; -A0 = A01*A00; - -%Astart = inv(A0)*A*A0; -Astart = A0*A*inv(A0); - - diff --git a/visionary/findpatch2.m b/visionary/findpatch2.m deleted file mode 100644 index 25b786b..0000000 --- a/visionary/findpatch2.m +++ /dev/null @@ -1,305 +0,0 @@ -function [pos2,residual]=findpatch2(im1,im2,pos1,predpos,precalc) -% [pos2,residual]=findpatch2(im1,im2,pos1,predpos,precalc) -% Finds a patch in image im2, -% using a patch around pos1 in image im1 -% by means of affine subpixel normalized correlation -% Input: -% im1,im2 - image matrices -% pos1 - position in image im1 -% predpos - predicted position in im2 (optional) -% precalc - Precalculated data (optional) -% This used if findpatch2 is called several times. -% Output: -% pos2 - position in image im2 -% residual - intensity residual between patches -% See also: findpatch - -patchsize=5; - -searchsize=120; -stdev=1.5; - - -if nargin<4 - predpos=pos1; -end; -if sum(pos1(1:2)0 | ... - pos1(1)>size(im1,2)-patchsize-1 | ... - pos1(2)>size(im1,1)-patchsize-1, - residual=Inf; - pos2=predpos; -else -%%%%%%%%%%%%%%%%%%% -n=2*patchsize+1; -index1=max(1,round(predpos(2))-searchsize):min(size(im2,1),round(predpos(2))+searchsize); -index2=max(1,round(predpos(1))-searchsize):min(size(im2,2),round(predpos(1))+searchsize); -searchpatch=im2(index1,index2); - -if nargin<5 - searchones=conv2(searchpatch,ones(n),'valid'); - search2ones=conv2(searchpatch.^2,ones(n),'valid'); - searchsquared=search2ones-searchones.^2/n^2; - searchsquared(find(searchsquared==0))=1; -else - searchsquared=precalc(index1(1+patchsize:end-patchsize)-patchsize,... - index2(1+patchsize:end-patchsize)-patchsize); - -%im2conv=conv2(images{2}.^2,ones(n),'valid')-conv2(images{2},ones(n),'valid').^2/n^2; - -end - - -patch=im1(round(pos1(2))+(-patchsize:patchsize),round(pos1(1))+(-patchsize:patchsize)); -patch = flipud(fliplr(patch)); - -patch=patch-mean(patch(:)); -patch2=sum(patch(:).^2);if patch2==0,patch2=1;end; - -s_patch=conv2(searchpatch,patch,'valid'); - -distmap=(searchsquared+patch2-2*s_patch)./sqrt(searchsquared)/sqrt(patch2); - -[posxx,posyy]=find(distmap==min(distmap(:))); -posxx=posxx(1)-1+index1(1)+patchsize; -posyy=posyy(1)-1+index2(1)+patchsize; - - -%pos2=[posyy,posxx]'; -%residual=min(distmap(:)); - -startpos=pos1(1:2)'; -destpos=[posyy,posxx]; - -Astart = calcAstartC(startpos,destpos); - -[u, Afirst1,residual] = affineoptC(im1, im2, startpos, Astart, patchsize, stdev); -%[u, Afirst1,residual] = affineoptC(im1, im2, startpos, Astart, 4,2.5); - -%figure(1);clf;hold on;plot(imagedata(im1,startpos'),'ro');zoom on; -%figure(2);clf;hold on;plot(imagedata(im2,destpos'),'ro');zoom on; -%residual, -%min(distmap(:)) -%keyboard; - - -um = mean(u'); -A00 = [1 0 -um(1) ; ... - 0 1 -um(2) ; ... - 0 0 1 ]; -u1 = A00*u; -u1m = std(u1'); -A01 = [ 1/u1m(1) 0 0 ; 0 1/u1m(2) 0 ; 0 0 1]; -A0 = A01*A00; -Aopt= inv(A0)*Afirst1*A0; - -pos2=Aopt*[startpos,1]'; - - - - - -%%%%%%%%%%%%%%%%%%% -end % if pos outside image - -%%%%%%%%%%%%%%%%%%% -function [patch, A,f0] = affineoptC(bild1, bild2, pos1, A, patchsize, a); -patch = createpatch(pos1,patchsize); - -[f0, Idiff, Idiffgrad] = affineresC(bild1, bild2,patch,A,a); -dA= inf; -counter=0; -while log(norm(dA)+eps)/log(10)>-6 & counter<30; -% log(norm(dA)+eps)/log(10) - S = Idiffgrad'*Idiffgrad; - dA=-inv(S+0.001*eye(size(S,1)))*Idiffgrad'*Idiff; - Anew=A+reshape([dA ; 0 ; 0 ; 0],size(A))'; - [f1, Idiff, Idiffgrad] = affineresC(bild1, bild2,patch,Anew,a); - while f1>f0 & counter<30, -% f0 -% f1 -% disp('Det har tar vi en gang till'); - dA=dA/2; - Anew=A+reshape([dA ; 0 ; 0 ; 0],size(A))'; - [f1, Idiff, Idiffgrad] = affineresC(bild1, bild2,patch,Anew,a); - counter=counter+1; - end; - - if f1<=f0, - f0=f1; - A = Anew; - end; - counter=counter+1; -end; - - -function [f,intendiff_normalized, intendiffgrad, i1, i2]=... - affineresC(image1,image2,points1,A,a); - -% First calculate A0 - -um = mean(points1'); - -A00 = [1 0 -um(1) ; ... - 0 1 -um(2) ; ... - 0 0 1 ]; - -u1 = A00*points1; -u1m = std(u1'); -A01 = [ 1/u1m(1) 0 0 ; 0 1/u1m(2) 0 ; 0 0 1]; -A0 = A01*A00; - -points2 = inv(A0)*A*A0*points1; - -if max(points2(1,:))>(size(image2,2)-5) | ... - max(points2(2,:))>(size(image2,1)-5) | ... - min(points2(1,:))<6 | ... - min(points2(2,:))<6, - - f=Inf; - intendiff_normalized=Inf; - intendiffgrad=Inf*ones(1,6); - i1=Inf; - i2=Inf; -else - -%%%%%%% - - - -[i1, dummy] = measure(image1,points1,a); -[i2, i2grad] = measure(image2,points2,a); -i1mean=i1-mean(i1); -i2mean=i2-mean(i2); - -intendiff = (i2mean-i1mean); -i1square=sum(i1mean.^2); -N=i1square*sum(i2mean.^2); - -M = eye([9 9]); -d1 = inv(A0)*reshape(M(1,1:9),3,3)'*A0; -d2 = inv(A0)*reshape(M(2,1:9),3,3)'*A0; -d3 = inv(A0)*reshape(M(3,1:9),3,3)'*A0; -d4 = inv(A0)*reshape(M(4,1:9),3,3)'*A0; -d5 = inv(A0)*reshape(M(5,1:9),3,3)'*A0; -d6 = inv(A0)*reshape(M(6,1:9),3,3)'*A0; - -dAx1 = d1 * points1; -dAx2 = d2 * points1; -dAx3 = d3 * points1; -dAx4 = d4 * points1; -dAx5 = d5 * points1; -dAx6 = d6 * points1; - -tmp = sum(i2grad .* dAx1(1:2,:)); -dfel1 = tmp-mean(tmp); -tmp = sum(i2grad .* dAx2(1:2,:)); -dfel2 = tmp-mean(tmp); -tmp = sum(i2grad .* dAx3(1:2,:)); -dfel3 = tmp-mean(tmp); -tmp = sum(i2grad .* dAx4(1:2,:)); -dfel4 = tmp-mean(tmp); -tmp = sum(i2grad .* dAx5(1:2,:)); -dfel5 = tmp-mean(tmp); -tmp= sum(i2grad .* dAx6(1:2,:)); -dfel6 = tmp-mean(tmp); - - -dN1=2*sum(i2mean.*dfel1)*i1square; -ddfel1=dfel1/N^(1/4)-intendiff/4/N^(5/4)*dN1; -dN2=2*sum(i2mean.*dfel2)*i1square; -ddfel2=dfel2/N^(1/4)-intendiff/4/N^(5/4)*dN2; -dN3=2*sum(i2mean.*dfel3)*i1square; -ddfel3=dfel3/N^(1/4)-intendiff/4/N^(5/4)*dN3; -dN4=2*sum(i2mean.*dfel4)*i1square; -ddfel4=dfel4/N^(1/4)-intendiff/4/N^(5/4)*dN4; -dN5=2*sum(i2mean.*dfel5)*i1square; -ddfel5=dfel5/N^(1/4)-intendiff/4/N^(5/4)*dN5; -dN6=2*sum(i2mean.*dfel6)*i1square; -ddfel6=dfel6/N^(1/4)-intendiff/4/N^(5/4)*dN6; - - -intendiffgrad = [ ddfel1 ; ddfel2 ; ddfel3 ; ddfel4 ; ddfel5 ; ddfel6]'; - -intendiff_normalized=intendiff'/N^(1/4); - - -f = intendiff_normalized'*intendiff_normalized; - -end; - -function u = createpatch(pos,s); -% Creates a list of points that make up a patch. -% (Rectangular shape) - -i=1; -for x=-s:s - for y=-s:s - u(1:3,i)=[pos(1)+x pos(2)+y 1]'; - i=i+1; - end; -end; - -function [inten, intengrad]=measure(image,points,a); - -nbr=size(points,2); - -inten=zeros(1,nbr); -intengrad=zeros(2,nbr); - -[m,n]=size(image); -NN=round(a*3); - - -for i=1:nbr, - - -%%%%%%%% [In, Igrad] = measurepoint(image,points(1:2,i),a); -%measurepoint -%function [inten, intengrad]=measurepoint(bild,punkt,a); - - -x0=points(1,i); -y0=points(2,i); - - -cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -cutbild=image(cuty,cutx); - -[x,y]=meshgrid(cutx,cuty); - -filter=exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^2*pi); -filtx=2*(x-x0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -filty=2*(y-y0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -%if size(filtx,1)~=size(cutbild,1) | size(filtx,2)~=size(cutbild,2), -% keyboard; -%end -Igrad(1,1)=sum(sum(filtx.*cutbild)); -Igrad(2,1)=sum(sum(filty.*cutbild)); -In=sum(sum(filter.*cutbild)); - -%measurepoint -%%%%%%%%%%%%%%%%%%%% - - - inten(i) = In; - intengrad(1:2,i) = Igrad; -end; - -function Astart = calcAstartC(pos1, pos2); - -delta = pos2-pos1; -A = eye(3); -A(1,3)=delta(1); -A(2,3)=delta(2); - -um=pos1; - -A00 = [1 0 -um(1) ; 0 1 -um(2) ; 0 0 1 ]; -A01 = [ 1/3.1754 0 0 ; 0 1/3.1754 0 ; 0 0 1]; -A0 = A01*A00; - -%Astart = inv(A0)*A*A0; -Astart = A0*A*inv(A0); - - diff --git a/visionary/findpatch3.m b/visionary/findpatch3.m deleted file mode 100644 index c17213d..0000000 --- a/visionary/findpatch3.m +++ /dev/null @@ -1,169 +0,0 @@ -function [pos2,reslist]=findpatch3(im1,im2,pos1,pred2,searchsize) -% [pos2,residual]=findpatch3(im1,im2,pos1) -% Finds a patch in image im2, -% using a patch around pos1 in image im1 -% by means of affine subpixel correlation -% Input: -% im1,im2 - image matrices -% pos1 - positions in image im1 -% pred2 - predicted position (optional) -% searchsize - size of search area (optional) -% Output: -% pos2 - positions in image im2 -% residual - intensity residual between patches -% See also: findpatch,findpatch2 - -if nargin<=3 | isempty(pred2), - pred2=pos1; -end; -if nargin<=4, - searchsize=100; -end; - -patchsize=5; -stdev=1.5; -n=2*patchsize+1; - -alfa1=2/180*pi; -alfa2=4/180*pi; -scale1=0.06; -scale2=0.12; - -nbrpoints=size(pos1,2); -pos2=NaN*ones(3,nbrpoints); -reslist=Inf*ones(1,nbrpoints); - -borderignore=8; - -a=stdev; -[x,y]=meshgrid(-3:3,-3:3); -filter=exp(- ((x).^2 + (y).^2)/a^2)/(a^2*pi); -im1filter=conv2(im1,filter,'same'); -im2filter=conv2(im2,filter,'same'); - -im2ones=conv2(im2filter,ones(n),'valid'); -im2ones(find(im2ones==0))=1; -im2rho=1./im2ones; -im2squared=conv2(im2filter.^2,ones(n),'valid'); - - - -for pp=1:nbrpoints, - - pt1=pos1(:,pp); - predpos=pred2(:,pp); - - - if isnan(pt1(1)) | sum(pt1(1:2)<2*patchsize+1)>0 | ... - pt1(1)>size(im1,2)-2*patchsize | ... - pt1(2)>size(im1,1)-2*patchsize, - residual=Inf; - pt2=NaN*ones(3,1); -else %inside image -%%%%%%%%%%%%%%%%%%% - - - - -index1=max(1+borderignore,round(predpos(2))-searchsize):min(size(im2,1)-borderignore,round(predpos(2))+searchsize); -index2=max(1+borderignore,round(predpos(1))-searchsize):min(size(im2,2)-borderignore,round(predpos(1))+searchsize); -searchpatch=im2filter(index1,index2); - -searchsquared=im2squared(index1(1+patchsize:end-patchsize)-patchsize,... - index2(1+patchsize:end-patchsize)-patchsize); - -rho=im2rho(index1(1+patchsize:end-patchsize)-patchsize,... - index2(1+patchsize:end-patchsize)-patchsize); - - -rhosquared=rho.^2; - -bestdist=Inf; -for method=1:5, - - switch method, - case 1, - patch=im1filter(round(pt1(2))+(-patchsize:patchsize),... - round(pt1(1))+(-patchsize:patchsize)); - theR=eye(2); - case 2, - tmp=imrotate(im1filter(round(pt1(2))+(-2*patchsize:2*patchsize),... - round(pt1(1))+(-2*patchsize:2*patchsize)),alfa1*180/pi,'bicubic','crop'); - patch=tmp(patchsize+1:3*patchsize+1,patchsize+1:3*patchsize+1); - theR=[cos(alfa1),sin(alfa1);-sin(alfa1),cos(alfa1)]; - case 3, - tmp=imrotate(im1filter(round(pt1(2))+(-2*patchsize:2*patchsize),... - round(pt1(1))+(-2*patchsize:2*patchsize)),-alfa1*180/pi,'bicubic','crop'); - patch=tmp(patchsize+1:3*patchsize+1,patchsize+1:3*patchsize+1); - theR=[cos(alfa1),sin(-alfa1);sin(alfa1),cos(alfa1)]; - case 4, - tmp=imresize(im1filter(round(pt1(2))+(-patchsize:patchsize),... - round(pt1(1))+(-patchsize:patchsize)),1+scale1,'bicubic'); - iii=(size(tmp,1)+1)/2+(-patchsize:patchsize); - patch=tmp(iii,iii); - theR=diag(1+[scale1,scale1]); - case 5, - tmp=imresize(im1filter(round(pt1(2))+(-2-patchsize:patchsize+2),... - round(pt1(1))+(-2-patchsize:2+patchsize)),1-scale1,'bicubic'); - iii=(size(tmp,1))/2+(-patchsize:patchsize); - patch=tmp(iii,iii); - theR=diag(1-[scale1,scale1]); - case 6, - tmp=imrotate(im1filter(round(pt1(2))+(-2*patchsize:2*patchsize),... - round(pt1(1))+(-2*patchsize:2*patchsize)),alfa2*180/pi,'bicubic','crop'); - patch=tmp(patchsize+1:3*patchsize+1,patchsize+1:3*patchsize+1); - theR=[cos(alfa2),sin(alfa2);-sin(alfa2),cos(alfa2)]; - case 7, - tmp=imrotate(im1filter(round(pt1(2))+(-2*patchsize:2*patchsize),... - round(pt1(1))+(-2*patchsize:2*patchsize)),-alfa2*180/pi,'bicubic','crop'); - patch=tmp(patchsize+1:3*patchsize+1,patchsize+1:3*patchsize+1); - theR=[cos(alfa2),sin(-alfa2);sin(alfa2),cos(alfa2)]; - case 8, - tmp=imresize(im1filter(round(pt1(2))+(-patchsize:patchsize),... - round(pt1(1))+(-patchsize:patchsize)),1+scale2,'bicubic'); - iii=size(tmp,1)/2+(-patchsize:patchsize); - patch=tmp(iii,iii); - theR=diag(1+[scale2,scale2]); - case 9, - tmp=imresize(im1filter(round(pt1(2))+(-3-patchsize:patchsize+3),... - round(pt1(1))+(-3-patchsize:3+patchsize)),1-scale2,'bicubic'); - iii=size(tmp,1)/2+(-patchsize:patchsize); - patch=tmp(iii,iii); - theR=diag(1-[scale2,scale2]); - end - - patch = flipud(fliplr(patch)); - patchsquared=sum(patch(:).^2);if patchsquared==0,patchsquared=1;end; - patchsum=sum(patch(:)); - - search_vs_patch=conv2(searchpatch,patch,'valid'); - - distmap=patchsum^2*(rho.^2).*searchsquared+patchsquared-... - 2*patchsum*rho.*search_vs_patch; - dist=min(distmap(:)); - - if distborderignore & uu(1)borderignore & uu(2) abs(x(i))), - cot = -x(i)/y(i); si = 1/sqrt(1+cot^2); co = si*cot; - else - tan = -y(i)/x(i); co = 1/sqrt(1+tan^2); si = co*tan; - end - s(i) = si; c(i) = co; - end - -%end % rot_cossin - -function [C,u,dudpv]=ellipsparam2dualconic(paramvector); - -alfa = paramvector(1); -a = paramvector(2); -b = paramvector(3); -z = paramvector(4:5); - - -D = diag([a^2 b^2 -1]); -T1 = [cos(alfa) -sin(alfa) 0; sin(alfa) cos(alfa) 0; 0 0 1]; -T2 = [eye(2) z; 0 0 1]; -C = T2*T1*D*T1'*T2'; -dT2dz1 = [0 0 1;0 0 0;0 0 0]; -dT2dz2 = [0 0 0;0 0 1;0 0 0]; -dT1dalfa = [-sin(alfa) -cos(alfa) 0;cos(alfa) -sin(alfa) 0; 0 0 0]; -dDda = diag([2*a 0 0]); -dDdb = diag([0 2*b 0]); - -u=m2v(C); -dudpv = [ ... -m2v(T2*dT1dalfa*D*T1'*T2' + T2*T1*D*dT1dalfa'*T2') ... -m2v(T2*T1*dDda*T1'*T2') ... -m2v(T2*T1*dDdb*T1'*T2') ... -m2v(dT2dz1*T1*D*T1'*T2' + T2*T1*D*T1'*dT2dz1') ... -m2v(dT2dz2*T1*D*T1'*T2' + T2*T1*D*T1'*dT2dz2') ... -]; - - -function v=m2v2(m); -% M2V v=m2v(m) returns vector of symmetric conic/quadric matrix - -if size(m,1)==4, - v=[m(1,1),2*m(1,2),m(2,2),2*m(1,3),2*m(2,3),m(3,3),... - 2*m(1,4),2*m(2,4),2*m(3,4),m(4,4)]'; -elseif size(m,1)==3, - v=[m(1,1),2*m(1,2),m(2,2),2*m(1,3),2*m(2,3),m(3,3)]'; -else - error('Wrong matrix-dimension'); -end - - -function [un,L,n]=fitconic2(points,normals); -% [un,L,n]=fitline2(points,normals); -% INPUT: -% points - image point positions in homogeneous coordinates (3xn matrix). -% normals - error normals for each point (3xn matrix) -% OUTPUT: -% c - line parameters -% L - cholesky factorisation of inverse of covariance of c. -% -% -% A future version [un,L,n]=fitconic2(points,stddevs,normals); -% could incorporate known standard deviation in estimated point -% positions. -% stddevs - estimated standard deviation for each image point (1xn matrix) - - sc=500; %rescale points - - K=diag([1/sc 1/sc 1]); - kpoints=K*points; - M=[]; - dC=zeros(3,3,6); - E=eye(6); - for i=1:6, dC(:,:,i)=v2m(E(:,i)); end; - for i=1:size(kpoints,2); - M=[M; m2v2(kpoints(:,i)*kpoints(:,i)')']; - end; - [U,S,V]=svd(M); - u=V(:,6); - ss=zeros(1,size(kpoints,2)); - for k=1:5; - u=u/norm(u); - C=v2m(u); - for kk=1:5; - idealpoints=kpoints+normals*diag(ss); - %diag(idealpoints'*C*idealpoints)' - deltass=-min(max((diag(idealpoints'*C*idealpoints))./ ... - (diag(normals'*C*idealpoints)+... - diag(idealpoints'*C*normals)),-0.02),0.02)'; % KOLLA DESSA GRÄNSER - ss = ss+deltass; - normss=norm(deltass); - end; - idealpoints=kpoints+normals*diag(ss); - for i=1:6, - dss1dC(:,i) = -diag(idealpoints'*dC(:,:,i)*idealpoints)./ ... - (diag(normals'*C*idealpoints)+... - diag(idealpoints'*C*normals)) + ... - (diag(normals'*dC(:,:,i)*idealpoints)+... - diag(idealpoints'*dC(:,:,i)*normals)).* ... - diag(idealpoints'*C*idealpoints)./ ... - (diag(normals'*C*idealpoints)+... - diag(idealpoints'*C*normals)).^2; - end; -% u(1)=u(1)-litet; -% u(2)=u(2)+litet; -% ss1=ss'; -% [(ss1-ss0)/litet dss1dC(:,2)] - [U,S,V]=svd(dss1dC); - du = V(:,1:5)*inv(S(1:5,1:5))*U(:,1:5)'*(ss'); - u=u-du; - normdu=norm(du); - stddev=sqrt( (ss*ss')/(size(ss,2)-5) ); - end; - %normss=norm(deltass) - %normdu=norm(du) - %stddev=sqrt( (ss*ss')/(size(ss,2)-5) ); - % Estimate standard deviation in normal direction - % from residuals. - [U,S,V]=svd(dss1dC); - M= V(:,1:5)*inv(S(1:5,1:5))*U(:,1:5)'; - Cu = M*diag(stddev^2*ones(size(ss)))*M'; - % transformera till - E=eye(6); - C1 = K'*v2m(u)*K; - C2 = inv( K'*v2m(u)*K ); - up = m2v(inv( K'*v2m(u)*K )); - for i=1:6, - dupdu(:,i) = -m2v( C2*K'*v2m(E(:,i))*K*C2 ); - end; -% up0 = m2v(inv( K'*v2m(u)*K )); -% up1 = m2v(inv( K'*v2m(u+litet*E(:,i))*K )); -% [(up1-up0)/litet dupdu(:,i)]; - Cup = dupdu*Cu*dupdu'; - % PROJICERA NER up - n=up/norm(up); - un= up/(up'*n); - dundup = (eye(size(up,1))/(up'*n) - up*n'/(up'*n)^2); - Cun = dundup*Cup*dundup'; - [U,S,V]=svd(Cun); - U=U(:,1:5); - S=S(1:5,1:5); - L=inv(sqrtm(S))*U'; - n=un; - -% END OF fitconic2 - diff --git a/visionary/fitline.m b/visionary/fitline.m deleted file mode 100644 index a2745a4..0000000 --- a/visionary/fitline.m +++ /dev/null @@ -1,105 +0,0 @@ -function i=fitline(points,normals); -% i=fitline(points,normals); -% INPUT: -% points - image point positions in homogeneous coordinates (3xn matrix). -% normals - error normals for each point (3xn matrix) -% OUTPUT: -% i - imagedata object -% If no normals are specified, the error is assumed to be isotropic. - - -if nargin<=1, - [l,L]=fitline1(points); -else - [l,L]=fitline2(points,normals); -end - -i=imagedata; -i=addlines(i,l,L); - - -function [l,L]=fitline1(points); -% [l,L]=fitline1(points); -% INPUT: -% points - image point positions in homogeneous coordinates (3xn matrix). -% OUTPUT: -% l - line parameters -% L - cholesky factorisation of inverse of covariance of l. -% - -% Normalise points - points=pflat(points); - -% Rough estimate - [u,S,v]=svd(points); - l=u(:,3); - l=l/norm(l(1:2)); - -% Find line such that l'*idealpoints=0, with idealpoints as close -% to points as possible. - for i=1:5; - dldx = [0 0 100;-l(2) l(1) 0]'; -% res = (points'*l)./stddevs'; -% dresdx = points'./(stddevs'*ones(1,3))*dldx; - res = (points'*l); - dresdx = points'*dldx; - dl=-dldx*(dresdx\res); - l=l+dl; - l=l/norm(l(1:2)); -% norm(dl) - end; - estimstd = sqrt((res'*res) / (size(res,1)-2)); - - Fx = dresdx'*dresdx/estimstd^2; - Cx = pinv(Fx); -% Lx = chol(Fx); -% Fl = dldx*Fx*dldx'; % FEL -% L = Lx*dldx'; - Cl = dldx*Cx*dldx'; - %pinv(Fl); - [U,S,V]=svd(Cl); - U=U(:,1:2); - S=S(1:2,1:2); - L=inv(sqrtm(S))*U'; - -function [l,L]=fitline2(points,normals); -% [l,L]=fitline2(points); -% INPUT: -% points - image point positions in homogeneous coordinates (3xn matrix). -% normals - error normals for each point (3xn matrix) -% OUTPUT: -% l - line parameters -% L - cholesky factorisation of inverse of covariance of l. -% - - [u,S,v]=svd(points); - u=u(:,3); - for k=1:5; - u=u/norm(u); - ss = -(u'*points)./(u'*normals); - normss=norm(ss); - idealpoints=points+normals*diag(ss); - meanpoint = mean(points')'; - dssdu = -(points*diag(ones(size(ss))./(u'*normals)) - ... - normals*diag(u'*points./((u'*normals).^2))); - [U,S,V]=svd(dssdu'); - du = V(:,1:2)*inv(S(1:2,1:2))*U(:,1:2)'*(ss'); - u=u-du; - end; -% normdu=norm(du) - stddev=sqrt(ss*ss'/(size(ss,2)-2)); - M= V(:,1:2)*inv(S(1:2,1:2))*U(:,1:2)'; - Cu = M*diag(stddev^2*ones(size(ss)))*M'; -% [U,S,V]=svd(Cu); -% U=U(:,1:2); -% S=S(1:2,1:2); -% L=inv(sqrtm(S))*U'; -% keyboard; - - l=u/norm(u(1:2)); - dldu = eye(3)/norm(u(1:2)) -u*(1/norm(u(1:2))^3)*[u(1:2)' 0]; - Cl = dldu*Cu*dldu'; - [U,S,V]=svd(Cl); - U=U(:,1:2); - S=S(1:2,1:2); - L=inv(sqrtm(S))*U'; diff --git a/visionary/flexlinear.m b/visionary/flexlinear.m deleted file mode 100644 index 0f6a255..0000000 --- a/visionary/flexlinear.m +++ /dev/null @@ -1,84 +0,0 @@ -function T=flexlinear(mot,pp); -% function T=flexlinear(mot,pp); -% Computes projective to Euclidean upgrade linearly -% using all intrinsics known but focallengths -% Input: -% mot - projective motion -% pp - approximate principal point (optional) -% Output: -% T - 4 x 4 coordinate transformation for Euclidean upgrade -% Use changecsystem(mot,T) for upgrade! -% Default: pp=[0,0]'; - -if nargin<2, - pp=[0,0]'; -end; - -Kpp=eye(3);Kpp(1:2,3)=-pp; - -nbrcameras=size(mot); - -if nbrcameras<3 - error('Too few cameras'); -end; - - -P=zeros(4,3*nbrcameras); -for i=1:nbrcameras, - P(:,3*i-2:3*i)=(Kpp*getcameras(mot,i))'; -end - -[u,s,v]=svd(P); - -T0=inv(u*s(:,1:4)); -P=T0*P; -M=zeros(4*nbrcameras,10); - -for i=1:nbrcameras; - - p=P(:,3*i-2:3*i)'; - p=p/norm(p); - - ii=(i-2)*4+1; - - M(4*i-3:4*i,:)=[... - p(1,1)*p(1,1)-p(2,1)*p(2,1),-2*p(2,1)*p(2,2)+2*p(1,1)*p(1,2),p(1,2)*p(1,2)-p(2,2)*p(2,2),-2*p(2,1)*p(2,3)+2*p(1,1)*p(1,3),2*p(1,2)*p(1,3)-2*p(2,2)*p(2,3),p(1,3)*p(1,3)-p(2,3)*p(2,3),-2*p(2,1)*p(2,4)+2*p(1,1)*p(1,4),2*p(1,2)*p(1,4)-2*p(2,2)*p(2,4),-2*p(2,3)*p(2,4)+2*p(1,3)*p(1,4),p(1,4)*p(1,4)-p(2,4)*p(2,4); - p(1,1)*p(2,1),p(2,1)*p(1,2)+p(2,2)*p(1,1),p(2,2)*p(1,2),p(2,1)*p(1,3)+p(2,3)*p(1,1),p(2,2)*p(1,3)+p(2,3)*p(1,2),p(2,3)*p(1,3),p(2,1)*p(1,4)+p(2,4)*p(1,1),p(2,2)*p(1,4)+p(2,4)*p(1,2),p(2,3)*p(1,4)+p(2,4)*p(1,3),p(2,4)*p(1,4); - p(1,1)*p(3,1),p(3,1)*p(1,2)+p(3,2)*p(1,1),p(3,2)*p(1,2),p(3,1)*p(1,3)+p(3,3)*p(1,1),p(3,2)*p(1,3)+p(3,3)*p(1,2),p(3,3)*p(1,3),p(3,1)*p(1,4)+p(3,4)*p(1,1),p(3,2)*p(1,4)+p(3,4)*p(1,2),p(3,3)*p(1,4)+p(3,4)*p(1,3),p(3,4)*p(1,4); - p(2,1)*p(3,1),p(3,1)*p(2,2)+p(3,2)*p(2,1),p(3,2)*p(2,2),p(3,1)*p(2,3)+p(3,3)*p(2,1),p(3,2)*p(2,3)+p(3,3)*p(2,2),p(3,3)*p(2,3),p(3,1)*p(2,4)+p(3,4)*p(2,1),p(3,2)*p(2,4)+p(3,4)*p(2,2),p(3,3)*p(2,4)+p(3,4)*p(2,3),p(3,4)*p(2,4)]; - -end - - - - - -[u,s,v]=svd(M); -Om=v2m(v(:,10)); -[u,s,v]=svd(Om); -%dd=diag(s); -%dd(4)=0; -%Om=u*diag(dd)*v'; - -T=inv(T0'*u*diag(sqrt([s(1,1),s(2,2),s(3,3),1]))); -T=T/norm(T); - - - - - - - - - - - - - - - - - - - - diff --git a/visionary/flexnonlinear.m b/visionary/flexnonlinear.m deleted file mode 100644 index e09af2c..0000000 --- a/visionary/flexnonlinear.m +++ /dev/null @@ -1,343 +0,0 @@ -function [T,Kout]=flexnonlinear(mot,pp,option); -% function [T,K]=flexnonlinear(mot,pp,option); -% Computes projective to Euclidean upgrade nonlinearly -% optimizing min(sum(norm(Ki*Ki')-norm(Pi*Om*Pi'))). -% Pollefey's method for initialization. -% Input: -% mot - projective motion -% pp - approximate principal point (optional) -% option: -% 'autocalib=XXXXX' - Autocalibration -% specify for each of focal length, aspect ratio, skew, -% principal point x, principal point y, -% if the parameter is -% 1 known and nominal, -% 2 unknown but constant in the sequence, -% 3 unknown and varying. -% 'nolinearinit' - No linear initialization (Tinit=eye(4)) -% Output: -% T - 4 x 4 coordinate transformation for Euclidean upgrade -% Use changecsystem(mot,T) for upgrade! -% Kout - cell list of calibration matrices -% Default: pp=[0,0]'; and constant intrinsic parameters - -if nargin<2, - pp=[0,0]'; -end; -linearinit=1; -caliboptions=2*ones(1,5); - -if nargin>=3, - if strmatch('nolinearinit',option); - linearinit=0; - end - - if strmatch('autocalib=',option), - q=strmatch('autocalib=',option); - strautocalib = option{q}(11:length(option{q})); - caliboptions=[str2num(strautocalib(1)) str2num(strautocalib(2)) ... - str2num(strautocalib(3)) str2num(strautocalib(4)) ... - str2num(strautocalib(5))]; - end -end - -if linearinit, - T0=flexlinear(mot,pp); - T0=T0/norm(T0); - iT0=inv(T0); -else - T0=eye(4); - iT0=eye(4); -end - -nbrcameras=size(mot); -Afull=iT0(1:3,1:3); -[A,R]=rq(Afull); -c=R*iT0(4,1:3)'/A(3,3); -A=A/A(3,3); - -%Om=[A*A', A*c;c'*A',c'*c] -%Om=inv(T0)*diag([1,1,1,0])*inv(T0)'; -%Om=Om/norm(Om); - -Kmean=zeros(3,3); -P=zeros(3,4*nbrcameras); -Kout=cell(1,nbrcameras); - -for i=1:nbrcameras; - Pi=getcameras(mot,i); - [K,R]=rq(Pi*iT0); - K=K/K(3,3);K([1,5])=abs(K([1,5])); - Kmean=Kmean+K; -end -Kmean=Kmean/nbrcameras; -keyboard; - -index=[1,4,5,7,8]; - -params=[A(index)';c(:)]; -Kvnom=[1,1,0,0,0]; -Kvmean=[Kmean(1,1),Kmean(2,2)/Kmean(1,1),Kmean(1,2)/Kmean(1,1),... - Kmean(1,3),Kmean(2,3)]; - -focalmean=Kvmean(1); - -for i=1:nbrcameras, - Pi=getcameras(mot,i); - [K,R]=rq(Pi*iT0); - K=K/K(3,3); - Kv=[K(1,1),K(2,2)/K(1,1),K(1,2)/K(1,1),K(1,3),K(2,3)]; - -% for j=1:5, -% if caliboptions(j)==2 & i==1, -% params=[params;Kvmean(j)]; -% elseif caliboptions(j)==3, -% params=[params;Kv(j)]; -% end -% end -% P(:,4*i-3:4*i)=Pi/norm(Pi); -%end - - - for j=1:5, - if caliboptions(j)==1, - Kv(j)=Kvnom(j); - elseif caliboptions(j)==2, - Kv(j)=Kvmean(j); - if i==1, - params=[params;Kv(j)/focalmean]; - end - elseif caliboptions(j)==3, - params=[params;Kv(j)/focalmean]; - end - end; - K0=[Kv(1),Kv(1)*Kv(3),Kv(4);0,Kv(1)*Kv(2),Kv(5);0,0,1]; - Pi=diag([1/focalmean,1/focalmean,1])*Pi; - P(:,4*i-3:4*i)=Pi/norm(Pi); -end - - -params=absconstopt(P,params,caliboptions); - -A=eye(3);A(index)=params(1:5); -c=params(6:8); -T=inv([A,zeros(3,1);c',1]); - - -indexp=9; -Kvmean=zeros(1,5); - -for i=1:nbrcameras, - Kv=[1,1,0,0,0]; - for j=1:5, - if caliboptions(j)==2, - if i==1, - Kvmean(j)=params(indexp); - indexp=indexp+1; - end; - Kv(j)=Kvmean(j); - elseif caliboptions(j)==3, - Kv(j)=params(indexp); - indexp=indexp+1; - end - end - Kout{i}=diag([focalmean,focalmean,1])*[Kv(1),Kv(1)*Kv(3),Kv(4);0,Kv(1)*Kv(2),Kv(5);0,0,1]; -end; - - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [A,f0] = absconstopt(P,A,caliboptions); - - -[f0, Idiff, Idiffgrad] = absconstres(P,A,caliboptions); -dA=inf; -counter=0; -while log(norm(dA)+eps)/log(10)>-15 & counter<1000; -% log(norm(dA)+eps)/log(10) - S = Idiffgrad'*Idiffgrad; - dA=-inv(S+0.0000001*eye(size(S,1)))*Idiffgrad'*Idiff; -% Anew=A+reshape(dA,size(A))'; - Anew=A+dA; - [f1, Idiff, Idiffgrad] = absconstres(P,Anew,caliboptions); - while f1>f0 & counter<1000, -% f0 -% f1 -% disp('Det har tar vi en gang till'); - dA=dA/2; -% Anew=A+reshape(dA,size(A))'; - Anew=A+dA; - [f1, Idiff, Idiffgrad] = absconstres(P,Anew,caliboptions); - counter=counter+1; - end; - - if f1<=f0, - f0=f1; - A = Anew; - end; - counter=counter+1; -end; - -function [f,Idiff,Idiffgrad]=absconstres(P,params,caliboptions); - -nbrcameras=size(P,2)/4; -Idiff=zeros(6*nbrcameras,1); -nbrvar=8+sum(caliboptions==2)+sum(caliboptions==3)*nbrcameras; -Idiffgrad=zeros(6*nbrcameras,nbrvar); - - -index=[1,4,5,7,8]; -index2=[index,9]; -A=eye(3);A(index)=params(1:5); -c=params(6:8); - - - - -%new -Om=[A*A',A*c;c'*A',c'*c]; - - -dOm1=zeros(4,4);dOm1(1,1)=2*A(1,1);dOm1(1,4)=c(1);dOm1(4,1)=c(1); -dOm2=zeros(4,4);dOm2(1,1)=2*A(1,2);dOm2(1,2)=A(2,2);dOm2(2,1)=A(2,2); -dOm2(1,4)=c(2);dOm2(4,1)=c(2); -dOm3=zeros(4,4);dOm3(1,2)=A(1,2);dOm3(2,1)=A(1,2);dOm3(2,2)=2*A(2,2); -dOm3(2,4)=c(2);dOm3(4,2)=c(2); -dOm4=zeros(4,4);dOm4(1,1)=2*A(1,3);dOm4(1,2)=A(2,3);dOm4(2,1)=A(2,3); -dOm4(1,3)=A(3,3);dOm4(3,1)=A(3,3);dOm4(1,4)=c(3);dOm4(4,1)=c(3); -dOm5=zeros(4,4);dOm5(1,2)=A(1,3);dOm5(2,1)=A(1,3);dOm5(2,2)=2*A(2,3); -dOm5(2,3)=A(3,3);dOm5(3,2)=A(3,3);dOm5(2,4)=c(3);dOm5(4,2)=c(3); -%dOm6=zeros(4,4);dOm6(1,3)=A(1,3);dOm6(3,1)=A(1,3);dOm6(2,3)=A(2,3); -%dOm6(3,2)=A(2,3);dOm6(3,3)=2*A(3,3);dOm6(3,4)=c(3);dOm6(4,3)=c(3); -dOm7=zeros(4,4);dOm7(1,4)=A(1,1);dOm7(4,1)=A(1,1);dOm7(4,4)=2*c(1); -dOm8=zeros(4,4);dOm8(1,4)=A(1,2);dOm8(4,1)=A(1,2);dOm8(2,4)=A(2,2);dOm8(4,2)=A(2,2);dOm8(4,4)=2*c(2); -dOm9=zeros(4,4);dOm9(1,4)=A(1,3);dOm9(4,1)=A(1,3);dOm9(2,4)=A(2,3);dOm9(4,2)=A(2,3);dOm9(3,4)=A(3,3);dOm9(4,3)=A(3,3);dOm9(4,4)=2*c(3); - -indexp=9; -Kvmean=zeros(1,5); -Kvmeanindex=zeros(1,5); - -for i=1:nbrcameras, - -%first term - - Kv=[1,1,0,0,0]; - Kvindex=zeros(1,5); - - for j=1:5, - if caliboptions(j)==2, - if i==1, - Kvmeanindex(j)=indexp; - Kvmean(j)=params(indexp); - indexp=indexp+1; - end - Kv(j)=Kvmean(j); - Kvindex(j)=Kvmeanindex(j); - elseif caliboptions(j)==3, - Kv(j)=params(indexp); - Kvindex(j)=indexp; - indexp=indexp+1; - end - end - K=[Kv(1),Kv(1)*Kv(3),Kv(4);0,Kv(1)*Kv(2),Kv(5);0,0,1]; - KK=K*K'; - N1=sum(KK(:).^2); - - for j=1:5, - if Kvindex(j)>0, - dKK=zeros(3,3); - switch j, - case 1 - dKK(1,1)=2*Kv(1)*(1+Kv(3)^2); - dKK(1,2)=2*Kv(1)*Kv(2)*Kv(3); - dKK(2,1)=2*Kv(1)*Kv(2)*Kv(3); - dKK(2,2)=2*Kv(1)*Kv(2)^2; - case 2 - dKK(1,2)=Kv(1)^2*Kv(3); - dKK(2,1)=Kv(1)^2*Kv(3); - dKK(2,2)=2*Kv(1)^2*Kv(2); - case 3 - dKK(1,1)=2*Kv(1)^2*Kv(3); - dKK(1,2)=Kv(1)^2*Kv(2); - dKK(2,1)=Kv(1)^2*Kv(2); - case 4 - dKK(1,1)=2*Kv(4); - dKK(1,2)=Kv(5); - dKK(2,1)=Kv(5); - dKK(1,3)=1; - dKK(3,1)=1; - case 5 - dKK(1,2)=Kv(4); - dKK(2,1)=Kv(4); - dKK(2,2)=2*Kv(5); - dKK(2,3)=1; - dKK(3,2)=1; - end - df=dKK/sqrt(N1)-KK/N1^(3/2)*sum(sum(KK.*dKK)); - Idiffgrad(6*i-5:6*i,Kvindex(j))=df(index2)'; - end - end - - - -%second term - p=P(:,4*i-3:4*i); - pop=p*Om*p'; - N2=sum(pop(:).^2); - - res=KK/sqrt(N1)-pop/sqrt(N2); - Idiff(6*i-5:6*i)=res(index2); - - pdop1=p*dOm1*p'; - dfO1=pdop1/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop1)); - Idiffgrad(6*i-5:6*i,1)=-dfO1(index2)'; - - pdop2=p*dOm2*p'; - dfO2=pdop2/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop2)); - Idiffgrad(6*i-5:6*i,2)=-dfO2(index2)'; - - pdop3=p*dOm3*p'; - dfO3=pdop3/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop3)); - Idiffgrad(6*i-5:6*i,3)=-dfO3(index2)'; - - pdop4=p*dOm4*p'; - dfO4=pdop4/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop4)); - Idiffgrad(6*i-5:6*i,4)=-dfO4(index2)'; - - pdop5=p*dOm5*p'; - dfO5=pdop5/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop5)); - Idiffgrad(6*i-5:6*i,5)=-dfO5(index2)'; - -% pdop6=p*dOm6*p'; -% dfO6=pdop6/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop6)); -% Idiffgrad(6*i-5:6*i,6)=-dfO6(index2)'; - - pdop7=p*dOm7*p'; - dfO7=pdop7/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop7)); - Idiffgrad(6*i-5:6*i,6)=-dfO7(index2)'; - - pdop8=p*dOm8*p'; - dfO8=pdop8/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop8)); - Idiffgrad(6*i-5:6*i,7)=-dfO8(index2)'; - - pdop9=p*dOm9*p'; - dfO9=pdop9/sqrt(N2)-pop/N2^(3/2)*sum(sum(pop.*pdop9)); - Idiffgrad(6*i-5:6*i,8)=-dfO9(index2)'; -end; - - -f=Idiff'*Idiff; - - -%if 0, -% -% slask=0.001; -% param0=params; -% Idiffgr0=Idiffgrad; -% Idiff0=Idiff; -% params=param0; -% params(9)=params(9)+slask; -% -% [Idiffgr0(:,1) (Idiff-Idiff0)/slask] -% -%end; diff --git a/visionary/focallength6pt.m b/visionary/focallength6pt.m deleted file mode 100644 index 3e0fe9d..0000000 --- a/visionary/focallength6pt.m +++ /dev/null @@ -1,86 +0,0 @@ -function mot = focallength6pt(imseq) -% mot = focallength(imseq) solves the 6pt-2view minimal case for cameras -% with a common unknown focallength. Has 15 solutions in general but only -% real solutions with positive depths for all points are returned. This -% means typically 1-4 solutions. -% -% Input: -% imseq - cellist of imagedata. 2 views with 6 points each. -% -% Output: -% mot - cellist of motion objects containing two cameras each. - -% get first 6 points in each image -ims_old = imseq; -x1 = getpoints(imseq{1}); -x1 = x1(:, 1:6); -imseq{1} = imagedata([], x1); -x2 = getpoints(imseq{2}); -x2 = x2(:, 1:6); -imseq{2} = imagedata([], x2); - -% normalize data for numerical performance -scale = max(abs([x1(:); x2(:)])); -H = diag([1/scale 1/scale 1]); -imseq{1} = changecsystem(imseq{1}, H); -imseq{2} = changecsystem(imseq{2}, H); - -% generate equations -[eqs, F_parts] = focallength6pt_create_eqs(imseq); - -% solve equations -% [sol, condition] = focallength6pt_svd_internal(eqs); -% [sol, condition] = classic_camera_varf(eqs); -[sol, condition] = focallength6pt_internal(eqs); - -% reformat output -for k = 1 : size(sol, 2) - F{k} = F_parts{1} + sol(1,k)*F_parts{2} + sol(2,k)*F_parts{3}; - f(k) = 1 / sqrt(sol(3,k)); -end - -% pick only real solutions -F = F(f==real(f)); -f = f(f==real(f)); - -% construct cameras -w = [0 -1 0 - 1 0 0 - 0 0 1]; -kk = 0; -for k = 1 : length(f) - if(~isreal(F{k})) - continue; - end - kk = kk + 1; - % factorize essential matrix to get cameras - K = diag([f(k) f(k) 1]); - E = K'*F{k}'*K; % we transpose F to comply with the H-Z definition of F. - [u s v] = svd(E); - t = u(:, 3); - PP{1} = K*[u*w*v' t]; - PP{2} = K*[u*w*v' -t]; - PP{3} = K*[u*w'*v' t]; - PP{4} = K*[u*w'*v' -t]; - P = K*eye(3, 4); - - % triangulate a point to resolve factorization ambiguity. - best = -inf; - for i = 1 : 4 - X = getpoints(intsec2views(motion({P, PP{i}}), imseq)); - x = PP{i}*X; - X3 = X(3,:); - X3 = X3 / norm(X3); - x3 = x(3,:); - x3 = x3 / norm(x3); - score = sum(x3) + sum(X3); - if(score > best) - best = score; - bestcam = i; - end - end - - P2 = H\PP{bestcam}; - P1 = H\P; - mot{kk} = motion({P1, P2}); -end \ No newline at end of file diff --git a/visionary/focallength6pt_create_eqs.m b/visionary/focallength6pt_create_eqs.m deleted file mode 100644 index 21fbd17..0000000 --- a/visionary/focallength6pt_create_eqs.m +++ /dev/null @@ -1,80 +0,0 @@ -function [poly_eqs, F_part] = focallength6pt_create_eqs(imseq) -% this is an internal function used by focallength6pt.m - -x1 = getpoints(imseq{1}); -x2 = getpoints(imseq{2}); - -% -% Construct the Fundamental matrix -% - -% Setup all constraints on the fundamental matrix -M = [x1(1,:).*x2(1,:); - x1(1,:).*x2(2,:); - x1(1,:).*x2(3,:); - x1(2,:).*x2(1,:); - x1(2,:).*x2(2,:); - x1(2,:).*x2(3,:); - x1(3,:).*x2(1,:); - x1(3,:).*x2(2,:); - x1(3,:).*x2(3,:); - ]'; - -% Find the nullspace of M -[U,S,V] = svd(M,0); -FF = V(:,7:9); - -% The three parts of the fundamental matrix -F_part{1} = reshape(FF(:,1),3,3)'; -F_part{2} = reshape(FF(:,2),3,3)'; -F_part{3} = reshape(FF(:,3),3,3)'; - -% -% Construct the equations -% - -% Setup the monomials -l1 = multipol(1,[1 0 0]'); -l2 = multipol(1,[0 1 0]'); -p = multipol(1,[0 0 1]'); - -% Create the fundamental matrix -F = F_part{1}+l1*F_part{2}+l2*F_part{3}; - -% Create all equations according to figure 3 of stewes paper; -tmp = [F(1,1) F(1,2) F(1,3)*p;... - F(2,1) F(2,2) F(2,3)*p;... - F(3,1) F(3,2) F(3,3)*p] *... - [F(1,1) F(2,1) F(3,1)*p;... - F(1,2) F(2,2) F(3,2)*p;... - F(1,3) F(2,3) F(3,3)*p]; - -eqs_tmp = 2*tmp*F-trace(tmp)*F; - -eqs = {}; -for ii = 1:9 - eqs{ii} = eqs_tmp(ii); -end - -% det(F) = 0 -eqs{10} = det(F); -eqs{11} = eqs{10}*p; -eqs{12} = eqs{11}*p; - -% Multiply all equations with p and p^2 to get enough equations -eqs_p = {}; -eqs_p2 = {}; -for ii = 1:length(eqs); - eqs_p{ii} = eqs{ii}*p; - eqs_p2{ii} = eqs_p{ii}*p; -end -eqs = [eqs eqs_p eqs_p2]; - -% Build the coefficent matrix -[poly_eqs mons] = polynomials2matrix(eqs); - -% Change the order of the monomials to simplify things -poly_eqs(:,35:37) = poly_eqs(:,[37 35 36]); - -% Remove the columns that will not be correctly eliminated -poly_eqs(:,[4 20 30]) = []; diff --git a/visionary/focallength6pt_internal.m b/visionary/focallength6pt_internal.m deleted file mode 100644 index 8fba2a2..0000000 --- a/visionary/focallength6pt_internal.m +++ /dev/null @@ -1,34 +0,0 @@ -function [sol, condition] = focallength6pt_internal(poly_eqs) -% for internal use with focallength6pt - -% Remove redundant equations -[u s v] = svd(poly_eqs); -poly_eqs = v'; -poly_eqs = poly_eqs(1:32,:); - -% Eliminate -poly_eqs_elim = poly_eqs(:,1:32)\poly_eqs; - -condition = cond(poly_eqs(:,1:32)); -% Create action matrix -% Begin with the modulo matrix -mod_matrix = zeros(15,47); -mod_matrix(:,33:47) = eye(15); - -% Complete the modulo matrix -mod_matrix(:,1:32) = -poly_eqs_elim(1:32,33:47)'; - -% Now construct the action matrix for l1 -% These are the indices that are reached when the basis monomials are -% multiplied with l1. -ind_l1 = [20:24 28:31 33 34 38:40 44]; -% Create the big action matrix -Ahat_l1 = zeros(47,15); -Ahat_l1(ind_l1,:) = eye(15); -% Reduce to the quotient space -A_l1 = mod_matrix*Ahat_l1; -% Calculate the eigen vectors of the transposed action matrix -[v1, d1] = eig(A_l1'); -% Extract the solution for l1, l2 and p -sol = pflat(v1); -sol = sol(12:14, :); \ No newline at end of file diff --git a/visionary/getcommonfeatures.m b/visionary/getcommonfeatures.m deleted file mode 100644 index 3100a61..0000000 --- a/visionary/getcommonfeatures.m +++ /dev/null @@ -1,38 +0,0 @@ -function [p,l,c]=getcommonfeatures(imseq); -% [p,l,c]=getcommonfeatures(imseq) returns indexes of point, line -% and conic features present in whole sequence - -p0=ones(1,size(imseq{1},1)); -l0=ones(1,size(imseq{1},2)); -c0=ones(1,size(imseq{1},3)); - -for i=1:length(imseq); - - pi=getpoints(imseq{i}); - if size(pi,2)>0 - pi=~isnan(pi(1,:)); - p0 = p0 & pi; - else - p0=0; - end - - li=getlines(imseq{i}); - if size(li,2)>0 - li=~isnan(li(1,:)); - l0 = l0 & li; - else - l0=0; - end - - ci=getconics(imseq{i}); - if size(ci,2)>0 - ci=~isnan(ci(1,:)); - c0 = c0 & ci; - else - c0=0; - end -end - -p=find(p0); -l=find(l0); -c=find(c0); diff --git a/visionary/getpluckercameras.m b/visionary/getpluckercameras.m deleted file mode 100644 index 440b167..0000000 --- a/visionary/getpluckercameras.m +++ /dev/null @@ -1,22 +0,0 @@ -function p=getpluckercameras(P) -%MOTION/GETPLUCKERCAMERAS getpluckercameras(P) returns -%cameras (with index) in plucker coordinates. - -% up row -ur = [2 3; 3 1; 1 2]; -% down col -dc = [1 2; 1 3; 1 4; 3 4; 4 2; 2 3]; - -for i=1:size(P,2) - p{i} = zeros(3,6); - tt=P{i}; - for row = 1:3 - for col = 1:6 - p{i}(row,col) = det([tt(ur(row,1),dc(col,1)) ... - tt(ur(row,1),dc(col,2)); - tt(ur(row,2),dc(col,1)) ... - tt(ur(row,2),dc(col,2))]); - end - end -end - diff --git a/visionary/getrotation.m b/visionary/getrotation.m deleted file mode 100644 index bf85327..0000000 --- a/visionary/getrotation.m +++ /dev/null @@ -1,15 +0,0 @@ - -function R=getrotation(alfa,beta,gamma) - - - -R = [cos(alfa) -sin(alfa) 0;sin(alfa) cos(alfa) 0;0 0 1]; -R = R*[cos(beta) 0 -sin(beta);0 1 0;sin(beta) 0 cos(beta)]; -R = R*[1 0 0;0 cos(gamma) sin(gamma);0 -sin(gamma) cos(gamma)]; - - - - - - - diff --git a/visionary/ginput2.m b/visionary/ginput2.m deleted file mode 100644 index ea3607c..0000000 --- a/visionary/ginput2.m +++ /dev/null @@ -1,208 +0,0 @@ -function [out1,out2,out3] = ginput(arg1) -%GINPUT Graphical input from mouse. -% [X,Y] = GINPUT(N) gets N points from the current axes and returns -% the X- and Y-coordinates in length N vectors X and Y. The cursor -% can be positioned using a mouse (or by using the Arrow Keys on some -% systems). Data points are entered by pressing a mouse button -% or any key on the keyboard except carriage return, which terminates -% the input before N points are entered. -% -% [X,Y] = GINPUT gathers an unlimited number of points until the -% return key is pressed. -% -% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that -% contains a vector of integers specifying which mouse button was -% used (1,2,3 from left) or ASCII numbers if a key on the keyboard -% was used. - -% Copyright (c) 1984-96 by The MathWorks, Inc. -% $Revision: 1.3 $ $Date: 2005/07/13 11:49:03 $ - -out1 = []; out2 = []; out3 = []; y = []; -c = computer; -if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') - tp = get(0,'TerminalProtocol'); -else - tp = 'micro'; -end - -if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), - if nargout == 1, - if nargin == 1, - eval('out1 = trmginput(arg1);'); - else - eval('out1 = trmginput;'); - end - elseif nargout == 2 | nargout == 0, - if nargin == 1, - eval('[out1,out2] = trmginput(arg1);'); - else - eval('[out1,out2] = trmginput;'); - end - if nargout == 0 - out1 = [ out1 out2 ]; - end - elseif nargout == 3, - if nargin == 1, - eval('[out1,out2,out3] = trmginput(arg1);'); - else - eval('[out1,out2,out3] = trmginput;'); - end - end -else - - fig = gcf; - figure(gcf); - - if nargin == 0 - how_many = -1; - b = []; - else - how_many = arg1; - b = []; - if isstr(how_many) ... - | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... - | ~(fix(how_many) == how_many) ... - | how_many < 0 - error('Requires a positive integer.') - end - if how_many == 0 - ptr_fig = 0; - while(ptr_fig ~= fig) - ptr_fig = get(0,'PointerWindow'); - end - scrn_pt = get(0,'PointerLocation'); - loc = get(fig,'Position'); - pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; - out1 = pt(1); y = pt(2); - elseif how_many < 0 - error('Argument must be a positive integer.') - end - end - - pointer = get(gcf,'pointer'); - set(gcf,'pointer','crosshair'); - fig_units = get(fig,'units'); - char = 0; - figure(gcf); - - while how_many ~= 0 - % Use no-side effect WAITFORBUTTONPRESS - waserr = 0; - eval('keydown = wfbp;', 'waserr = 1;'); - if(waserr == 1) - if(ishandle(fig)) - set(fig,'pointer',pointer,'units',fig_units); - error('Interrupted'); - else - error('Interrupted by figure deletion'); - end - end - - ptr_fig = get(0,'CurrentFigure'); - if(ptr_fig == fig) - if keydown - char = get(fig, 'CurrentCharacter'); - button = abs(get(fig, 'CurrentCharacter')); - scrn_pt = get(0, 'PointerLocation'); - set(fig,'units','pixels') - loc = get(fig, 'Position'); - pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; - set(fig,'CurrentPoint',pt); - else - button = get(fig, 'SelectionType'); - if strcmp(button,'open') - button = b(length(b)); - elseif strcmp(button,'normal') - button = 1; - elseif strcmp(button,'extend') - button = 2; - elseif strcmp(button,'alt') - button = 3; - else - error('Invalid mouse selection.') - end - end - pt = get(gca, 'CurrentPoint'); -%fredrik -plot(pt(1,1),pt(1,2),'rx'); -drawnow; -figure(gcf); - how_many = how_many - 1; - - if(char == 13) % & how_many ~= 0) - % if the return key was pressed, char will == 13, - % and that's our signal to break out of here whether - % or not we have collected all the requested data - % points. - % If this was an early breakout, don't include - % the key info in the return arrays. - % We will no longer count it if it's the last input. - break; - end - - out1 = [out1;pt(1,1)]; - y = [y;pt(1,2)]; - b = [b;button]; - end - end - - set(fig,'pointer',pointer,'units',fig_units); - - if nargout > 1 - out2 = y; - if nargout > 2 - out3 = b; - end - else - out1 = [out1 y]; - end - -end - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function key = wfbp -%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. - -% Remove figure button functions -fprops = {'windowbuttonupfcn','buttondownfcn', ... - 'windowbuttondownfcn','windowbuttonmotionfcn'}; -fig = gcf; -fvals = get(fig,fprops); -set(fig,fprops,{'','','',''}) - -% Remove all other buttondown functions -ax = findobj(fig,'type','axes'); -if isempty(ax) - ch = {}; -else - ch = get(ax,{'Children'}); -end -for i=1:length(ch), - ch{i} = ch{i}(:)'; -end -h = [ax(:)',ch{:}]; -vals = get(h,{'buttondownfcn'}); -mt = repmat({''},size(vals)); -set(h,{'buttondownfcn'},mt); - -% Now wait for that buttonpress, and check for error conditions -waserr = 0; -eval(['if nargout==0,', ... - ' waitforbuttonpress,', ... - 'else,', ... - ' keydown = waitforbuttonpress;',... - 'end' ], 'waserr = 1;'); - -% Put everything back -if(ishandle(fig)) - set(fig,fprops,fvals) - set(h,{'buttondownfcn'},vals) -end - -if(waserr == 1) - error('Interrupted'); -end - -if nargout>0, key = keydown; end -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/visionary/harris.m b/visionary/harris.m deleted file mode 100644 index 532b508..0000000 --- a/visionary/harris.m +++ /dev/null @@ -1,111 +0,0 @@ -function [i,crf]=harris(im,option); -% [i,crf]=HARRIS(im,option) - creates an IMAGEDATA object with corners using HARRIS -% im: image matrix -% option: (cell of strings) -% - 'threshold=X', sets corner response threshold to X -% - 'nbrcorners=X', gets the X strongest corners in the image -% - 'noimage', image is not included in IMAGEDATA object i -% - 'mindistance=X' - minimum pixel distance between corners -% Output: -% i - imagedata object with corner points -% crf - corner response function -% Default is 'threshold=1', 'mindistance=20' -% See also: harrispro.m - -cornerthreshold=1; -nbrcorners=0; -includeimage=1; - -mindistance=20; - -if nargin>=2, - if strmatch('threshold=',option); - q=strmatch('threshold=',option); - strthreshold = option{q}(11:length(option{q})); - cornerthreshold=str2num(strthreshold); - end - if strmatch('nbrcorners=',option); - q=strmatch('nbrcorners=',option); - strnbrcorners = option{q}(12:length(option{q})); - nbrcorners=str2num(strnbrcorners); - end - if strmatch('mindistance=',option); - q=strmatch('mindistance=',option); - mindistance=str2num(option{q}(13:length(option{q}))); - end - if strmatch('noimage',option); - includeimage=0; - end -end %option - -a1=1.5; -filtsize1=round(2*a1); -[x,y]=meshgrid(-filtsize1:filtsize1,-filtsize1:filtsize1); -filtx=-2*x.*exp(- (x.^2 + y.^2)/a1^2)/(a1^4*pi); -filty=-2*y.*exp(- (x.^2 + y.^2)/a1^2)/(a1^4*pi); -Lx=conv2(im,filtx,'valid'); -Ly=conv2(im,filty,'valid'); - -a2=1.5; -filtsize2=round(2*a2); -[x,y]=meshgrid(-filtsize2:filtsize2,-filtsize2:filtsize2); -filt=exp(- (x.^2 + y.^2)/a2^2)/(a2^2*pi); - -Wxx=conv2(Lx.*Lx,filt,'valid'); -Wxy=conv2(Lx.*Ly,filt,'valid'); -Wyy=conv2(Ly.*Ly,filt,'valid'); - -%Wdet=Wxx.*Wyy-Wxy.^2; -%Wtr=Wxx+Wyy; - -ignoreborder=10; -%utbild=abs(Wxx.*Wyy-Wxy.^2); -utbild=abs((Wxx.*Wyy-Wxy.^2)./(Wxx+Wyy+eps)); -utbild=utbild(1+ignoreborder:end-ignoreborder,1+ignoreborder:end-ignoreborder); - - -% Lite extra test -%[m,n]=size(utbild); -%mask=zeros(m,n); -%cutsize=((filtsize1+filtsize2)*2+1); -%mask(cutsize:m-cutsize,cutsize:n-cutsize)=ones(m-2*cutsize+1,n-2*cutsize+1); -%utbild = mask.*utbild; - -cont=1; -topplista=[]; -crf=[]; -[m,n]=size(utbild); - -shifted=ignoreborder+filtsize1+filtsize2; -while cont, - [xmax,ypos]=max(utbild); - [ymax,xpos]=max(xmax); - ymax=ymax/9; %normalize - ypos=ypos(xpos); - - if nbrcorners==0, - if ymax>cornerthreshold, - topplista=[topplista,(shifted+[xpos;ypos])]; - crf=[crf,ymax]; - else - cont=0; - end - else %pick a fixed number of corners - topplista=[topplista,(shifted+[xpos;ypos])]; - crf=[crf,ymax]; - nbrcorners=nbrcorners-1; - if nbrcorners==0, - cont=0; - end - end - ind1=max(ypos-mindistance,1):min(ypos+mindistance,m); - ind2=max(xpos-mindistance,1):min(xpos+mindistance,n); - - utbild(ind1,ind2)=zeros(length(ind1),length(ind2)); -end; - -if includeimage, - i=imagedata(im,topplista); -else - i=imagedata([],topplista); -end diff --git a/visionary/harrispro.m b/visionary/harrispro.m deleted file mode 100644 index df1904f..0000000 --- a/visionary/harrispro.m +++ /dev/null @@ -1,64 +0,0 @@ -function [i,crf]=harrispro(im,option); -% i=HARRISPRO(im,option) - creates an IMAGEDATA object with corners -% using HARRIS, plus auto-correlation in neighbourhood -% im: image matrix -% option: (cell of strings) -% - 'threshold=X', sets corner response threshold to X -% - 'nbrcorners=X', gets the X strongest corners in the image -% - 'noimage', image is not included in IMAGEDATA object i -% Output: -% i - IMAGEDATA object -% crf - corner response function (vector) -% Default is 'threshold=1' -% See also: harris.m - -ignore=3; -autothreshold=0.2; -patchsize=5; -searchsize=50; - -if nargin<2, - option=[]; -end - -i1=harris(im,option); - -n=2*patchsize+1; -imconv=conv2(im.^2,ones(n),'valid')-conv2(im,ones(n),'valid').^2/n^2; -imconv(find(imconv==0))=1; - -i=imagedata; -crf=[]; - -for jj=1:size(i1,1); - - pos=round(getpoints(i1,jj)); - patch=im(pos(2)+(-patchsize:patchsize),pos(1)+(-patchsize:patchsize)); - patch = flipud(fliplr(patch)); - patch=patch-mean(patch(:)); - patch2=sum(patch(:).^2);if patch2==0,patch2=1;end; - - index1=max(1,pos(2)-searchsize):min(size(im,1),pos(2)+searchsize); - index2=max(1,pos(1)-searchsize):min(size(im,2),pos(1)+searchsize); - searchpatch=im(index1,index2); - - - searchsquared=imconv(index1(1+patchsize:end-patchsize)-patchsize,... - index2(1+patchsize:end-patchsize)-patchsize); - - - s_patch=conv2(searchpatch,patch,'valid'); - - res=(searchsquared+patch2-2*s_patch)./sqrt(searchsquared)/sqrt(patch2); - - res(pos(2)-index1(1)+1-patchsize+(-ignore:ignore),... - pos(1)-index2(1)+1-patchsize+(-ignore:ignore))=Inf; - - residual=min(res(:)); - - if residual>autothreshold, - i=addpoints(i,pos); - crf=[crf;residual]; - end -end - diff --git a/visionary/intsec2views.m b/visionary/intsec2views.m deleted file mode 100644 index 8d3e3be..0000000 --- a/visionary/intsec2views.m +++ /dev/null @@ -1,108 +0,0 @@ -function X=intsec2views(P1,P2,x1,x2); - -nbr=size(x1,2); -X=zeros(4,nbr); - -%fundamental matrix -F=ptobi(P1,P2); -F=F/norm(F); -e1=pflat(cross(F(:,1),F(:,2))); -e2=pflat(cross(F(1,:)',F(2,:)')); - -T1=eye(3);T1(:,3)=e1; -T2=eye(3);T2(:,3)=e2; - -Ftmp=T1'*F*T2; -[u,s,v]=svd(Ftmp(1:2,1:2)); -T1(1:2,1:2)=T1(1:2,1:2)*u'; -T2(1:2,1:2)=T2(1:2,1:2)*v; -Ftmp=T1'*F*T2; -lambda=Ftmp(1,1)/Ftmp(2,2); - -x1=inv(T1)*x1; -x2=inv(T2)*x2; - -P1=inv(T1)*P1; -P2=inv(T2)*P2; - -M=[P1,zeros(3,2);P2,zeros(3,2)]; -M(3,5)=1;M(6,6)=1; - -for ii=1:nbr - mx1=x1(1,ii); - my1=x1(2,ii); - mx2=x2(1,ii); - my2=x2(2,ii); - -%syms s;dist=(s*mx1+my1)^2/(s^2+1)+(lambda*mx2-s*my2)^2/(s^2+lambda^2) -%deriv=diff(dist,s); -%n=maple('numer',diff(dist,s)) -%maple('coeff',n,'s',6)/2 -%syms s mx1 mx2 my1 my2 lambda - - poly=[mx1*my1-mx2*lambda*my2,... - -lambda^2*my2^2+mx2^2*lambda^2-mx1^2+my1^2,... - -mx1*my1+2*mx1*my1*lambda^2+mx2*lambda^3*my2-2*mx2*lambda*my2,... - -2*lambda^2*my2^2-2*mx1^2*lambda^2+2*mx2^2*lambda^2+2*my1^2*lambda^2,... - mx1*my1*lambda^4-2*mx1*my1*lambda^2+2*mx2*lambda^3*my2-mx2*lambda*my2,... - -mx1^2*lambda^4-lambda^2*my2^2+mx2^2*lambda^2+my1^2*lambda^4,... - -mx1*my1*lambda^4+mx2*lambda^3*my2]; - - r=roots(poly); - r=r(find(imag(r)==0)); - - [dist,ind]=min((r*mx1+my1).^2./(r.^2+1)+(lambda*mx2-r*my2).^2./(r.^2+lambda^2)); - - %noise free points - s=r(ind); - - n=[s;1]/sqrt(s^2+1); - tmp=[mx1;my1]-[-1;s]; - nx1=[mx1;my1]-tmp'*n*n; - - n=[lambda;-s]/sqrt(s^2+lambda^2); - tmp=[mx2;my2]-[s;lambda]; - nx2=[mx2;my2]-tmp'*n*n; - - M(1:2,5)=nx1; - M(4:5,6)=nx2; - [u,s,v]=svd(M);X(:,ii)=pflat(v(1:4,end)); - %tmp=null(M);X(:,ii)=pflat(tmp(1:4)); -end - - - - - - - -function B=ptobi(P1,P2) -% B=ptobi(m,index) -% Calculates the bilinear tensor from the two camera matrices in MOTION m - -for i=1:3 - for j=1:3 - B(i,j)=det([P1(1+mod(i,3),:) ; P1(1+mod(i+1,3),:) ; P2(1+mod(j,3),:) ; P2(1+mod(j+1,3),:)]); - end -end - - - - - - - - - -function y = pflat(x) -y = x./repmat(x(end,:),[size(x,1) 1]); - -function y = pextend(x) -y = [x; ones(1,size(x,2))]; - - - - - - - diff --git a/visionary/intsec2views_midpoint.m b/visionary/intsec2views_midpoint.m deleted file mode 100644 index 1837f46..0000000 --- a/visionary/intsec2views_midpoint.m +++ /dev/null @@ -1,52 +0,0 @@ -% U = intsec2views_midpoint(P1,P2,u1,u2) -% -% Triangulates one or more points seen in two cameras using the midpoint -% method. -% -% INPUTS: P1 Camera matrix for the first camera -% P2 Camera matrix for the second camera -% u1 [3xN] Matrix containing the -% homogenous coordinates of the image -% points in the first view -% u2 [3xN] Matrix containing the -% homogenous coordinates of the image -% points in the second view -% OUTPUTS: U [4xN] Matrix containing the -% homogenous coordinates of the -% triangulated 3D points. -% -function U = intsec2views_midpoint(P1,P2,u1,u2) - % Calculate the corresponding lines - v1 = inv(P1(:,1:3))*u1; - v1 = v1./repmat(sqrt(sum(v1.^2)),[3,1]); - c1 = -inv(P1(:,1:3))*P1(:,4); - - v2 = inv(P2(:,1:3))*u2; - v2 = v2./repmat(sqrt(sum(v2.^2)),[3,1]); - c2 = -inv(P2(:,1:3))*P2(:,4); - - % Find the normal for the lines v1 cross v2 - n = [v1(2,:).*v2(3,:)-v1(3,:).*v2(2,:); ... - v1(3,:).*v2(1,:)-v1(1,:).*v2(3,:); ... - v1(1,:).*v2(2,:)-v1(2,:).*v2(1,:)]; - n = n./repmat(sqrt(sum(n.^2)),[3 1]); - - % Shortest vector between the lines - d = repmat((c1 - c2)'*n,[3 1]).*n; - - % Point on l2 closest to l1 - - npi = [v1(2,:).*n(3,:)-v1(3,:).*n(2,:); ... - v1(3,:).*n(1,:)-v1(1,:).*n(3,:); ... - v1(1,:).*n(2,:)-v1(2,:).*n(1,:)]; - - dpi = -c1'*npi; - - % Intersection with l2 - lambda = (-dpi-c2'*npi)./(sum(npi.*v2)); - p2 = repmat(c2,[1 size(v2,2)])+repmat(lambda,[3 1]).*v2; - - % Midpoints - U = [p2+d/2; ones(1,size(p2,2))]; -end - diff --git a/visionary/intsecconics.m b/visionary/intsecconics.m deleted file mode 100644 index 5cb2d57..0000000 --- a/visionary/intsecconics.m +++ /dev/null @@ -1,165 +0,0 @@ -function s=intsecconics(m,imseq,quadrics,option); -% INTSECCONICS s=intsecconics(m,imseq,quadrics,option) intersects quadrics from conics -% with linear method -% INPUT: -% m - motion object -% imseq - cell array of imagedata objects -% quadrics - (optional) specifies quadrics to be intersected. Otherwise all quadrics. -% If 'quadrics' is a structure object, already existing quadrics are copied. -% option: -% 'nocoordtrans' - No coordinate transformation -% 'conic' - Quadrics are diskquadrics, i.e. conics in a plane -% -% OUTPUT: -% s - structure object containing reconstructed 3D quadrics -% The results may be inaccurate. - -if nargin<=2 | isempty(quadrics) - quadrics = 1:size(imseq{1},3); -end - -if nargin<=3 - option = []; -end -if isa(quadrics,'structure'); - nbrquadrics = size(quadrics,3); - if nbrquadrics>0, - U = getquadrics(quadrics); - else - nbrquadrics=size(imseq{1},3); - U = NaN*ones(10,nbrquadrics); - end -else - nbrquadrics=length(quadrics); - U = NaN*ones(10,nbrquadrics); -end - -nbrimages=length(imseq); - -for qq=1:nbrquadrics; - - if ~finite(U(1,qq)) %already existing? - jj=0; - M=[]; - for i=1:nbrimages; - P=getcameras(m,i); - c=getconics(imseq{i},qq); - if finite(P(1)) & finite(c(1)), - if strmatch('nocoordtrans',option), - T=eye(3); - else - T=getnormtrans(imseq{i}); - end - c=m2v(T*v2m(c)*T'); - conicP=coniccamera(T*P); - z0=zeros(jj*6,1); - z1=zeros(6,jj); - M=[M,z0;conicP/norm(conicP,'fro'),z1,psphere(c)]; - jj=jj+1; - end - end - - if jj>2, - [u,ss,v]=svd(M); - if strmatch('conic',option); - -% Old method. Find 'la' such that det(v1+la*v2)=0. -% v1 = v(1:10,jj+10); -% v2 = v(1:10,jj+9); -% la=calclambdaconic(v1,v2); -% L = v1 + la * v2; - - v1 = v(1:10,jj+10); - [u,ss,v]=svd(v2m(v1)); - ss(4,4)=0; - U(:,qq) = m2v(u*ss*v'); - else - U(:,qq) = v(1:10,jj+10); - end - end - end %already existing -end - -s=structure([],[],U); - - -%end of main function - -function la=calclambdaconic(L,K); -%L "best" quadric, K "second best" -% find best conic - -K1=K(1);K2=K(2);K3=K(3);K4=K(4);K5=K(5); -K6=K(6);K7=K(7);K8=K(8);K9=K(9);K10=K(10); -L1=L(1);L2=L(2);L3=L(3);L4=L(4);L5=L(5); -L6=L(6);L7=L(7);L8=L(8);L9=L(9);L10=L(10); - -%calc. polynom - - p4 = K7*K7*K5*K5-K1*K3*K9*K9-K4*K4*K3*K10-2.0*K4*K8*K7*K5-K7*K7*K3*K6+K1* ... -K3*K6*K10+2.0*K2*K8*K7*K6-2.0*K2*K8*K4*K9-K1*K5*K5*K10+K4*K4*K8*K8+K2*K2*K9*K9- ... -K2*K2*K6*K10+2.0*K1*K5*K8*K9+2.0*K2*K5*K4*K10+2.0*K4*K3*K7*K9-K1*K8*K8*K6-2.0* ... -K2*K5*K7*K9; - - tmp = 2.0*L2*K8*K7*K6-K1*L3*K9*K9+2.0*K4*K3*L7*K9+2.0*K4*L3*K7*K9-2.0*K4* ... -K8*K7*L5-2.0*K4*K8*L7*K5-2.0*K4*K3*L4*K10+2.0*K4*K3*K7*L9-K4*K4*K3*L10-2.0*L2* ... -K8*K4*K9-2.0*L2*K5*K7*K9+2.0*L2*K5*K4*K10-2.0*K2*L8*K4*K9+2.0*K2*L8*K7*K6+2.0* ... -K2*K8*K7*L6+2.0*K2*K8*L7*K6-2.0*K2*K8*K4*L9-2.0*K2*L5*K7*K9-2.0*K2*K8*L4*K9+2.0 ... -*K2*K5*K4*L10+2.0*K2*K5*L4*K10-2.0*K2*L2*K6*K10-2.0*K2*K5*K7*L9-2.0*K2*K5*L7*K9 ... -+2.0*K2*L5*K4*K10+2.0*L1*K5*K8*K9-L1*K5*K5*K10-L1*K8*K8*K6; - p3 = tmp+2.0*K2*L2*K9*K9-K2*K2*K6*L10-K2*K2*L6*K10+2.0*K1*L5*K8*K9+2.0*K1* ... -K5*L8*K9+L1*K3*K6*K10+2.0*K2*K2*K9*L9+2.0*K4*K8*K8*L4-2.0*K1*K3*K9*L9+K1*L3*K6* ... -K10-2.0*K1*K5*L5*K10+2.0*K1*K5*K8*L9-2.0*K1*K8*L8*K6-K1*K5*K5*L10-K1*K8*K8*L6- ... -L1*K3*K9*K9+K1*K3*K6*L10+K1*K3*L6*K10-2.0*K7*K3*L7*K6-2.0*L4*K8*K7*K5-2.0*K4*L8 ... -*K7*K5+2.0*L4*K3*K7*K9-K7*K7*L3*K6+2.0*K7*K7*K5*L5-K7*K7*K3*L6-K4*K4*L3*K10+2.0 ... -*K4*K4*K8*L8+2.0*K7*K5*K5*L7; - - tmp = K4*K4*L8*L8+K2*K2*L9*L9+L2*L2*K9*K9+K8*K8*L4*L4-K1*K3*L9*L9-2.0*K4* ... -L3*L4*K10+2.0*K4*L3*K7*L9+2.0*K4*L3*L7*K9+4.0*K4*K8*L4*L8-2.0*K4*K8*L7*L5-2.0* ... -K4*K3*L4*L10+2.0*L2*K8*K7*L6+2.0*L2*K8*L7*K6-2.0*L2*L8*K4*K9+2.0*L2*L8*K7*K6 ... --2.0*L2*K8*K4*L9-2.0*L2*L5*K7*K9-2.0*L2*K8*L4*K9+2.0*L2*L5*K4*K10+2.0*L2*K5*L4* ... -K10-2.0*L2*K5*K7*L9-2.0*L2*K5*L7*K9+2.0*K2*L8*K7*L6+2.0*K2*L8*L7*K6+2.0*L2*K5* ... -K4*L10+2.0*K2*K8*L7*L6-2.0*K2*L8*K4*L9-2.0*K2*L8*L4*K9-2.0*K2*K8*L4*L9-2.0*K2* ... -L5*L7*K9+2.0*K2*L5*L4*K10+K7*K7*L5*L5+2.0*K2*L5*K4*L10-2.0*K2*L5*K7*L9-2.0*K2* ... -L2*K6*L10-2.0*K2*L2*L6*K10+4.0*K2*L2*K9*L9+2.0*K2*K5*L4*L10-2.0*K2*K5*L7*L9+2.0 ... -*L1*L5*K8*K9; - p2 = tmp-2.0*L1*K8*L8*K6-2.0*L1*K5*L5*K10+2.0*L1*K5*K8*L9+L1*K3*K6*L10+L1* ... -K3*L6*K10-2.0*L1*K3*K9*L9+L1*L3*K6*K10+K5*K5*L7*L7-L1*L3*K9*K9-L1*K5*K5*L10-L1* ... -K8*K8*L6+2.0*K1*K5*L8*L9+2.0*K1*L5*K8*L9+2.0*K1*L5*L8*K9-2.0*K1*K8*L8*L6+2.0*L1 ... -*K5*L8*K9-K2*K2*L6*L10-L2*L2*K6*K10+K1*L3*K6*L10+K1*L3*L6*K10-2.0*K1*L3*K9*L9 ... --2.0*K1*K5*L5*L10-K1*L5*L5*K10-K1*L8*L8*K6+K1*K3*L6*L10-2.0*K7*K3*L7*L6-2.0*K7* ... -L3*L7*K6+4.0*K7*K5*L7*L5-2.0*L4*L8*K7*K5-2.0*L4*K8*K7*L5+2.0*L4*K3*K7*L9+2.0*L4 ... -*K3*L7*K9+2.0*L4*L3*K7*K9-2.0*K4*L8*K7*L5-2.0*K4*L8*L7*K5-2.0*L4*K8*L7*K5-K7*K7 ... -*L3*L6-K3*L7*L7*K6+2.0*K4*K3*L7*L9-K4*K4*L3*L10-K3*L4*L4*K10; - - tmp = 2.0*L2*L8*K7*L6+K1*L3*L6*L10+2.0*K1*L5*L8*L9-K1*L3*L9*L9-K1*L5*L5* ... -L10-K1*L8*L8*L6+2.0*L2*L8*L7*K6-2.0*K4*L3*L4*L10+2.0*K4*L3*L7*L9-2.0*K4*L8*L7* ... -L5+2.0*L2*K8*L7*L6-2.0*L2*L8*K4*L9-2.0*L2*L8*L4*K9-2.0*L2*L5*K7*L9-2.0*L2*L5*L7 ... -*K9-2.0*L2*K8*L4*L9-2.0*L2*K5*L7*L9+2.0*L2*L5*K4*L10+2.0*L2*L5*L4*K10+2.0*L2*K5 ... -*L4*L10+2.0*K2*L5*L4*L10+2.0*L1*K5*L8*L9+2.0*L1*L5*K8*L9+2.0*L1*L5*L8*K9-2.0*L1 ... -*K8*L8*L6+2.0*K2*L8*L7*L6-2.0*L1*K5*L5*L10+L1*K3*L6*L10; - p1 = tmp+L1*L3*K6*L10+L1*L3*L6*K10-L1*L5*L5*K10-L1*L8*L8*K6+2.0*K2*L2* ... -L9*L9-2.0*L1*L3*K9*L9-2.0*K2*L2*L6*L10-2.0*K2*L5*L7*L9-2.0*K2*L8*L4*L9-L2*L2*K6 ... -*L10-L2*L2*L6*K10+2.0*L2*L2*K9*L9+2.0*K4*L4*L8*L8-L1*K3*L9*L9-2.0*L4*K8*L7*L5 ... --2.0*L4*L8*K7*L5-2.0*L4*L8*L7*K5+2.0*L4*L3*L7*K9+2.0*L4*K3*L7*L9+2.0*L4*L3*K7* ... -L9-2.0*K7*L3*L7*L6-K3*L7*L7*L6-L3*L7*L7*K6+2.0*K5*L7*L7*L5-K3*L4*L4*L10-L3* ... -L4*L4*K10+2.0*K8*L4*L4*L8+2.0*K7*L7*L5*L5; - - p0 = L1*L3*L6*L10-L1*L3*L9*L9-L1*L5*L5*L10+2.0*L1*L5*L8*L9-L1*L8*L8*L6- ... -L2*L2*L6*L10+L2*L2*L9*L9+2.0*L2*L5*L4*L10-2.0*L2*L5*L7*L9-2.0*L2*L8*L4*L9+2.0* ... -L2*L8*L7*L6-L3*L4*L4*L10+2.0*L4*L3*L7*L9+L4*L4*L8*L8-2.0*L4*L8*L7*L5-L3*L7*L7* ... -L6+L7*L7*L5*L5; - -r=roots([p4,p3,p2,p1,p0]); - -rindex=find(real(r)==r); -%rindex=find(abs(imag(r))<1e-4); - -if length(rindex)>0, - rabs=abs(real(r(rindex))); - rmin=find(min(rabs)==rabs); - la = real(r(rindex(rmin(1)))); -else - la = 0; %not possible -end - diff --git a/visionary/intseclinear.m b/visionary/intseclinear.m deleted file mode 100644 index cbe0ed5..0000000 --- a/visionary/intseclinear.m +++ /dev/null @@ -1,28 +0,0 @@ -function s=intseclinear(m,imseq,option,soriginal); -% INTSECLINEAR s=intseclinear(m,imseq,option,soriginal) calculates 3D features -% with linear methods -% INPUT: -% m - motion object -% imseq - cell array of imagedata objects -% soriginal - If specified, only "missing" features in this object are intersected -% option: -% 'nocoordtrans' - No coordinate transformation -% 'conic' - Quadrics are intersected as conics -% OUTPUT: -% s - structure object containing reconstructed 3D features -% The results may be inaccurate. - - -if nargin<=2 - option = []; -end - -if nargin<=3; - soriginal=[]; -end - -s = intsecpoints(m,imseq,soriginal,option); -s = s + intseclines(m,imseq,soriginal); -s = s + intsecconics(m,imseq,soriginal,option); - - diff --git a/visionary/intseclines.m b/visionary/intseclines.m deleted file mode 100644 index 67384c6..0000000 --- a/visionary/intseclines.m +++ /dev/null @@ -1,56 +0,0 @@ -function s=intseclines(m,imseq,lines); -% INTSECLINES s=intseclines(m,imseq,lines,option) calculates 3D lines -% with linear method -% INPUT: -% m - motion object -% imseq - cell array of imagedata objects -% lines - (optional) specifies lines to be intersected. Otherwise all lines. -% if 'lines' is a structure object, already existing lines are copied -% OUTPUT: -% s - structure object containing reconstructed 3D lines -% The results may be inaccurate. - -if nargin<=2 | isempty(lines) - lines = 1:size(imseq{1},2); -end - -if isa(lines,'structure'); - nbrlines = size(lines,2); - if nbrlines>0, - Ldata = gethomogeneouslines(lines); - else - nbrlines=size(imseq{1},2); - Ldata = NaN*ones(8,nbrlines); - end -else - nbrlines=length(lines); - Ldata = NaN*ones(8,nbrlines); -end - -nbrimages=length(imseq); - -for qq=1:nbrlines; - if ~finite(Ldata(1,qq)), %already existing - jj=0; - M=[]; - for i=1:nbrimages; - P=getcameras(m,i); - l=gethomogeneouslines(imseq{i},qq); - if finite(P(1)) & finite(l(1)), - plane = P'*l; - M = [M;psphere(plane)']; - jj=jj+1; - end - end - if jj>2, %too few views? - [u,ss,v]=svd(M); - Ldata(:,qq)=[v(:,3);v(:,4)]; - end - end %already existing -end - -s=structure([],Ldata); - - - - diff --git a/visionary/intsecpoints.m b/visionary/intsecpoints.m deleted file mode 100644 index 4d8ad4b..0000000 --- a/visionary/intsecpoints.m +++ /dev/null @@ -1,70 +0,0 @@ -function s=intsecpoints(m,imseq,points,option); -% INTSECPOINTS s=intsecpoints(m,imseq,points,option) calculates 3D points -% with linear method -% INPUT: -% m - motion object -% imseq - cell array of imagedata objects -% points - (optional) specifies points to be intersected. Otherwise all points. -% if 'points' is a structure object, already existing points are copied -% option: 'nocoordtrans' - No coordinate transformation -% OUTPUT: -% s - structure object containing reconstructed 3D points -% The results may be inaccurate. - -if nargin<=2 | isempty(points) - points = 1:size(imseq{1},1); -end - -if nargin<=3 - option = []; -end -if isa(points,'structure'); - nbrpoints = size(imseq{1},1); - Upts = NaN*ones(4,nbrpoints); - if size(points,1)>0, - Upts(:,1:size(points,1)) = getpoints(points); - end - points = 1:nbrpoints; -else - nbrpoints=length(points); - Upts = NaN*ones(4,nbrpoints); -end -Tnorm=cell(1,length(imseq)); -for i=1:length(imseq), - if isempty(strmatch('nocoordtrans',option)), - Tnorm{i}=getnormtrans(imseq{i}); - else - Tnorm{i}=eye(3); - end -end -for qq=1:nbrpoints; - if ~isfinite(Upts(1,qq)), %already existing? - M=[]; - jj=0; - for i=1:length(imseq); - P=getcameras(m,i); - u=getpoints(imseq{i},points(qq)); - if isfinite(P(1,1)) & isfinite(u(1)), - %normalise - P=Tnorm{i}*P; - u=Tnorm{i}*u; - jj=jj+1; - M=[M zeros(3*(jj-1),1); ... - P/norm(P,'fro') zeros(3,jj-1) psphere(u)]; - end; - end; - if jj>1, %too few views? - [u,s,v]=svd(M); - [U,alpha]=psphere(v(1:4,4+jj)); - dd=-v(5:(4+jj),4+jj)/alpha; - if sum(sign(dd))<0; - dd=-dd; - U=-U; - end; - Upts(:,qq)=U; - end - end %already existing - -end - -s=structure(Upts); diff --git a/visionary/invessentialm.m b/visionary/invessentialm.m deleted file mode 100644 index c978a05..0000000 --- a/visionary/invessentialm.m +++ /dev/null @@ -1,88 +0,0 @@ - -function [t,R,R2]=invessentialm(E,u1,u2); -% [t,R,R2]=invessentialm(E,u1,u2) - decompose the essential matrix -% into a unit direction t and a rotation matrix so that -% E=essentialm(t,R) (up to scale) -% INPUT: -% E - essential matrix -% u1,u2 - 3xn homogenous image points (optional) -% -% OUTPUT -% t,R - translation rotation -% R2 - second choice of rotation. Not needed if u1,u2 are specified -% since this solution can be discarded by chirality -P=[0 1 0;-1 0 0; 0 0 -1]; % perm. matrix -P2=[0 1 0;-1 0 0; 0 0 1]; % perm. matrix -Q=[0 -1 0; 1 0 0; 0 0 0]; % cross matrix - -[uE,sE,vE]=svd(E); - -s1=sE(1,1); -s2=sE(2,2); -if abs (s1-s2)>1e-3, - error('Not essential matrix'); -else - % s(3,3) should be zero - s3 = s2; % to make S full rang - % F = u*P*Q*inv(u*P)*u*P*diag([s1 s2 s3])*v' - - T= uE*P*Q*inv(uE*P); - t=[ T(3,2) T(1,3) T(2,1)]'; - R=uE*P*diag([1 1 det(uE)*det(vE)])*vE'; - R=sign(det(R))*R; - R2=uE*P2*diag([1 1 det(uE)*det(vE)])*vE'; - R2=sign(det(R2))*R2; - - if nargin>1, - % Choose the one that gives as much positive depth vectors as - % possible. - nr_of_points=size(u1,2); - P1=[eye(3) zeros(3,1)]; - P2=[R' -R'*t]; - % Calculate the structure U. - for i=1:nr_of_points, - M=[P1 u1(:,i) zeros(3,1); P2 zeros(3,1) u2(:,i)]; - % One ought to check that the intersection is well defined. - [u,s,v]=svd(M); - %diag(s)' - U(1:4,i)=v(1:4,6); - %l1(1,i)=-v(5,6); - %l2(1,i)=-v(6,6); - end; - [U,aa]=pflat(U); - [u1t,l1]=psphere(P1*U); - [u2t,l2]=psphere(P2*U); - o1=sign(sum(u1t.*u1)); - o2=sign(sum(u2t.*u2)); - % the vectors o1 and o2 indicate whether the points in - % image 1 and 2 have positive depth or not. Ideally - % all elements of both o1 and o2 should be positive. - if sum(o1.*o2)<0, - R=R2; %If they have different signs change R from R1 to R2. - P2=[R' -R'*t]; - % Calculate the structure U. - for i=1:nr_of_points, - M=[P1 u1(:,i) zeros(3,1); P2 zeros(3,1) u2(:,i)]; - % One ought to check that the intersection is well defined. - [u,s,v]=svd(M); - %diag(s)' - U(1:4,i)=v(1:4,6); - %l1(1,i)=-v(5,6); - %l2(1,i)=-v(6,6); - end; - U=pflat(U); - u1t=psphere(P1*U); - u2t=psphere(P2*U); - o1=sign(sum(u1t.*u1)); - o2=sign(sum(u2t.*u2)); - end; - if sum(o2)<0, - t=-t; % If they have negative sign change t from t to -t. - end; - % Now we have done our best at getting positive depth in the camera - % equation. Calculate once more the camera matrices P1 and P2, - % and the reprojected points u1 and u2 and the sign of the depths o1 - % and o2. - end -end - diff --git a/visionary/m2v.m b/visionary/m2v.m deleted file mode 100644 index 9264d70..0000000 --- a/visionary/m2v.m +++ /dev/null @@ -1,13 +0,0 @@ -function v=m2v(m); -% M2V v=m2v(m) returns vector of symmetric conic/quadric matrix - -if size(m,1)==4, - v=[m(1,1),m(1,2),m(2,2),m(1,3),m(2,3),m(3,3),... - m(1,4),m(2,4),m(3,4),m(4,4)]'; -elseif size(m,1)==3, - v=[m(1,1),m(1,2),m(2,2),m(1,3),m(2,3),m(3,3)]'; -else - error('Wrong matrix-dimension'); -end - - diff --git a/visionary/manconic.m b/visionary/manconic.m deleted file mode 100644 index 3c3037a..0000000 --- a/visionary/manconic.m +++ /dev/null @@ -1,52 +0,0 @@ -function i=manconic(im,fh); -% i=manconic(im,fh) extracts a conic in image im. -% The user starts with giving the coordinate of conic centre followed by -% a point just outside the conic radius. -% INPUT: -% im - image matrix -% fh - (optional) figure handle, if image already plotted - -if nargin<=1; - plot(imagedata(im)); - fh=figure(gcf); -end - -figure(fh); hold on; -% ask for centre and approx. radius -disp(['Mark center of conic']); -x=ginput2(1); -disp(['Mark approximate radius']); -y=ginput2(1); - -dist = norm(x-y); - -%extraction -a=1; -troeskel=10; -punkt=[x(1);x(2);1]; -antalpunkter=16; -stddevs=zeros(1,antalpunkter); -normals=zeros(3,antalpunkter); -edgepoints=zeros(3,antalpunkter); -k=0; -for theta=(1:antalpunkter)*2*pi/antalpunkter; - n=[cos(theta);sin(theta);0]; - sintervall=0:round(dist); - [s,sigmas,toppar]=soklangslinje3(im,punkt,n,a,sintervall,troeskel); - if ~isnan(s), - k=k+1; - stddevs(k)=sigmas; - normals(:,k)=n; - edgepoints(:,k)=punkt+s*n; - end -end; - -if k<6, - disp(['Could not extract sufficient number of edgepoints']); - i=imagedata; -else - %fit conic with normal - i=fitconic(edgepoints(:,1:k),normals(:,1:k)); -end - -%END OF MAIN FUNCTION diff --git a/visionary/manline.m b/visionary/manline.m deleted file mode 100644 index db01cec..0000000 --- a/visionary/manline.m +++ /dev/null @@ -1,52 +0,0 @@ -function i=manline(im,fh); -% i=manline(im,fh) extracts a line in image im. -% The user starts with giving the endpoints of line -% INPUT: -% im - image matrix -% fh - (optional) figure handle, if image already plotted - -if nargin<=1; - plot(imagedata(im)); - fh=figure(gcf); -end - -figure(fh); hold on; -% ask for centre and approx. radius -disp(['Mark endpoints of line']); -[x,y]=ginput2(2); - - -p1 = [x(1);y(1);1]; -p2 = [x(2);y(2);1]; -dist = norm(p1-p2); -antalpunkter=round(dist/5); -plist = p1*ones(1,antalpunkter)+(p2-p1)*((1:antalpunkter)+2)/(antalpunkter+5); -n=[0 -1 0;1 0 0;0 0 1]*(p2-p1); -n=n/norm(n); -stddevs=zeros(1,antalpunkter); -normals=zeros(3,antalpunkter); -edgepoints=zeros(3,antalpunkter); -a=1; -troeskel=10; -k=0; -for j=1:antalpunkter; - punkt=plist(:,j); - sintervall=-6:6; - [s,sigmas,toppar]=soklangslinje3(im,punkt,n,a,sintervall,troeskel); - if ~isnan(s), - k=k+1; - stddevs(k)=sigmas; - normals(:,k)=n; - edgepoints(:,k)=punkt+s*n; - end -end; - -if k<3, - disp(['Could not extract sufficient number of edgepoints']); - i=imagedata; -else - %fit line with normal - i=fitline(edgepoints(:,1:k),normals(:,1:k)); -end - -%END OF MAIN FUNCTION diff --git a/visionary/manpolygon.m b/visionary/manpolygon.m deleted file mode 100644 index 05690db..0000000 --- a/visionary/manpolygon.m +++ /dev/null @@ -1,93 +0,0 @@ -function i=manpolygon(im,corners,fh); -% i=manline(im,fh) extracts a polygonline in image im. -% The user starts with giving the endpoints of line -% INPUT: -% im - image matrix -% corners - nbr of corners in polygon -% fh - (optional) figure handle, if image already plotted -% OUTPUT: -% i - imagedata object with lines and corner points of polygon - -if nargin<=2; - plot(imagedata(im)); - fh=figure(gcf); -end - - -figure(fh); hold on; - -disp(['Mark the ' num2str(corners) ' corners of the polygon']); -[x,y]=ginput2(corners); -x=[x;x(1)]; -y=[y;y(1)]; -i=imagedata; -a=1; -troeskel=10; -extracted=zeros(1,corners); - -for q=1:corners; - p1 = [x(q);y(q);1]; - p2 = [x(q+1);y(q+1);1]; - dist = norm(p1-p2); - antalpunkter=round(dist/5); - plist = p1*ones(1,antalpunkter) + (p2-p1)*((1:antalpunkter)+2)/(antalpunkter+5); - n=[0 -1 0;1 0 0;0 0 1]*(p2-p1); - n=n/norm(n); - stddevs=zeros(1,antalpunkter); - normals=zeros(3,antalpunkter); - edgepoints=zeros(3,antalpunkter); - k=0; - for j=1:antalpunkter; - punkt=plist(:,j); - sintervall=-6:6; - [s,sigmas,toppar,mat,matp]=soklangslinje3(im,punkt,n,a,sintervall,troeskel); - if ~isnan(s), - k=k+1; - stddevs(k)=sigmas; - normals(:,k)=n; - edgepoints(:,k)=punkt+s*n; - end; - end; - if k<3, - disp(['Insufficient edge points for a line in polygon']); - else - iline=fitline(edgepoints(:,1:k),normals(:,1:k)); - i = i + iline; - extracted(1,q)=1; - end; -end; - -% calculate corner points -for q=1:corners; - qprev=mod((q-2),corners)+1; - if extracted(1,q) & extracted(1,qprev); - [pn,L]=intersect(i,qprev,q); - i = addpoints(i,pn,L); - end -end - -%END OF MAIN-FUNCTION - - -function [pn,L]=intersect(i,line1,line2); -% Intersect two images lines - -[l1,L1]=gethomogeneouslines(i,line1); -[l2,L2]=gethomogeneouslines(i,line2); - -C1=pinv(L1'*L1); -C2=pinv(L2'*L2); - -T1=[0 -l1(3) l1(2);l1(3) 0 -l1(1);-l1(2) l1(1) 0]; -T2=[0 -l2(3) l2(2);l2(3) 0 -l2(1);-l2(2) l2(1) 0]; - -p1 = cross(l1,l2); -Cp = T1*C2*T1' + T2*C1*T2'; - -n=[0;0;1]; -pn= p1/(p1'*n); -dpndp1 = (eye(3)/(p1'*n)) - (p1*n')/(p1'*n)^2; -Cpn = dpndp1*Cp*dpndp1'; -[U,S,V]=svd(Cpn); -L=sqrtm(inv(S(1:2,1:2)))*U(:,1:2)'; - diff --git a/visionary/matchsiftvectors.m b/visionary/matchsiftvectors.m deleted file mode 100644 index bff1487..0000000 --- a/visionary/matchsiftvectors.m +++ /dev/null @@ -1,28 +0,0 @@ -% [ind1, ind2] = matchsiftvectors(SIFT1, SIFT2, loweRatio) -% -% Matches two sets of SIFT vectors. Takes as argument two matrices with 128 -% rows (SIFT features as columns), and a Lowe ratio, and returns two row -% vectors ind1 and ind2, where all pairs (ind1{i}, ind2{i}) correspond to -% matched vectors. Each column in SIFT1 is matched to the closest column in -% SIFT2. -% -function [ind1, ind2] = matchsiftvectors(SIFT1, SIFT2, loweRatio) - tmp1 = normc(double(SIFT1)); - tmp2 = normc(double(SIFT2)); - - kdtree = vl_kdtreebuild(tmp2,'NumTrees', 12); - [index,~] = vl_kdtreequery(kdtree,tmp2,tmp1,'MAXCOMPARISONS',50,'NUMNEIGHBORS',2); - maxvals = sum(tmp2(:,index(1,:)).*tmp1,1)'; - secondmaxvals = sum(tmp2(:,index(2,:)).*tmp1,1)'; - tmp = acos(maxvals) < loweRatio*acos(secondmaxvals); - - matches = zeros(1,size(tmp1,2)); - matches(tmp) = index(1,tmp); - ind1 = find(matches ~= 0); - ind2 = matches(ind1); - - % Remove non unique matches (Caused by NN approximation?) - [~,I,~]=unique(ind2); - ind1 = ind1(I); - ind2 = ind2(I); -end \ No newline at end of file diff --git a/visionary/normc.m b/visionary/normc.m deleted file mode 100644 index e4e8f2e..0000000 --- a/visionary/normc.m +++ /dev/null @@ -1,30 +0,0 @@ -function n = normc(m) -%NORMC Normalize columns of a matrix. -% -% Syntax -% -% normc(M) -% -% Description -% -% NORMC(M) normalizes the columns of M to a length of 1. -% -% Examples -% -% m = [1 2; 3 4] -% n = normc(m) -% -% See also NORMR - -% Mark Beale, 1-31-92 -% Copyright (c) 1992-1998 The MathWorks, Inc. -% $Revision: 1.1.1.1 $ $Date: 1998/12/14 13:02:21 $ - -if nargin < 1,error('Not enough input arguments.'); end - -[mr,mc] = size(m); -if (mr == 1) - n = ones(1,mc); -else - n =ones(mr,1)*sqrt(ones./sum(m.*m)).*m; -end diff --git a/visionary/normr.m b/visionary/normr.m deleted file mode 100644 index cee142c..0000000 --- a/visionary/normr.m +++ /dev/null @@ -1,30 +0,0 @@ -function n = normr(m) -%NORMR Normalize rows of a matrix. -% -% Syntax -% -% normr(M) -% -% Description -% -% NORMR(M) normalizes the columns of M to a length of 1. -% -% Examples -% -% m = [1 2; 3 4] -% n = normr(m) -% -% See also NORMC. - -% Mark Beale, 1-31-92 -% Copyright (c) 1992-1998 The MathWorks, Inc. -% $Revision: 1.1.1.1 $ $Date: 1998/12/14 13:02:21 $ - -if nargin < 1,error('Not enough input arguments.'); end - -[mr,mc]=size(m); -if (mc == 1) - n = m ./ abs(m); -else - n=sqrt(ones./(sum((m.*m)')))'*ones(1,mc).*m; -end diff --git a/visionary/pextend.m b/visionary/pextend.m deleted file mode 100644 index ff339e4..0000000 --- a/visionary/pextend.m +++ /dev/null @@ -1,14 +0,0 @@ -function xut=pextend(x); -% but=pextend(bin); add ones to get extended (or homogeneous) coordinates -% INPUT : -% x - matrix in which each column is a point. -% OR structure or imagedata object with points -% OUTPUT : -% xut - result after normalization (in homogeneous coordinates) - -if isa(x,'structure') | isa (x,'imagedata'), - x=getpoints(x); -end - -[sx,sy]=size(x); -xut=[x;ones(1,sy)]; diff --git a/visionary/pflat.m b/visionary/pflat.m deleted file mode 100644 index dd49f91..0000000 --- a/visionary/pflat.m +++ /dev/null @@ -1,20 +0,0 @@ -function [y,alpha]=pflat(x); -% [y,alpha]=pflat(x) - normalization of projective points. -% Do a normalization on the projective -% points in x. Each column is considered to -% be a point in homogeneous coordinates. -% Normalize so that the last coordinate becomes 1. -% WARNING! This is not good if the last coordinate is 0. -% INPUT : -% x - matrix in which each column is a point. -% OR structure or imagedata object with points -% OUTPUT : -% y - result after normalization (in homogeneous coordinates) -% alpha - depth - -if isa(x,'structure') | isa (x,'imagedata'), - x=getpoints(x); -end -[a,n]=size(x); -alpha=x(a,:); -y=x./(ones(a,1)*alpha); diff --git a/visionary/plotconic.m b/visionary/plotconic.m deleted file mode 100644 index 1787768..0000000 --- a/visionary/plotconic.m +++ /dev/null @@ -1,144 +0,0 @@ -function plotconic(conics,style) -%PLOTCONIC plot conics - 6xn matrix where columns are dual conics - -if nargin<2, - style = '-'; -end - -for t = 1:size(conics,2) - v = pflat(conics(:,t)); - if ~isnan(v(1)); - c = inv(v2m(v)); - drawconic([c(1,1),c(1,2)*2,c(2,2),c(1,3)*2,c(2,3)*2,c(3,3)],style); - end -end - -function drawconic (u,style) -%DRAWCONIC -% -% drawconic (u,style) -% draw solutions of x'Ax + bb'x + c = 0. -% - A = [u(1), u(2)/2; u(2)/2, u(3)]; - bb = [u(4); u(5)]; - c = u(6); - - [Q D] = eig(A); - det = D(1,1)*D(2,2); - if (det == 0), - if ((D(1,1) == 0) & (D(2,2) == 0)), - z = -c*(bb' / norm(bb, 2)); - alpha = atan2(-bb(1), bb(2)); - drawline (z, alpha,style); - else - if (D(1,1) == 0), - D = D*[0 1; 1 0]; - Q = Q*[0 1; 1 0]; - end - alpha = atan2(Q(2,1), Q(1,1)); - z(1) = bb(1)/2/D(1,1); - a = D(1,1)/bb(2); - z(2) = (z(1)^2 - c)*a; - drawparabola (z, a, alpha, style); - end - else - bs = Q'*bb; - alpha = atan2(Q(2,1), Q(1,1)); - zs = -(2*D)\bs; - z = Q*zs; - h = -bs'*zs/2-c; - a = h/D(1,1); - b = h/D(2,2); - if ((a > 0) & (b > 0)), - drawellipse (z, sqrt(a), sqrt(b), alpha, style); - else - if (a < 0), - tmp = a; - a = b; - b = -a; - alpha = alpha + pi/2; - else - b = -b; - end - if b>=0, - drawhyperbola (z, sqrt(a), sqrt(b), alpha, style); - end - end - end % if - -%end % drawconic - -function drawline (C, alpha, style) -%DRAWLINE -% -% drawline(C, alpha, style) -% draws line through C. - - s = sin(alpha); c = cos(alpha); - Q =[c -s; s c]; - theta = [-10:0.2:10]; - u = diag(C)*ones(2,length(theta)) + ... - Q*[sin(theta); cos(theta)]; - plot(u(1,:),u(2,:),style); - -%end % drawline - - -function drawparabola (z, a, alpha, style) -%DRAWPARABOLA -% -% drawparabola (z, a, alpha, style) -% draw parabola for a*(x - z(1))^2 + (y - z(2)) = 0 -% rotated by alpha -% - s = sin(alpha); c = cos(alpha); - Q = [c -s; s c]; - theta = [-10:0.2:10]; - u = diag(z)*ones(2,length(theta)) + ... - Q*[theta; a*theta.*theta]; - plot(u(1,:),u(2,:), style); - -%end % drawparabola - -function drawellipse (z, a, b, alpha, style) -%DRAWELLIPSE Draw ellipse -% -% drawellipse (z, a, b, alpha, style) -% draws ellipse into current figure. -% -% z, a, b, alpha: parameters of ellipse -% style: pattern to be used - - s = sin(alpha); c = cos(alpha); - Q =[c -s; s c]; - theta = [0:0.01:2*pi]; - u = diag(z)*ones(2,length(theta)) + ... - Q*[a*cos(theta); b*sin(theta)]; - plot(u(1,:),u(2,:), style); -%end drawellipse - -function drawhyperbola(C, a, b, alpha, style) -%DRAWHYPERBOLA -% -% drawhyperbola(C, a, b, alpha, style) -% draw hyperbola with center C semiaxis a and b -% alpha angle between a and x-axe -% - s = sin(alpha); c = cos(alpha); - Q = [c -s; s c]; - fac = (a + b + 2)*log(a + b + 2); - theta = [-fac:fac / 50:fac]; - u = diag(C)*ones(2,length(theta)) + ... - Q*[a*cosh(theta); b*sinh(theta)]; - plot(u(1,:),u(2,:),style); - u = diag(C)*ones(2,length(theta)) + ... - Q*[-a*cosh(theta); b*sinh(theta)]; - plot(u(1,:),u(2,:),style); -%end % drawhyperbola - - - - - - - diff --git a/visionary/plotcorrs.m b/visionary/plotcorrs.m deleted file mode 100644 index 4cec9a8..0000000 --- a/visionary/plotcorrs.m +++ /dev/null @@ -1,28 +0,0 @@ -function fnbr=plotmatches(im1,im2,ps1,ps2,pmode,plotargs) -if nargin<6 - plotargs='-'; -end -if nargin<5 - pmode=0; -end - - -[m1,n1,~]=size(im1); -[m2,n2,~]=size(im1); - -imtot=[im1 im2]; -fnbr=figure(); - -if pmode==0 - imagesc(imtot);hold on; - plot([ps1(1,:);ps2(1,:)+n1],[ps1(2,:);ps2(2,:)],plotargs); -else - for ii=1:pmode:length(ps1)-pmode - imagesc(imtot);hold on; - plot([ps1(1,ii:ii+pmode-1);ps2(1,ii:ii-1+pmode)+n1],... - [ps1(2,ii:ii-1+pmode);ps2(2,ii:ii-1+pmode)],plotargs); - hold off; - pause - end -end -hold off; \ No newline at end of file diff --git a/visionary/plotline.m b/visionary/plotline.m deleted file mode 100644 index 97c3dd7..0000000 --- a/visionary/plotline.m +++ /dev/null @@ -1,95 +0,0 @@ -function plotline(lines,style) -%PLOTLINE plot(lines,style) plot lines -% lines - 3xn (8xn) matrix where columns are homog. lines -% style - style parameters for plot -% The current axis of figure are used as limitations of the lines -% 2D lines are represented by homogeneous coordinates -% (or by two homogeneous coordinates for end points). -% 3D lines are represented by two homogeneous 3D points. - -if nargin<2, - style = '-'; -end - -numberlines=0; -if strncmp(style,'numbered',8), - numberlines=1; - style=style(9:end); - if length(style)==0, - style = '-'; - end -end - - -holdmode=ishold; - -if size(lines,1)==3, - ax = axis; - for t = 1:size(lines,2) - tmp = lines(:,t); - if abs(tmp(1))1, - neg=find(d>eps); - end - - if length(neg)==1, - dnew = d; - dnew(neg)=d(4); - dnew(4)=d(neg); - - e=eye(4); - p=e; - p(:,neg)=e(:,4); - p(:,4)=e(:,neg); - - d0=sqrt(abs(dnew(1:3)/dnew(4))); - q=diag([d0;1]); - T=u*inv(p')*q; - Xnew=T*X; - xnew = reshape(Xnew(1,:)./Xnew(4,:),mx,nx); - ynew = reshape(Xnew(2,:)./Xnew(4,:),my,ny); - znew = reshape(Xnew(3,:)./Xnew(4,:),mz,nz); - mesh(xnew,ynew,znew,ones(size(znew))); - end - -end - - - - - - - - - diff --git a/visionary/private/accalcdPdx.m b/visionary/private/accalcdPdx.m deleted file mode 100644 index 6301d92..0000000 --- a/visionary/private/accalcdPdx.m +++ /dev/null @@ -1,122 +0,0 @@ -function [dPdx,ppout,focalout,arout] = accalcdPdx(K,R,t,caliboptions,lockcsystem,pp,focal,ar); - -focalout=[]; -focalcnt=0; -arout=[]; -arcnt=0; -ppout=[]; -ppcnt=0; - -b1=[0 1 0;-1 0 0;0 0 0]; -b2=[0 0 -1;0 0 0;1 0 0]; -b3=[0 0 0;0 0 1;0 -1 0]; - -nbrcam=size(K,1); -index=0; -dkindex=zeros(nbrcam,5); -for jj=1:5; - if caliboptions(jj)==2; - dkindex(:,jj)=(index+1); - index=index+1; - elseif caliboptions(jj)==3; - ii=(index+1):(index+nbrcam); - dkindex(:,jj)=ii'; - index=index+nbrcam; - end; -end; - -mut=cell(nbrcam,1); -dt4=[1 0 0]'; -dt5=[0 1 0]'; -dt6=[0 0 1]'; - -dK4 =[0 0 1;0 0 0;0 0 0]; -dK5 =[0 0 0;0 0 1;0 0 0]; -dPdx=sparse(0,0); -for i=1:nbrcam; - ii=(index+1):(index+6); - iii=(12*i-11):(12*i); - dR1=b1*R{i}; - dR2=b2*R{i}; - dR3=b3*R{i}; - KK = [K(i,1) K(i,1)*K(i,3) K(i,4); ... - 0 K(i,1)*K(i,2) K(i,5); ... - 0 0 1 ]; - -%not: if i>1, %first camera is fix - if i>1 | lockcsystem==0, - dP1 = KK*[dR1 (-dR1*t{i})]; - dP2 = KK*[dR2 (-dR2*t{i})]; - dP3 = KK*[dR3 (-dR3*t{i})]; - dP4 = KK*[zeros(3,3) (-R{i}*dt4)]; - dP5 = KK*[zeros(3,3) (-R{i}*dt5)]; - dP6 = KK*[zeros(3,3) (-R{i}*dt6)]; - dPdx(iii,ii(1))=dP1(:); - dPdx(iii,ii(2))=dP2(:); - dPdx(iii,ii(3))=dP3(:); - if i==2 & lockcsystem>0, - tmp=[dP4(:),dP5(:),dP6(:)]; - tmp(:,lockcsystem)=[]; - dPdx(iii,ii(4:5))=tmp; - index=index+5; - else - dPdx(iii,ii(4))=dP4(:); - dPdx(iii,ii(5))=dP5(:); - dPdx(iii,ii(6))=dP6(:); - index=index+6; - end - end - - if dkindex(i,1)>0, - dK1 =[1 K(i,3) 0;0 K(i,2) 0; 0 0 0]; - dPk1 = dK1*[R{i} (-R{i}*t{i})]; - dPdx(iii,dkindex(i,1)) = dPk1(:); - if ~isempty(focal), - if (caliboptions(1)==2 & i==1) | caliboptions(1)==3, - focalcnt=focalcnt+1; - focalout(focalcnt,dkindex(i,1))=1/focal(2); - end - end - end; - if dkindex(i,2)>0, - dK2 =[0 0 0;0 K(i,1) 0; 0 0 0]; - dPk2 = dK2*[R{i} (-R{i}*t{i})]; - dPdx(iii,dkindex(i,2)) = dPk2(:); - if ~isempty(ar), - if (caliboptions(2)==2 & i==1) | caliboptions(2)==3, - arcnt=arcnt+1; - arout(arcnt,dkindex(i,2))=1/ar(2); - end - end - end; - if dkindex(i,3)>0, - dK3 =[0 K(i,1) 0; 0 0 0; 0 0 0]; - dPk3 = dK3*[R{i} (-R{i}*t{i})]; - dPdx(iii,dkindex(i,3)) = dPk3(:); - end; - if dkindex(i,4)>0, - dPk4 = dK4*[R{i} (-R{i}*t{i})]; - dPdx(iii,dkindex(i,4)) = dPk4(:); - if ~isempty(pp), - if (caliboptions(4)==2 & i==1) | caliboptions(4)==3, - ppcnt=ppcnt+1; - ppout(ppcnt,dkindex(i,4))=1/pp(3); - end - end - end; - if dkindex(i,5)>0, - dPk5 = dK5*[R{i} (-R{i}*t{i})]; - dPdx(iii,dkindex(i,5)) = dPk5(:); - if ~isempty(pp), - if (caliboptions(5)==2 & i==1) | caliboptions(5)==3, - ppcnt=ppcnt+1; - ppout(ppcnt,dkindex(i,5))=1/pp(3); - end - end - end; -end; - - - - - diff --git a/visionary/private/ackrtextract.m b/visionary/private/ackrtextract.m deleted file mode 100644 index c058a7d..0000000 --- a/visionary/private/ackrtextract.m +++ /dev/null @@ -1,19 +0,0 @@ -function [K,R,t]=ackrtextract(m); -nbrcam=length(m); -K=zeros(nbrcam,5); -R=cell(nbrcam,1); -t=cell(nbrcam,1); - -for i=1:nbrcam - P =pdp(m{i}); - [KK,Rt]=rq(P);KK=KK/KK(3,3); - K(i,1)=KK(1,1); - K(i,2)=KK(2,2)/KK(1,1); - K(i,3)=KK(1,2)/KK(1,1); - K(i,4)=KK(1,3); - K(i,5)=KK(2,3); - R{i}=Rt(:,1:3); - t{i}=-R{i}'*Rt(:,4); -end - - diff --git a/visionary/private/aclocparam.m b/visionary/private/aclocparam.m deleted file mode 100644 index 380fca3..0000000 --- a/visionary/private/aclocparam.m +++ /dev/null @@ -1,44 +0,0 @@ -function [mut] = aclocparam(K,R,t,dy,caliboptions,lockcsystem); - - b1=[0 1 0;-1 0 0;0 0 0]; - b2=[0 0 -1;0 0 0;1 0 0]; - b3=[0 0 0;0 0 1;0 -1 0]; - -nbrcam=size(K,1); -index=0; -for jj=1:5; - if caliboptions(jj)==2; - ii=index+1; - K(:,jj)=K(:,jj)+dy(ii); - index=index+1; - elseif caliboptions(jj)==3; - ii=(index+1):(index+nbrcam); - K(:,jj)=K(:,jj)+dy(ii); - index=index+nbrcam; - end; -end; - -mut=cell(nbrcam,1); -for i=1:nbrcam; - if i>1 | lockcsystem==0, %first camera is fix - if i==2 & lockcsystem>0, - ii=(index+1):(index+5); - dxx=dy(ii); - indt=1:3;indt(lockcsystem)=[]; - dt=zeros(3,1); - dt(indt)=dxx(4:5); - t{i}=t{i} + dt; - index=index+5; - else - ii=(index+1):(index+6); - dxx=dy(ii); - t{i}=t{i} + dxx(4:6); - index=index+6; - end - R{i}=expm(dxx(1)*b1+dxx(2)*b2+dxx(3)*b3)*R{i}; - end - KK = [K(i,1) K(i,1)*K(i,3) K(i,4); ... - 0 K(i,1)*K(i,2) K(i,5); ... - 0 0 1 ]; - mut{i}=pmatrix(KK*[R{i} (-R{i}*t{i})]); -end; diff --git a/visionary/private/bitoone.m b/visionary/private/bitoone.m deleted file mode 100644 index 7da445f..0000000 --- a/visionary/private/bitoone.m +++ /dev/null @@ -1,24 +0,0 @@ -function [E12,E21]=bitoone(F); - -% function [E12,E21]=bitoone(F); -% Determines the one forms between image 1 and 2 from -% the fundamental matrix between images 1 and 2, -% with a correct scale between them. -% -% Input F=fundamental matrix between images 1 and 2 -% Output [E12,E21]=the one forms between image 1 and 2 and -% betweem image 2 and 1 respectively - -AF=[... -det(F([2,3],[2,3])) det(F([2,3],[3,1])) det(F([2,3],[1,2]));... -det(F([3,1],[2,3])) det(F([3,1],[3,1])) det(F([3,1],[1,2]));... -det(F([1,2],[2,3])) det(F([1,2],[3,1])) det(F([1,2],[1,2]))]; - -[U,S,V]=svd(AF); - -E21=sqrt(S(1,1)).*U(:,1)'; -E12=sqrt(S(1,1)).*V(:,1)'; - - - - diff --git a/visionary/private/bundlestepplc.m b/visionary/private/bundlestepplc.m deleted file mode 100644 index bbc4324..0000000 --- a/visionary/private/bundlestepplc.m +++ /dev/null @@ -1,141 +0,0 @@ -function [sut,mut,step,lambda,oldf,newf,resnew,didbetter]=bundlestepplc(im,s,m,lambda,mode,caliboptions,lockcsystem,pp,focal,ar,zplane); - - -%calculate indices -[mi,si,imi,offsetmotion,offsetsm,offsetres]=calcindexplc(im,s,m,mode,zplane); - -%calculate residual and gradient -[res,resgrad]=calcresgradplc(im,s,m,imi,si,mi,offsetres,offsetsm,pp,focal,ar,zplane); - -oldf=norm(res); -resgrad=sparse(resgrad); -if mode == 1 | mode == 4 | mode ==5 | mode==7, %ordinary projective or only - %structure or only motion or - %knownrotation - -%sum(log(svd(full(resgrad)))<-10)-length(mi)-15 -%keyboard; - - dx=-((resgrad'*resgrad+lambda*speye(offsetsm)))\(resgrad'*res); - [sut,mut]=dolocparamplc(s,m,si,mi,dx,zplane); -elseif mode == 2, %affine - nbrvar=size(resgrad,2); - nbrcam=size(mi,1); - nbrstr=nbrvar-12*nbrcam; - dPdX=sparse(nbrvar,9*nbrcam+nbrstr); - index=(9*nbrcam+1):(9*nbrcam+nbrstr); - dPdX(3*nbrcam+index,index)=speye(nbrstr); - - dpdx=sparse(12,9); - dpdx(1,1)=1;dpdx(2,2)=1; - dpdx(4,3)=1;dpdx(5,4)=1; - dpdx(7,5)=1;dpdx(8,6)=1; - dpdx(10,7)=1;dpdx(11,8)=1;dpdx(12,9)=1; - for qq=1:nbrcam; - dPdX(12*qq-11:12*qq,9*qq-8:9*qq)=dpdx; - end - - resgrad2=resgrad*dPdX; - dy=-((resgrad2'*resgrad2+lambda*speye(9*nbrcam+nbrstr)))\(resgrad2'*res); - dx=zeros(offsetsm,1); - dx(12*nbrcam+1:offsetsm)=dy(index); - for qq=1:nbrcam; - q=9*qq-8; - dx(qq*12-11:qq*12)=... - [dy(q:q+1);0;dy(q+2:q+3);0;dy(q+4:q+5);0;dy(q+6:q+8)]; - end - [sut,mut]=dolocparamplc(s,m,si,mi,dx,zplane); -elseif mode == 3 | mode == 6, %autocalibration - % Extract K,R and t from camera matrices - [K,R,t]=ackrtextract(m); - % Generate dPDX derivatives of all parameters with respect - % to fewer (incorporating autocalibration constraints). - nbrvar=size(resgrad,2); - nbrcam=size(mi,1); - nbrstr=nbrvar-12*nbrcam; - if lockcsystem==0, - nbrcamfreedom = 6*nbrcam; - else - nbrcamfreedom = 6*(nbrcam-1)-1; %first camera is fix - end - nbrcamfreedom = nbrcamfreedom +sum(caliboptions==2); - nbrcamfreedom = nbrcamfreedom +sum(caliboptions==3)*nbrcam; - % - dPdX=sparse(nbrvar,nbrcamfreedom+nbrstr); - index1=(12*nbrcam+1):(12*nbrcam+nbrstr); - index2=(nbrcamfreedom+1):(nbrcamfreedom+nbrstr); - dPdX(index1,index2)=speye(nbrstr); - % - [dPdx,ppout,focalout,arout] = accalcdPdx(K,R,t,caliboptions,lockcsystem,pp,focal,ar); % <oldf, - ii=0; - lambda=lambda*2; - while ( (ii<10) & (newf>oldf) ), - if mode ~= 3 & mode ~=6, - dx=dx/2; - [sut,mut]=dolocparamplc(s,m,si,mi,dx,zplane); - else - dy=dy/2; - [mut] = aclocparam(K,R,t,dy(1:nbrcamfreedom),caliboptions,lockcsystem); - [sut,mskrap]=dolocparamplc(s,m,si,mi,[zeros(12*nbrcam,1);dy(index2)],zplane); - end; - [resnew]=calcresplc(im,sut,mut,imi,si,mi,offsetres,offsetsm,pp,focal,ar); - newf=norm(resnew); - ii=ii+1; - end; -else - lambda=lambda/3; -end; - -if oldf0, - sx=sx-1; - end - si{ii} = (offset+1):(offset+sx); - offset=offset+sx; - end; -end - -offsetsm=offset; - -offset=0; -for ii=1:antalbilder; - for jj=1:antalfeatures; - if size(im{ii,jj},1)>0, - sr = sizeres(im{ii,jj}); - imi{ii,jj} = (offset+1):(offset+sr); - offset=offset+sr; - end; - end; -end; -offsetres=offset; - diff --git a/visionary/private/calcresgradplc.m b/visionary/private/calcresgradplc.m deleted file mode 100644 index 3bde5be..0000000 --- a/visionary/private/calcresgradplc.m +++ /dev/null @@ -1,59 +0,0 @@ -function [res,resgrad]=calcresgradplc(im,s,m,imi,si,mi,offsetres,offsetsm,pp,focal,ar,zplane); - -antalbilder=size(m,1); -antalfeatures=size(s,2); - -res=zeros(offsetres,1); -resgrad=sparse(offsetres,offsetsm); -for ii=1:antalbilder; - for jj=1:antalfeatures; -% [r,drdP,drdU]=dres(im{ii,jj},m{ii},s{jj}); - if size(im{ii,jj},1)>0, - [res(imi{ii,jj}),motgrad,strgrad]=dres(im{ii,jj},m{ii},s{jj}); - if ~isempty(mi{ii}), - if length(mi{ii})==3, %knownrotation case - resgrad(imi{ii,jj},mi{ii})=motgrad(:,10:12); - else - resgrad(imi{ii,jj},mi{ii})=motgrad; - end - end - if ~isempty(si{jj}), - if ~isempty(find(jj==zplane))>0, - strgrad(:,3)=[]; - end - resgrad(imi{ii,jj},si{jj})=strgrad; - end - end; - end; -end; - -%focal length residual (the gradpart is in accalcgrad.m) -if ~isempty(focal), - for ii=1:antalbilder, - K=rq(pdp(m{ii}));K=K/K(3,3); - if (focal(3)==2 & ii==1) | focal(3)==3, - res(end+1)=(K(1,1)-focal(1))/focal(2); - end - end -end -%aspect ratio residual (the gradpart is in accalcgrad.m) -if ~isempty(ar), - for ii=1:antalbilder, - K=rq(pdp(m{ii}));K=K/K(3,3); - if (ar(3)==2 & ii==1) | ar(3)==3, - res(end+1)=(K(2,2)/K(1,1)-ar(1))/ar(2); - end - end -end -%principal point residual (the gradpart is in accalcgrad.m) -if ~isempty(pp), - for ii=1:antalbilder, - K=rq(pdp(m{ii}));K=K/K(3,3); - if (pp(4)==2 & ii==1) | pp(4)==3, - res(end+1)=(K(1,3)-pp(1))/pp(3); - end - if (pp(5)==2 & ii==1) | pp(5)==3, - res(end+1)=(K(2,3)-pp(2))/pp(3); - end - end -end diff --git a/visionary/private/calcresplc.m b/visionary/private/calcresplc.m deleted file mode 100644 index 1b98520..0000000 --- a/visionary/private/calcresplc.m +++ /dev/null @@ -1,43 +0,0 @@ -function res=calcresplc(im,s,m,imi,si,mi,offsetres,offsetsm,pp,focal,ar); - -antalbilder=size(m,1); -antalfeatures=size(s,2); - -res=zeros(offsetres,1); -for ii=1:antalbilder; - for jj=1:antalfeatures; - if size(im{ii,jj},1)>0, - res(imi{ii,jj})=calcres(im{ii,jj},m{ii},s{jj}); - end; - end; -end; -%focal length residual -if ~isempty(focal), - for ii=1:antalbilder, - K=rq(pdp(m{ii}));K=K/K(3,3); - if (focal(3)==2 & ii==1) | focal(3)==3, - res(end+1)=(K(1,1)-focal(1))/focal(2); - end - end -end -%aspect ratio residual -if ~isempty(ar), - for ii=1:antalbilder, - K=rq(pdp(m{ii}));K=K/K(3,3); - if (ar(3)==2 & ii==1) | ar(3)==3, - res(end+1)=(K(2,2)/K(1,1)-ar(1))/ar(2); - end - end -end -%principal point residual -if ~isempty(pp), - for ii=1:antalbilder, - K=rq(pdp(m{ii}));K=K/K(3,3); - if (pp(4)==2 & ii==1) | pp(4)==3, - res(end+1)=(K(1,3)-pp(1))/pp(3); - end - if (pp(5)==2 & ii==1) | pp(5)==3, - res(end+1)=(K(2,3)-pp(2))/pp(3); - end - end -end diff --git a/visionary/private/dolocparamplc.m b/visionary/private/dolocparamplc.m deleted file mode 100644 index 5711469..0000000 --- a/visionary/private/dolocparamplc.m +++ /dev/null @@ -1,36 +0,0 @@ -function [sut,mut]=dolocparamplc(s,m,si,mi,dx,zplane); - -antalbilder=size(m,1); -antalfeatures=size(s,2); - -sut=cell(1,antalfeatures); -mut=cell(antalbilder,1); - -for ii=1:antalbilder; - if ~isempty(mi{ii}), - if length(mi{ii})==3, %knownrotation - dxtmp=zeros(12,1); - dxtmp(10:12)=dx(mi{ii}); - mut{ii}=locparam(m{ii},dxtmp); - else - mut{ii}=locparam(m{ii},dx(mi{ii})); - end - else - mut{ii}=m{ii}; - end -end; - -for ii=1:antalfeatures; - if ~isempty(si{ii}), - if ~isempty(find(ii==zplane))>0, - sut{ii}=locparam(s{ii},[dx(si{ii});0]); - else - sut{ii}=locparam(s{ii},dx(si{ii})); - end - - - else - sut{ii}=s{ii}; - end -end; - diff --git a/visionary/private/getpointsonconic.m b/visionary/private/getpointsonconic.m deleted file mode 100644 index d3df961..0000000 --- a/visionary/private/getpointsonconic.m +++ /dev/null @@ -1,45 +0,0 @@ -function pts=getpointsconic(u, conicsampling) -% WORKS ONLY FOR ELLIPSE - A = [u(1), u(2)/2; u(2)/2, u(3)]; - bb = [u(4); u(5)]; - c = u(6); - [Q D] = eig(A); - det = D(1,1)*D(2,2); - if (det == 0), -%error('not implemented'); -pts=[]; - else - bs = Q'*bb; - alpha = atan2(Q(2,1), Q(1,1)); - zs = -(2*D)\bs; - z = Q*zs; - h = -bs'*zs/2-c; - a = h/D(1,1); - b = h/D(2,2); - if ((a > 0) & (b > 0)), - pts=getellipse (z, sqrt(a), sqrt(b), alpha, conicsampling); - else -pts=[]; -%error('not implemented'); - end - end % if - -%end % drawconic - -function pts=getellipse (z, a, b, alpha, conicsampling) -% -% z, a, b, alpha: parameters of ellipse - - s = sin(alpha); c = cos(alpha); - Q =[c -s; s c]; - theta = [0:2*pi/conicsampling:2*pi]; - pts = diag(z)*ones(2,length(theta)) + ... - Q*[a*cos(theta); b*sin(theta)]; - - pts(:,size(pts,2))=[]; -%end drawellipse - - - - - diff --git a/visionary/private/matgauss.m b/visionary/private/matgauss.m deleted file mode 100644 index c68357f..0000000 --- a/visionary/private/matgauss.m +++ /dev/null @@ -1,14 +0,0 @@ -function [matning]=matgauss(bild,punkt,a); - -x0=punkt(1); -y0=punkt(2); -[m,n]=size(bild); -NN=round(a*3); -cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -cutbild=bild(cuty,cutx); -[x,y]=meshgrid(cutx,cuty); -filter=exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^2*pi); -matning=sum(sum(filter.*cutbild)); - - diff --git a/visionary/private/matgaussderiv.m b/visionary/private/matgaussderiv.m deleted file mode 100644 index 3538a9d..0000000 --- a/visionary/private/matgaussderiv.m +++ /dev/null @@ -1,18 +0,0 @@ -function [matning,komplext]=matgaussderiv(bild,punkt,theta,a); - -x0=punkt(1); -y0=punkt(2); -[m,n]=size(bild); -NN=round(a*3); -cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -cutbild=bild(cuty,cutx); -[x,y]=meshgrid(cutx,cuty); -filtx=2*(x-x0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -filty=2*(y-y0).*exp(- ((x-x0).^2 + (y-y0).^2)/a^2)/(a^4*pi); -matx=sum(sum(filtx.*cutbild)); -maty=sum(sum(filty.*cutbild)); -matning=cos(theta)*matx+sin(theta)*maty; -komplext=matx+sqrt(-1)*maty; - - diff --git a/visionary/private/matgaussderiv2.m b/visionary/private/matgaussderiv2.m deleted file mode 100644 index 7a2e0c1..0000000 --- a/visionary/private/matgaussderiv2.m +++ /dev/null @@ -1,28 +0,0 @@ -function [m1,m2,m3]=matgaussderiv2(bild,punkt,theta,a); - -x0=punkt(1); -y0=punkt(2); -[m,n]=size(bild); -NN=round(a*3); -cutx=max(round(x0)-NN,1):min(round(x0)+NN,n); -cuty=max(round(y0)-NN,1):min(round(y0)+NN,m); -% -%[slask,sx]=size(cutx); -%[slask,sy]=size(cuty); -%if (sx*sy == 0), m1=0; m2=0; m3=0; end; - -cutbild=bild(cuty,cutx); -[x,y]=meshgrid(cutx,cuty); - -filt1 = (2*(x-x0)*cos(theta)+2*(y-y0)*sin(theta))/(a^4*pi).*exp(-((x-x0).^2+(y-y0).^2)/(a^2)); -filt2 = -(2*cos(theta).^2+2*sin(theta).^2)/(a^4*pi).*exp(-((x-x0).^2+(y-y0).^2)/(a^2))+ ... - (-2*(x-x0)*cos(theta)-2*(y-y0)*sin(theta)).^2/(a^6*pi).*exp(-((x-x0).^2+(y-y0).^2)/(a^2)); -filt3 = 3*( 2*cos(theta).^2+2*sin(theta).^2)/(a^6*pi)* ... - (-2*(x-x0)*cos(theta)-2*(y-y0)*sin(theta)).*exp(-((x-x0).^2+(y-y0).^2)/(a^2)) ... --(-2*(x-x0)*cos(theta)-2*(y-y0)*sin(theta)).^3/(a^8*pi).*exp(-((x-x0).^2+(y-y0).^2)/(a^2)); -m1=sum(sum(filt1.*cutbild)); -m2=sum(sum(filt2.*cutbild)); -m3=sum(sum(filt3.*cutbild)); - - - diff --git a/visionary/private/soklangslinje3.m b/visionary/private/soklangslinje3.m deleted file mode 100644 index d673774..0000000 --- a/visionary/private/soklangslinje3.m +++ /dev/null @@ -1,42 +0,0 @@ -function [s,sigmas,toppar,mat,matp]=soklangslinje3(inbild,punkt,normal,a,sintervall,troeskel); -% Line search in image - theta=atan2(normal(2),normal(1)); - toppar=[]; - i=0; - for s=sintervall, - i=i+1; - mat(i)=matgaussderiv(inbild,punkt+s*normal,theta,a); - matp(i)=matgauss(inbild,punkt+s*normal,a); - end; - imax=size(sintervall,2); - mat=abs(mat); % white->black or black->white dont matter - for i=2:imax-1, - if (mat(i) >= mat(i-1)) & ... - (mat(i) >= mat(i+1)) & ... - (mat(i) > troeskel), - s=sintervall(i); - for j=1:5, - punkts=punkt+s*normal; -% if punkts(2) < 0, keyboard; end; -% OBS INGEN KONTROLL PA ATT M2/M3 BLIR STOR!!! - [m1,m2,m3]=matgaussderiv2(inbild,punkts,theta,a); - s=s-max(min(m2/m3,0.3),-0.3); - end; - toppar = [toppar [s;m1]]; - end; - end; - antaltoppar=size(toppar,2); - if antaltoppar>0, -% [mbast,sbast] = min(abs(toppar(1,:))); - [mbast,sbast] = max(abs(toppar(2,:))); - s=toppar(1,sbast); - [m1,m2,m3]=matgaussderiv2(inbild,punkt+s*normal,theta,a); - vint=3^2; - vnormal=vint*3/(16*a^6*pi)/(m3^2); - sigmas=sqrt(vnormal); - else - s=NaN; - toppar=[]; - sigmas=[]; - end; - diff --git a/visionary/private/taylorerror.m b/visionary/private/taylorerror.m deleted file mode 100644 index 1ca3013..0000000 --- a/visionary/private/taylorerror.m +++ /dev/null @@ -1,263 +0,0 @@ -function d=taylorerror(imdata,index,T,ptsindex); - -if nargin<4, - ptsindex=1:size(imdata{1},1); -end; - - -imT1=getnormtrans(imdata{index(1)}); -imT2=getnormtrans(imdata{index(2)}); -imT3=getnormtrans(imdata{index(3)}); - -x1=imT1*getpoints(imdata{index(1)},ptsindex); -x2=imT2*getpoints(imdata{index(2)},ptsindex); -x3=imT3*getpoints(imdata{index(3)},ptsindex); - -m=tritop(T); -P1=getcameras(m,1); -P2=getcameras(m,2); -P3=getcameras(m,3); -m=motion({imT1*P1,imT2*P2,imT3*P3}); -T=ptotri(m); - - - -for i=1:3 - for j=1:3 - for k=1:3 - - t(9*(i-1)+3*(j-1)+k)=T(i,j,k); - - end - end -end -t=t'; - -%kb jb kp jp k j -J=[1 1 3 3 2 2 1 - 1 1 3 2 2 3 -1 - 1 2 3 3 2 1 -1 - 1 2 3 1 2 3 1 - 1 3 3 1 2 2 -1 - 1 3 3 2 2 1 1 - - 2 1 1 3 3 2 1 - 2 1 1 2 3 3 -1 - 2 2 1 3 3 1 -1 - 2 2 1 1 3 3 1 - 2 3 1 1 3 2 -1 - 2 3 1 2 3 1 1 - - 3 1 2 3 1 2 1 - 3 1 2 2 1 3 -1 - 3 2 2 3 1 1 -1 - 3 2 2 1 1 3 1 - 3 3 2 1 1 2 -1 - 3 3 2 2 1 1 1 - - - 1 1 2 3 3 2 -1 - 1 1 2 2 3 3 1 - 1 2 2 3 3 1 1 - 1 2 2 1 3 3 -1 - 1 3 2 1 3 2 1 - 1 3 2 2 3 1 -1 - - 2 1 3 3 1 2 -1 - 2 1 3 2 1 3 1 - 2 2 3 3 1 1 1 - 2 2 3 1 1 3 -1 - 2 3 3 1 1 2 1 - 2 3 3 2 1 1 -1 - - 3 1 1 3 2 2 -1 - 3 1 1 2 2 3 1 - 3 2 1 3 2 1 1 - 3 2 1 1 2 3 -1 - 3 3 1 1 2 2 1 - 3 3 1 2 2 1 -1 - ]; - -n1=size(x1,2); -n2=size(x2,2); -n3=size(x3,2); - - -M=zeros(9*n1,27); -for n=1:n1 -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+x1(i,n)*x2(jp,n)*x3(kp,n)*J(m,7); - end - end - end - -R=M*t; - -for i=1:n1 - -r=R(9*(i-1)+1:9*i); -jJ=trijacob(t,[x1(:,i),x2(:,i),x3(:,i)]); -d(i)=r'*pinv(jJ*jJ')*r; - -end -dd=abs(eig(imT1)); -scale=mean(dd(1:2))/dd(3); -d=sqrt(d/scale/6)*10; - - - - -%%%%%%%%%%%%%%%%%%%%%%%%%% - - -function jJ=trijacob(t,points) - -mm1=points(:,1); -mm2=points(:,2); -mm3=points(:,3); -m1=mm1; -m2=mm2; -m3=mm3; -n=1; - - -%kb jb kp jp k j -J=[1 1 3 3 2 2 1 - 1 1 3 2 2 3 -1 - 1 2 3 3 2 1 -1 - 1 2 3 1 2 3 1 - 1 3 3 1 2 2 -1 - 1 3 3 2 2 1 1 - - 2 1 1 3 3 2 1 - 2 1 1 2 3 3 -1 - 2 2 1 3 3 1 -1 - 2 2 1 1 3 3 1 - 2 3 1 1 3 2 -1 - 2 3 1 2 3 1 1 - - 3 1 2 3 1 2 1 - 3 1 2 2 1 3 -1 - 3 2 2 3 1 1 -1 - 3 2 2 1 1 3 1 - 3 3 2 1 1 2 -1 - 3 3 2 2 1 1 1 - - - 1 1 2 3 3 2 -1 - 1 1 2 2 3 3 1 - 1 2 2 3 3 1 1 - 1 2 2 1 3 3 -1 - 1 3 2 1 3 2 1 - 1 3 2 2 3 1 -1 - - 2 1 3 3 1 2 -1 - 2 1 3 2 1 3 1 - 2 2 3 3 1 1 1 - 2 2 3 1 1 3 -1 - 2 3 3 1 1 2 1 - 2 3 3 2 1 1 -1 - - 3 1 1 3 2 2 -1 - 3 1 1 2 2 3 1 - 3 2 1 3 2 1 1 - 3 2 1 1 2 3 -1 - 3 3 1 1 2 2 1 - 3 3 1 2 2 1 -1 - ]; - - - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -m1(:,1)=[1 0 0]'; - -M=zeros(9,27); -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+m1(i,n)*m2(jp,n)*m3(kp,n)*J(m,7); - end - end - - -jJ(:,1)=M(1:9,:)*t; -m1=mm1; -%%%%%%%%%%%%%%%%%%%%%% - -m1(:,1)=[0 1 0]'; - -M=zeros(9,27); -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+m1(i,n)*m2(jp,n)*m3(kp,n)*J(m,7); - end - end - -jJ(:,2)=M(1:9,:)*t; -m1=mm1; -%%%%%%%%%%%%%%%%%%%%%% - -m2(:,1)=[1 0 0]'; - -M=zeros(9,27); -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+m1(i,n)*m2(jp,n)*m3(kp,n)*J(m,7); - end - end - -jJ(:,3)=M(1:9,:)*t; -m2=mm2; -%%%%%%%%%%%%%%%%%%%%%% - -m2(:,1)=[0 1 0]'; - -M=zeros(9,27); -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+m1(i,n)*m2(jp,n)*m3(kp,n)*J(m,7); - end - end - -jJ(:,4)=M(1:9,:)*t; -m2=mm2; -%%%%%%%%%%%%%%%%%%%%%% - -m3(:,1)=[1 0 0]'; - -M=zeros(9,27); -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+m1(i,n)*m2(jp,n)*m3(kp,n)*J(m,7); - end - end - -jJ(:,5)=M(1:9,:)*t; - -m3=mm3; -%%%%%%%%%%%%%%%%%%%%%% - -m3(:,1)=[0 1 0]'; - -M=zeros(9,27); -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+m1(i,n)*m2(jp,n)*m3(kp,n)*J(m,7); - end - end - - -jJ(:,6)=M(1:9,:)*t; - -m3=mm3; - - - diff --git a/visionary/private/tritobi.m b/visionary/private/tritobi.m deleted file mode 100644 index a9d4abb..0000000 --- a/visionary/private/tritobi.m +++ /dev/null @@ -1,79 +0,0 @@ -function [F]=tritobi(T) - -% function [F]=tritobi(T) -% Determines the bilinear form between images 1 and 2 from -% the trifocal tensor between images 1, 2 and 3 -% -% Input: T=trifocal tensor between images 1, 2 and 3 -% Output: F=fundamental matrix between images 1 and 2 - -T1=T; - -M1=[... -T1(1,1,1) T1(1,1,2) T1(1,1,3);... -T1(1,2,1) T1(1,2,2) T1(1,2,3);... -T1(1,3,1) T1(1,3,2) T1(1,3,3)]; - -M2=[... -T1(2,1,1) T1(2,1,2) T1(2,1,3);... -T1(2,2,1) T1(2,2,2) T1(2,2,3);... -T1(2,3,1) T1(2,3,2) T1(2,3,3)]; - -M3=[... -T1(3,1,1) T1(3,1,2) T1(3,1,3);... -T1(3,2,1) T1(3,2,2) T1(3,2,3);... -T1(3,3,1) T1(3,3,2) T1(3,3,3)]; - -[U,S,V]=svd(M1'); -F1=V(:,3); -[U,S,V]=svd(M2'); -F2=V(:,3); -[U,S,V]=svd(M3'); -F3=V(:,3); - -FF=[F1';F2';F3']; - -T2=tritotri(T1); -M1=[... -T2(1,1,1) T2(1,1,2) T2(1,1,3);... -T2(1,2,1) T2(1,2,2) T2(1,2,3);... -T2(1,3,1) T2(1,3,2) T2(1,3,3)]; - -M2=[... -T2(2,1,1) T2(2,1,2) T2(2,1,3);... -T2(2,2,1) T2(2,2,2) T2(2,2,3);... -T2(2,3,1) T2(2,3,2) T2(2,3,3)]; - -M3=[... -T2(3,1,1) T2(3,1,2) T2(3,1,3);... -T2(3,2,1) T2(3,2,2) T2(3,2,3);... -T2(3,3,1) T2(3,3,2) T2(3,3,3)]; - -[U,S,V]=svd(M1'); -FF1=V(:,3); -[U,S,V]=svd(M2'); -FF2=V(:,3); -[U,S,V]=svd(M3'); -FF3=V(:,3); - -FFF=[FF1 FF2 FF3]; - -M=[... -FF(1,1) 0 0 FFF(1,1) 0 0;... -FF(1,2) 0 0 0 FFF(1,2) 0;... -FF(1,3) 0 0 0 0 FFF(1,3);... -0 FF(2,1) 0 FFF(2,1) 0 0;... -0 FF(2,2) 0 0 FFF(2,2) 0;... -0 FF(2,3) 0 0 0 FFF(2,3);... -0 0 FF(3,1) FFF(3,1) 0 0;... -0 0 FF(3,2) 0 FFF(3,2) 0;... -0 0 FF(3,3) 0 0 FFF(3,3)]; - -[U,S,V]=svd(M); -t=V(:,6); -for i=1:3 - for j=1:3 - F(i,j)=t(i)*FF(i,j); - end; -end; - diff --git a/visionary/private/tritobi1.m b/visionary/private/tritobi1.m deleted file mode 100644 index c3d6dce..0000000 --- a/visionary/private/tritobi1.m +++ /dev/null @@ -1,33 +0,0 @@ -function F=tritobi1(T,im1,im2) - -% function F=tritobi1(T,im1,im2) -% Determines the bilinear form between images im1 and im2 from -% the trifocal tensor between images 1, 2 and 3 -% -% Input: T=trifocal tensor between images 1, 2 and 3 -% im1,im2=images for the bilinear form -% Output: F=fundamental matrix between images im1 and im2 - -ind=sort([im1,im2]); -if ind(1)==2 - im3=1; -elseif ind(2)==3 - im3=2; -else - im3=3; -end; - -for i=1:3 - for j=1:3 - for k=1:3 - ind=[i,j,k]; - ii=ind(im1); - jj=ind(im2); - kk=ind(im3); - TT(ii,jj,kk)=T(i,j,k); - end; - end; -end; - -F=tritobi(TT); - diff --git a/visionary/private/tritotri.m b/visionary/private/tritotri.m deleted file mode 100644 index afd7d82..0000000 --- a/visionary/private/tritotri.m +++ /dev/null @@ -1,115 +0,0 @@ -function T2=tritotri(T1); - -% function T2=tritotri(T1); -% Determines the trifocal tensor between images 2, 1 and 3, -% given the trifocal tensor between images 1, 2 and 3. -% -% Input T1=trifocal tensor between images 1, 2 and 3 -% Output T2=trifocal tensor between images 2, 1 and 3 - -T2(1,1,1)=T1(2,2,1)*T1(3,3,1)-T1(3,2,1)*T1(2,3,1); -T2(1,1,2)=T1(2,2,2)*T1(3,3,2)-T1(3,2,2)*T1(2,3,2); -T2(1,1,3)=T1(2,2,3)*T1(3,3,3)-T1(3,2,3)*T1(2,3,3); - -T2(1,2,1)=T1(3,2,1)*T1(1,3,1)-T1(1,2,1)*T1(3,3,1); -T2(1,2,2)=T1(3,2,2)*T1(1,3,2)-T1(1,2,2)*T1(3,3,2); -T2(1,2,3)=T1(3,2,3)*T1(1,3,3)-T1(1,2,3)*T1(3,3,3); - -T2(1,3,1)=T1(1,2,1)*T1(2,3,1)-T1(2,2,1)*T1(1,3,1); -T2(1,3,2)=T1(1,2,2)*T1(2,3,2)-T1(2,2,2)*T1(1,3,2); -T2(1,3,3)=T1(1,2,3)*T1(2,3,3)-T1(2,2,3)*T1(1,3,3); - - -T2(2,1,1)=-T1(2,1,1)*T1(3,3,1)+T1(3,1,1)*T1(2,3,1); -T2(2,1,2)=-T1(2,1,2)*T1(3,3,2)+T1(3,1,2)*T1(2,3,2); -T2(2,1,3)=-T1(2,1,3)*T1(3,3,3)+T1(3,1,3)*T1(2,3,3); - -T2(2,2,1)=-T1(3,1,1)*T1(1,3,1)+T1(1,1,1)*T1(3,3,1); -T2(2,2,2)=-T1(3,1,2)*T1(1,3,2)+T1(1,1,2)*T1(3,3,2); -T2(2,2,3)=-T1(3,1,3)*T1(1,3,3)+T1(1,1,3)*T1(3,3,3); - -T2(2,3,1)=-T1(1,1,1)*T1(2,3,1)+T1(2,1,1)*T1(1,3,1); -T2(2,3,2)=-T1(1,1,2)*T1(2,3,2)+T1(2,1,2)*T1(1,3,2); -T2(2,3,3)=-T1(1,1,3)*T1(2,3,3)+T1(2,1,3)*T1(1,3,3); - - -T2(3,1,1)=T1(2,1,1)*T1(3,2,1)-T1(3,1,1)*T1(2,2,1); -T2(3,1,2)=T1(2,1,2)*T1(3,2,2)-T1(3,1,2)*T1(2,2,2); -T2(3,1,3)=T1(2,1,3)*T1(3,2,3)-T1(3,1,3)*T1(2,2,3); - -T2(3,2,1)=T1(3,1,1)*T1(1,2,1)-T1(1,1,1)*T1(3,2,1); -T2(3,2,2)=T1(3,1,2)*T1(1,2,2)-T1(1,1,2)*T1(3,2,2); -T2(3,2,3)=T1(3,1,3)*T1(1,2,3)-T1(1,1,3)*T1(3,2,3); - -T2(3,3,1)=T1(1,1,1)*T1(2,2,1)-T1(2,1,1)*T1(1,2,1); -T2(3,3,2)=T1(1,1,2)*T1(2,2,2)-T1(2,1,2)*T1(1,2,2); -T2(3,3,3)=T1(1,1,3)*T1(2,2,3)-T1(2,1,3)*T1(1,2,3); - -% T2(:,j,k) har rätt skala - -M1=[... -T1(1,1,1) T1(1,1,2) T1(1,1,3);... -T1(1,2,1) T1(1,2,2) T1(1,2,3);... -T1(1,3,1) T1(1,3,2) T1(1,3,3)]; - -M2=[... -T1(2,1,1) T1(2,1,2) T1(2,1,3);... -T1(2,2,1) T1(2,2,2) T1(2,2,3);... -T1(2,3,1) T1(2,3,2) T1(2,3,3)]; - -M3=[... -T1(3,1,1) T1(3,1,2) T1(3,1,3);... -T1(3,2,1) T1(3,2,2) T1(3,2,3);... -T1(3,3,1) T1(3,3,2) T1(3,3,3)]; - -[U,S,V]=svd(M1); -F1=V(:,3); -[U,S,V]=svd(M2); -F2=V(:,3); -[U,S,V]=svd(M3); -F3=V(:,3); - -F13=[F1';F2';F3']; -[U,S,V]=svd(F13); -E13=V(:,3); - -[U,S,V]=svd(M1'); -F1=V(:,3); -[U,S,V]=svd(M2'); -F2=V(:,3); -[U,S,V]=svd(M3'); -F3=V(:,3); - -F12=[F1';F2';F3']; -[U,S,V]=svd(F12); -E12=V(:,3); - -T2(1,1,1)=T2(1,1,1)./E13(1); -T2(1,2,1)=T2(1,2,1)./E13(1); -T2(1,3,1)=T2(1,3,1)./E13(1); -T2(2,1,1)=T2(2,1,1)./E13(1); -T2(2,2,1)=T2(2,2,1)./E13(1); -T2(2,3,1)=T2(2,3,1)./E13(1); -T2(3,1,1)=T2(3,1,1)./E13(1); -T2(3,2,1)=T2(3,2,1)./E13(1); -T2(3,3,1)=T2(3,3,1)./E13(1); - -T2(1,1,2)=T2(1,1,2)./E13(2); -T2(1,2,2)=T2(1,2,2)./E13(2); -T2(1,3,2)=T2(1,3,2)./E13(2); -T2(2,1,2)=T2(2,1,2)./E13(2); -T2(2,2,2)=T2(2,2,2)./E13(2); -T2(2,3,2)=T2(2,3,2)./E13(2); -T2(3,1,2)=T2(3,1,2)./E13(2); -T2(3,2,2)=T2(3,2,2)./E13(2); -T2(3,3,2)=T2(3,3,2)./E13(2); - -T2(1,1,3)=T2(1,1,3)./E13(3); -T2(1,2,3)=T2(1,2,3)./E13(3); -T2(1,3,3)=T2(1,3,3)./E13(3); -T2(2,1,3)=T2(2,1,3)./E13(3); -T2(2,2,3)=T2(2,2,3)./E13(3); -T2(2,3,3)=T2(2,3,3)./E13(3); -T2(3,1,3)=T2(3,1,3)./E13(3); -T2(3,2,3)=T2(3,2,3)./E13(3); -T2(3,3,3)=T2(3,3,3)./E13(3); diff --git a/visionary/project.m b/visionary/project.m deleted file mode 100644 index bda6de4..0000000 --- a/visionary/project.m +++ /dev/null @@ -1,74 +0,0 @@ -function imseq=project(s,m,v); -% PROJECT imseq=project(s,m,v) projects structure s with motion m -% in view v to an image sequence -% INPUTS: -% s - structure -% m - motion -% v - views, optional. If not specified all views are assumed -% OUTPUT: -% imseq - cell of imagedata objects. If only one view, imseq is -% an imagedata object - - -if nargin<3, - v=1:size(m); -end - -imseq=cell(1,length(v)); -cnt=1; - -for view=v; - P=getcameras(m,view); - % project points - U=getpoints(s); - if ~isempty(U); - u=P*U; - ii=find(abs(u(3,:))>1e-5); - u(1,ii)=u(1,ii)./u(3,ii); - u(2,ii)=u(2,ii)./u(3,ii); - u(3,ii)=1; - else - u=[]; - end - - % project lines - L=getlines(s); - if ~isempty(L); - u1= P*L(1:4,:); - u2= P*L(5:8,:); - - %store end points - l=[u1;u2]; -% l=zeros(size(u1)); -% for i=1:size(u1,2); -% if ~isnan(u1(:,i)) -% li=null([u1(:,i)';u2(:,i)']); -% else -% li=NaN*ones(3,1); -% end -% l(:,i)=li; -% end - - else - l=[]; - end - - - %project quadrics - Q=getquadrics(s); - if ~isempty(Q); - Pconic = coniccamera(P); - c=Pconic*Q; - else - c=[]; - end - - imseq{cnt}=imagedata([],u,l,c); - cnt=cnt+1; -end - -if length(v)==1, - imseq=imseq{1}; -end - - diff --git a/visionary/projmotion.m b/visionary/projmotion.m deleted file mode 100644 index 421dfda..0000000 --- a/visionary/projmotion.m +++ /dev/null @@ -1,525 +0,0 @@ -function [str,mot,imseq]=projmotion(filenames,logfile); -% [str,mot]=projmotion(filenames,logfile); -% Calculates projective structure, motion and point correspondences -% from a sequence of closely spaced images -% Input: -% filenames - Filenames of images -% logfile - Save log-data during running (optional) -% Output: -% str - structure -% mot - motion -% imseq - cell of IMAGEDATA with points - -if nargin<2, - logfile=''; -end - - -%%%%%%%%%%%% Parameters -ransaciterations=2000; -ransacpoints=6; -threshold=3; -mincornerdistance=30; %pixels -intensitythreshold=3; -patchsize=5; -stdev=1.5; -minpoints=10; -borderignore=12; -%%%%%%%%%%%%%%%%%%%%%%%%%%%% -np=2*patchsize+1; -nbrimages=length(filenames); - - -images=cell(1,nbrimages); -for ii=1:nbrimages, - if strcmp(filenames{ii}(end-3:end),'.pgm'), - images{ii}=readpgm(filenames{ii}); - else - images{ii}=imread(filenames{ii}); - if length(size(images{ii}))==3, - images{ii}=rgb2gray(images{ii}); - end - end -end -[szimage1,szimage2]=size(images{1}); - -%%%%%%%%%%%%%%%%%%%%% -% Determine correspondences in views 1,2,3 -%%%%%%%%%%%%%%%%%%%%% - -imharris=harris(double(images{2}),{'noimage','threshold=1','mindistance=15'}); -%imharris=harris(double(images{2}),{'noimage','threshold=0.7','mindistance=15'}); - -% For each corner in image 2, find it in images 1 & 3 - -pos2=getpoints(imharris); -[pos1,res1]=findpatch3(double(images{2}),double(images{1}),pos2); -tmp=find(res1mincornerdistance^2, - imtmp=addpoints(imtmp,harrispts(:,i)); - end -end - -newimseq=findmorematches(images(1:3),imtmp,F12,F23,F13,threshold,4*intensitythreshold/3); - -newstr=intsecpoints(newimseq,mot); -newstr=bundleplc(newstr,mot,newimseq,{'lambda=1','iteration=50','outputoff','structure'}); - - -%calculate reprojection error -err=zeros(1,size(newstr,1)); -for i=1:3; - imreproj=project(newstr,mot,i); - pts=pflat(getpoints(newimseq{i})); - repts=pflat(getpoints(imreproj)); - err=err+sum((pts-repts).^2); -end -ind=find(err<15*threshold); - - -for i=1:3; - imseq{i}=imseq{i}+imagedata([],getpoints(newimseq{i},ind)); -end; -str=str+structure(getpoints(newstr,ind)); - -[str,mot] = bundleplc(str,mot,imseq,{'iteration=50','outputoff','lambda=500'}); - -%FINALLY determine which points to keep... -err=zeros(1,size(str,1)); -for i=1:3; - imreproj=project(str,mot,i); - pts=pflat(getpoints(imseq{i})); - repts=pflat(getpoints(imreproj)); - err=err+sum((pts-repts).^2); -end -ind=find(err<2*threshold); -for i=1:3; - imseq{i}=imagedata([],getpoints(imseq{i},ind)); -end; -str=structure(getpoints(str,ind)); - -[str,mot] = bundleplc(str,mot,imseq,{'iteration=50','outputoff','lambda=500'}); - - -%%%%%%%%% -nbrpoints=size(imseq{1},1); -%originalpatch=2*ones(1,nbrpoints); - -if length(logfile)>0, - eval(['save ',logfile,num2str(3),' str mot imseq nbrpoints']); -end; - -%save dummy - -%for q=1:3;figure(q);clf;plot(imagedata(double(images{q}),getpoints(imseq{q})));zoom on;end - - -for imnr=4:nbrimages; - - disp(['Image ',num2str(imnr),' Points: ',num2str(nbrpoints)]); - - prevpts=getpoints(imseq{imnr-1}); - -%%%%%%%%%%%%%%%%%%%%%%%%%%% -%follow old paths -%%%%%%%%%%%%%%%%%%%%%%%%%%% - - - [currpts,reslist]=findpatch3(double(images{imnr-1}),double(images{imnr}),prevpts,[],130); - tmp=find(reslist>intensitythreshold); - currpts(:,tmp)=NaN; - - - - -% im1rho=conv2(double(images{imnr}),ones(np),'valid'); -% im1rho(find(im1rho==0))=1; -% im1rho=1./im1rho; -% im1squared=conv2(double(images{imnr}).^2,ones(np),'valid'); - -% for ii=1:size(prevpts,2); -% if ~isnan(prevpts(1,ii)), - -% index=originalpatch(ii); -% index=imnr-1; -% inpos=getpoints(imseq{index},ii); -% predpos=prevpts(:,ii); -% [newpos,res]=findpatch3(double(images{index}),double(images{imnr}),inpos,predpos,im1rho,im1squared); -% if resmincornerdistance^2 | isnan(currpts(1,minindex)), - plist2=[plist2,pt]; - end - end - [plist1,reslist1]=findpatchepi(double(images{imnr-1}),double(images{imnr-2}),plist2,F); - pind1=find(reslist1n0); - ind2=indransac(ind2)-n0; - - tmp=NaN*ones(size(currpts)); - tmp(:,ind1)=currpts(:,ind1); - imseq{imnr}=imagedata(filenames{imnr},tmp); - - nbraddpts=length(ind2); - for qqq=1:imnr, - if qqq==imnr-2, - imseq{qqq}=addpoints(imseq{qqq},plist1(:,ind2)); - elseif qqq==imnr-1, - imseq{qqq}=addpoints(imseq{qqq},plist2(:,ind2)); - elseif qqq==imnr, - imseq{qqq}=addpoints(imseq{qqq},plist3(:,ind2)); - else - imseq{qqq}=addpoints(imseq{qqq},NaN*ones(3,nbraddpts)); - end - end - - %update structure & motion - -% works not so good -% tmp=[getcameras(randmot,1);getcameras(randmot,2)]; -% tmp2=[getcameras(mot,imnr-2);getcameras(mot,imnr-1)]; -% H=tmp\tmp2; -% bestP3=getcameras(randmot,3)*H;bestP3=bestP3/norm(bestP3); - -% works not so good -% bestP3=reseccam3(motion(getcameras(mot,imnr-2:imnr-1)),tmpimseq,1:3,indransac); - - tmpstr=structure(getpoints(str,ind1)); - tmpimseq={imagedata([],currpts(:,ind1))}; - - bestP3=reseclinear(tmpimseq,tmpstr); - - [dummy,tmpmot]=bundleplc(tmpstr,bestP3,tmpimseq,{'iteration=50','lambda=1e4','outputoff','motion'}); - - - mot=mot+tmpmot; - - tmpimseq{1}=imagedata(sparse(szimage1,szimage2),x1orig(:,ind2+n0)); - tmpimseq{2}=imagedata(sparse(szimage1,szimage2),x2orig(:,ind2+n0)); - tmpimseq{3}=imagedata(sparse(szimage1,szimage2),x3orig(:,ind2+n0)); - tmpmot=motion(getcameras(mot,imnr-2:imnr)); - tmpstr=intsecpoints(tmpimseq,tmpmot); - tmpstr=bundleplc(tmpstr,tmpmot,tmpimseq,{'lambda=1','iteration=50','outputoff','structure'}); - str=str+tmpstr; - - % REINITIALIZE due to homography in first triplet... -% if imnr==4 & gric_hborderignore & destpos(1)borderignore & destpos(2)mincornerdistance^2, - imtmp=addpoints(imtmp,harrispts(:,i)); - end - end - - - newimseq=findmorematches(images(imnr-2:imnr),imtmp,F12,F23,F13,threshold,4*intensitythreshold/3); - - tmpmot=motion(getcameras(mot,imnr-2:imnr)); - newstr=intsecpoints(newimseq,tmpmot); - newstr=bundleplc(newstr,tmpmot,newimseq,{'lambda=1','iteration=50','outputoff','structure'}); - - %calculate reprojection error - err=zeros(1,size(newstr,1)); - for i=1:3; - imreproj=project(newstr,tmpmot,i); - pts=pflat(getpoints(newimseq{i})); - repts=pflat(getpoints(imreproj)); - err=err+sum((pts-repts).^2); - end - ind=find(err<6*threshold); - - - for i=1:imnr; - if i0, - eval(['save ',logfile,num2str(imnr),' str mot imseq nbrpoints']); - end; - -end; %image-loop - -%ii=1;figure(ii);reproject(str,mot,imseq,ii,'numbered'); diff --git a/visionary/psphere.m b/visionary/psphere.m deleted file mode 100644 index ac40f05..0000000 --- a/visionary/psphere.m +++ /dev/null @@ -1,17 +0,0 @@ -function [y,alpha]=psphere(x); -% [y,alpha]=psphere(x) - normalization of projective points. -% Do a normalization on the projective -% points in x. Each column is considered to -% be a point in homogeneous coordinates. -% INPUT : -% x - matrix in which each column is a point. -% OUTPUT : -% y - result after normalization. -% alpha - depth - -if isa(x,'structure') | isa (x,'imagedata'), - x=getpoints(x); -end -[a,n]=size(x); -alpha=sqrt(sum(x.^2)); -y=x./(ones(a,1)*alpha); diff --git a/visionary/ptobi.m b/visionary/ptobi.m deleted file mode 100644 index 84336a4..0000000 --- a/visionary/ptobi.m +++ /dev/null @@ -1,18 +0,0 @@ -function B=ptobi(m,index) -% B=ptobi(m,index) -% Calculates the bilinear tensor from the two camera matrices in MOTION m -% index: specifies camera matrices in m (optional) -% Default: index = [1 2] - -if nargin<2, - index = [ 1 2]; -end; - -P1=getcameras(m,index(1)); -P2=getcameras(m,index(2)); - -for i=1:3 - for j=1:3 - B(i,j)=det([P1(1+mod(i,3),:) ; P1(1+mod(i+1,3),:) ; P2(1+mod(j,3),:) ; P2(1+mod(j+1,3),:)]); - end -end diff --git a/visionary/ptotri.m b/visionary/ptotri.m deleted file mode 100644 index de3230c..0000000 --- a/visionary/ptotri.m +++ /dev/null @@ -1,22 +0,0 @@ -function T=ptotri(m,index) -% T=ptotri(m,index) -% Calculates the trilinear tensor from the camera matrices in MOTION m -% index: specifies camera matrices in m (optional) -% Default: index = [1 2 3] - -if nargin<2, - index = [ 1 2 3]; -end; - -P1=getcameras(m,index(1));P1=P1/norm(P1); -P2=getcameras(m,index(2));P2=P2/norm(P2); -P3=getcameras(m,index(3));P3=P3/norm(P3); - - -for i=1:3 - for j=1:3 - for k=1:3 - T(i,j,k)=det([P1(1+mod(i,3),:) ; P1(1+mod(i+1,3),:) ; P2(j,:) ; P3(k,:)]); - end - end -end diff --git a/visionary/quat2rot.m b/visionary/quat2rot.m deleted file mode 100644 index 02e4fc2..0000000 --- a/visionary/quat2rot.m +++ /dev/null @@ -1,7 +0,0 @@ -function R=quat2rot(q); -% function R=quat2rot(q); -% Unit quaternion q to rotation matrix R - -R=[ q(1)^2+q(2)^2-q(3)^2-q(4)^2, 2*q(2)*q(3)-2*q(1)*q(4), 2*q(1)*q(3)+2*q(2)*q(4);... - 2*q(1)*q(4)+2*q(2)*q(3), q(1)^2-q(2)^2+q(3)^2-q(4)^2, 2*q(3)*q(4)-2*q(1)*q(2);... - 2*q(2)*q(4)-2*q(1)*q(3), 2*q(1)*q(2)+2*q(3)*q(4), q(1)^2-q(2)^2-q(3)^2+q(4)^2]; diff --git a/visionary/randomscene.m b/visionary/randomscene.m deleted file mode 100644 index 734a181..0000000 --- a/visionary/randomscene.m +++ /dev/null @@ -1,291 +0,0 @@ -function [str,mot,imseq,imseq0]=randomscene(nbrimages,nbrpoints,nbrlines,nbrconics,option) -% RANDOMSCENE creates a random scene and images with random camera positions -% [str,mot,imseq,imseq0]=randomscene(nbrimages,nbrpoints,nbrlines,nbrconics,option) -% INPUT: -% nbrimages - number of images -% nbrpoints - number of points -% nbrlines - number of lines -% nbrconics - numbder of conics -% option: -% 'affine' - use the affine camera model -% 'constant' - constant intrinsic parameters -% 'zeroprincipal' - principal point at zero -% 'skew' - varying intrinsic parameters, except skew is zero -% 'distance=X' - set nominal camera distance to 'X' -% 'cube=X' - set nominal cube to [-X,X] -% 'noise=X' - add noise to image-points with std 'X' -% 'calibrated' - calibration matrices equal to identity -% 'norotation' - no relative rotation, hence translating cameras -% 'conics' - Quadrics are rank 3 disk-quadrics, i.e. conics in a plane -% 'closecameras' - cameras are close to previous camera -% 'planar' - points will be on z=0-plane -% OUTPUT: -% str - structure -% mot - motion -% imseq - cell list of imagedata objects -% imseq0 - unperturbated version of imseq -% -% Default option is perspective camera with skew 0 and aspect ratio 1, -% with focal lengths around 1000, and 3D-features within the cube [-500,500] -% and camera distance around 1000 to origo. - -if nargin<1, - nbrimages=5; -end -if nargin<2, - nbrpoints=10; -end -if nargin<3, - nbrlines=0; -end -if nargin<4, - nbrconics=0; -end - -planar=0; -distance = 1000; -cube = 500; -noise = 0; -norotation=0; -affinecamera=0; -linesampling=10; -conicsampling=10; -redo=0; -camidentity=0; -diskquads=0; -zeroprincipal=0; -constantinternals=0; - -closecameras=0; - -if nargin>=5, %option set? - if strmatch('planar',option); - planar=1; - end - if strmatch('closecameras',option); - closecameras=1; - end - if strmatch('affine',option); - affinecamera=1; - end - if strmatch('constant',option); - constantinternals=1; - end - if strmatch('norotation',option); - norotation=1; - end - if strmatch('zeroprincipal',option); - zeroprincipal=1; - end - - if strmatch('skew',option); - disp('skew option not implemented. PLEASE do!'); - keyboard; - end - if strmatch('distance',option); - q=strmatch('distance=',option); - strdist = option{q}(10:length(option{q})); - distance=str2num(strdist); - end - if strmatch('cube',option); - q=strmatch('cube=',option); - strcube = option{q}(6:length(option{q})); - cube=str2num(strcube); - end - if strmatch('noise',option); - q=strmatch('noise=',option); - strnoise = option{q}(7:length(option{q})); - noise=str2num(strnoise); - end - if strmatch('calibrated',option); - camidentity=1; - end - if strmatch('conics',option); - diskquads=1; - end -end - -%create random points -point3d=[2*cube*(rand(3,nbrpoints)-0.5);ones(1,nbrpoints)]; -if planar, - point3d(3,:)=0; -end - -%create random lines -cont=1; -while cont==1, - line3d=[2*cube*(rand(3,nbrlines)-0.5);ones(1,nbrlines);... - 2*cube*(rand(3,nbrlines)-0.5);ones(1,nbrlines)]; - if nbrlines == 0, - cont=0; - else - tmp=sqrt(min(sum((line3d(1:4,:)-line3d(5:8,:)).^2))); - if tmp>cube/10, - cont=0; - end - end -end - -% create random 3D-ellipsoids -quadric = zeros(10,nbrconics); -for i=1:nbrconics, - % axes - Q=diag([(1/750+1/50*rand(3,1)).^2;-1]); - - % center - pos=2*cube*(rand(3,1)-0.5); - - % rotation - angles=rand(3,1)*2*pi; - rot=getrotation(angles(1),angles(2),angles(3)); - T=[rot,pos;0 0 0 1]; - - invT=inv(T); - Q=invT'*Q*invT; % locus - - if diskquads==0, - Ql=m2v(inv(Q)); - else -% [u,d,v]=svd(inv(Q)); -% d(2,2)=0; -% Ql=m2v(u*d*v'); - [u,t]=schur(Q);tmp=1./diag(t);tmp(1)=0; - Ql=m2v(u*diag(tmp)*u'); - end - quadric(:,i)=Ql; -end - -mot=motion; -str=structure(point3d,line3d,quadric); -imseq={}; -imseq0={}; - -% generate camera positions -for l=1:nbrimages - t=distance*[randn(1,1)*0.1 randn(1,1)*0.1 (1+randn(1,1)*0.1)]'; - if norotation, - R=eye(3); - elseif l>1 & closecameras, - R=R*expm(skew(randn(3,1)/5)); - else - R=randrot; - end - if affinecamera, - R(3,:)=0; - end - f=1000+randn*50; - if camidentity, - K=eye(3); - elseif zeroprincipal - K=[f 0 0; - 0 f 0; - 0 0 1]; - else - K=[f 0 100*randn; - 0 f 100*randn; - 0 0 1]; - end - - if constantinternals, - if l==1, - Kfirst=K; - else - K=Kfirst; - end - end - pcam=K*[R t]; - if affinecamera, - pcam=pcam/pcam(3,4); - [tmp1,tmp2]=rq(pcam(1:2,1:2)); - if det(tmp2)==-1, - [tmp3,tmp4]=eig(tmp2); - pcam(1:2,1:2)=tmp1*tmp3; - end - end - mot=addcameras(mot,pcam); - im = project(str,motion({pcam})); - imseq0={imseq0{:},im}; - - % perturbate points - pts = pflat(getpoints(im)); - im = clearpoints(im); - if nbrpoints>0, - pts(1:2,:)=pts(1:2,:)+noise*randn(2,size(pts,2)); - end - im = addpoints(im,pts); - - % perturbate lines - lend1=pflat(pcam*line3d(1:4,:)); - lend2=pflat(pcam*line3d(5:8,:)); - nline=zeros(3,nbrlines); - for j=1:nbrlines; - d=lend2(:,j)-lend1(:,j); - normal=normr([d(2), -d(1)]); - - lx=lend1(1,j):d(1)/(linesampling-1)-sign(d(1))*eps:lend2(1,j); - ly=lend1(2,j):d(2)/(linesampling-1)-sign(d(2))*eps:lend2(2,j); - - linenoise=noise*randn(size(lx)); - if size(lx,2)==size(ly,2), - lx=lx+normal(1)*linenoise; - ly=ly+normal(2)*linenoise; - if noise ==0, - [u,s,v]=svd([lx',ly',ones(size(lx'))]); - nline(:,j)=v(:,3); - else - nline(:,j)=gethomogeneouslines(fitline([lx;ly;ones(1,size(lx,2))])); - end - else - redo = 1; %a line is projected as "point" ->failed - end - end - im = clearlines(im); - im = addlines(im,nline); - - % perturbate conics - if nbrconics>0, - co=getconics(im); - - for iq=1:nbrconics, - c = inv(v2m(co(:,iq))); - pts=getpointsonconic([c(1,1),c(1,2)*2,c(2,2),... - c(1,3)*2,c(2,3)*2,c(3,3)],conicsampling); - -tmp=v2m(co(:,iq)); -tmp(1:2,3)=0;tmp(3,1:2)=0;tmp=inv(tmp); - if isempty(pts) | min(svd(tmp(1:2,1:2)/tmp(3,3)))<5e-7, - redo=1; - else - pts=pts+noise*randn(2,size(pts,2)); - tmp=fitconic([pts;ones(1,size(pts,2))]); - if isempty(tmp), - redo=1; - else - co(:,iq)=pflat(getconics(tmp)); - -tmp=v2m(co(:,iq)); -tmp(1:2,3)=0;tmp(3,1:2)=0;tmp=inv(tmp); -if isempty(pts) | min(svd(tmp(1:2,1:2)/tmp(3,3)))<5e-7, -redo=1; -end - end -% [u,s,v]=svd([pts(1,:).^2;2*pts(1,:).*pts(2,:);pts(2,:).^2;... -% 2*pts(1,:);2*pts(2,:);ones(1,size(pts,2))]'); -% co(:,iq)=pflat(m2v(inv(v2m(v(:,6)))))); - end - end - im = clearconics(im); - im = addconics(im,co); - end - - imseq={imseq{:},im}; -end; - - -if redo==1, %failed somehow - [str,mot,imseq,imseq0]=randomscene(nbrimages,nbrpoints,nbrlines,... - nbrconics,option); -end - - - diff --git a/visionary/randrot.m b/visionary/randrot.m deleted file mode 100644 index c26c9db..0000000 --- a/visionary/randrot.m +++ /dev/null @@ -1,20 +0,0 @@ -function R=randrot(direction); -% RANDROT - creates a rotation matrix that maps vectors in 'direction' -% towards the z-axis. The orientation around this axis is random. -% 'direction' is optional - -if nargin==0, - direction = 2*rand(3,3)-1; -end - - -[u,d,v]=svd(direction); - -if det(u)<0, - R=(u(:,[3 2 1]))'; -else - R=(u(:,[2 3 1]))'; -end; - -fi=rand*2*pi; -R=[cos(fi) sin(fi) 0;-sin(fi) cos(fi) 0;0 0 1]*R; diff --git a/visionary/ransac2views.m b/visionary/ransac2views.m deleted file mode 100644 index cc4dfdc..0000000 --- a/visionary/ransac2views.m +++ /dev/null @@ -1,122 +0,0 @@ -function [bestmot,ind,bestd]=ransac2views(imdata,iteration,threshold) -% [mot,ind]=ransac2views(imdata,iteration,threshold) -% Calculate the motion of two views using ransac -% Input: -% imdata - cell of IMAGEDATA -% iteration - iterations in RANSAC (optional) -% threshold - for inliers in pixels (optional) -% Output: -% mot - motion -% ind - indices of used points -% bestd - reprojection errors -% Default: iteration=100, threshold=1; - -if nargin<2, - iteration=500; -end; -if nargin<3 - threshold=1; -end - -xx1=getpoints(imdata{1}); -xx2=getpoints(imdata{2}); - - - -n1=size(xx1,2); -consistent=0; - -M=zeros(7,9); - - -for i=1:iteration, - - Per=randperm(n1); - - for jj=1:7, - index=Per(jj); - x1=xx1(1,index); - x2=xx1(2,index); - x3=xx2(1,index); - x4=xx2(2,index); - M(jj,:)=[x1*x3,x2*x3,x3,x1*x4,x2*x4,x4,x1,x2,1]; - end - [u,s,v]=svd(M); - f1=v(1,8);f2=v(2,8);f3=v(3,8);f4=v(4,8);f5=v(5,8);f6=v(6,8);f7=v(7,8);f8=v(8,8);f9=v(9,8); - g1=v(1,9);g2=v(2,9);g3=v(3,9);g4=v(4,9);g5=v(5,9);g6=v(6,9);g7=v(7,9);g8=v(8,9);g9=v(9,9); - F=[f1,f2,f3;f4,f5,f6;f7,f8,f9]; - G=[g1,g2,g3;g4,g5,g6;g7,g8,g9]; - - poly=[g1*g5*g9-g1*g6*g8-g4*g2*g9+g4*g3*g8+g7*g2*g6-g7*g3*g5,g1*f5*g9+g1*g5*f9-g1*f6*g8-g1*g6*f8+f1*g5*g9-f1*g6*g8-g4*f2*g9-g4*g2*f9+g4*f3*g8+g4*g3*f8-f4*g2*g9+f4*g3*g8+g7*f2*g6+g7*g2*f6-g7*f3*g5-g7*g3*f5+f7*g2*g6-f7*g3*g5,... - g1*f5*f9-g1*f6*f8+f1*f5*g9+f1*g5*f9-f1*f6*g8-f1*g6*f8-g4*f2*f9+g4*f3*f8-f4*f2*g9-f4*g2*f9+f4*f3*g8+f4*g3*f8+g7*f2*f6-g7*f3*f5+f7*f2*g6+f7*g2*f6-f7*f3*g5-f7*g3*f5,... - f4*f3*f8-f7*f3*f5+f7*f2*f6-f4*f2*f9-f1*f6*f8+f1*f5*f9]; - ll=roots(poly); - - ll=ll(find(abs(imag(ll))==0)); - - for jj=1:length(ll); - - H=F+ll(jj)*G; - - tmp=log(svd(H/norm(H))+eps); - if tmp(2)>-5, %rank one matrix not allowed - - %check feasibility of other points - % (HARTLEY/STURM triangulation) - H=H'/norm(H); - e1=pflat(cross(H(:,1),H(:,2))); - e2=pflat(cross(H(1,:)',H(2,:)')); - - T1=eye(3);T1(:,3)=e1; - T2=eye(3);T2(:,3)=e2; - - tmp=T1'*H*T2; - [u,s,v]=svd(tmp(1:2,1:2)); - T1(1:2,1:2)=T1(1:2,1:2)*u'; - T2(1:2,1:2)=T2(1:2,1:2)*v; - Ftmp=T1'*H*T2; - lambda=Ftmp(1,1)/Ftmp(2,2); - - xx1t=inv(T1)*xx1; - xx2t=inv(T2)*xx2; - - res=zeros(size(xx1t,2),1); - for qq=1:size(xx1t,2), - mx1=xx1t(1,qq); - my1=xx1t(2,qq); - mx2=xx2t(1,qq); - my2=xx2t(2,qq); - - - poly=[mx1*my1-mx2*lambda*my2,... - -lambda^2*my2^2+mx2^2*lambda^2-mx1^2+my1^2,... - -mx1*my1+2*mx1*my1*lambda^2+mx2*lambda^3*my2-2*mx2*lambda*my2,... - -2*lambda^2*my2^2-2*mx1^2*lambda^2+2*mx2^2*lambda^2+2*my1^2*lambda^2,... - mx1*my1*lambda^4-2*mx1*my1*lambda^2+2*mx2*lambda^3*my2-mx2*lambda*my2,... - -mx1^2*lambda^4-lambda^2*my2^2+mx2^2*lambda^2+my1^2*lambda^4,... - -mx1*my1*lambda^4+mx2*lambda^3*my2]; - - r=roots(poly); - r=r(find(imag(r)==0)); - - [dist,indqq]=min((r*mx1+my1).^2./(r.^2+1)+(lambda*mx2-r*my2).^2./(r.^2+lambda^2)); - res(qq)=dist; - end - - - - index=find(resconsistent, - consistent=length(index); - ind=index; - bestd=res; - bestF=H; - end - end %if rank one - end -end - -bestmot=bitop(bestF); - - diff --git a/visionary/ransac3pts.m b/visionary/ransac3pts.m deleted file mode 100644 index 3ea6419..0000000 --- a/visionary/ransac3pts.m +++ /dev/null @@ -1,47 +0,0 @@ -function [P, inliers] = ransac3pts(u, U, tol, iters) - % [P, inliers] = ransac3pts(u, U, tol, iters) - % - % Performs 3 point RANSAC for pose of calibrated camera. u should be a - % 2xN matrix with normalized image coordinates, U should be a 3xN - % matrix with corresponding 3D points. tol is the reprojection - % threshold for correspondences to be considered inliers, measured in - % the z=1 image plane. iters denotes the number of ransac iterations - - N = size(u,2); - U_h = pextend(U); - P = zeros(3,4); - inliers = zeros(N,1); - for iter = 1:iters - % Choose three points, make sure the correspondences are are unique - % (sometimes sift triggers twice at the same point), and fit camera - % pose to correspondences - perm = randperm(N,3); - if (all(U(:,perm(1))-U(:,perm(2)) == 0) || all(U(:,perm(2))-U(:,perm(3)) == 0) || all(U(:,perm(1))-U(:,perm(3)) == 0)) - continue; - end - if (all(u(:,perm(1))-u(:,perm(2)) == 0) || all(u(:,perm(2))-u(:,perm(3)) == 0) || all(u(:,perm(1))-u(:,perm(3)) == 0)) - continue; - end - - motlist = resec3pts(imagedata([],u(:,perm)),structure(U(:,perm))); - P_ransac = getcameras(motlist); - - % Go through all found cameras and count inliers - for k = 1:length(P_ransac) - U_proj = P_ransac{k}*U_h; - inFront = (U_proj(3,:) > 0); - U_proj = pflat(U_proj); - currInliers = inFront & (sum((U_proj(1:2,:)-u).^2,1) < tol*tol); - - if (nnz(currInliers) > nnz(inliers)) - inliers = currInliers; - P = P_ransac{k}; - end - end - - if (mod(iter, 250) == 0) - % display(['Finished iteration ', num2str(iter), '!']); - end - end -end - diff --git a/visionary/ransac3pts_weighted.m b/visionary/ransac3pts_weighted.m deleted file mode 100644 index 2cb42bb..0000000 --- a/visionary/ransac3pts_weighted.m +++ /dev/null @@ -1,49 +0,0 @@ -function [P, inliers] = ransac3pts_weighted(u, U, weights, tol, iters) - % [P, inliers] = ransac3pts_weighted(u, U, weights, tol, iters) - % - % Performs weighted 3-point-RANSAC for pose of calibrated camera. u - % should be a 2xN matrix with normalized image coordinates, U should be - % a 3xN matrix with corresponding 3D points. weights is a vector of - % length N where element i denotes the probability of picking - % correspondence i in the RANSAC loop. tol is the reprojection - % threshold for correspondences to be considered inliers, measured in - % the z=1 image plane. iters denotes the number of ransac iterations. - - N = size(u,2); - U_h = pextend(U); - P = zeros(3,4); - inliers = zeros(N,1); - for iter = 1:iters - % Choose three points, make sure the correspondences are are unique - % (sometimes sift triggers twice at the same point), and fit camera - % pose to correspondences - perm = randsample(N,3,true,weights); - if (all(U(:,perm(1))-U(:,perm(2)) == 0) || all(U(:,perm(2))-U(:,perm(3)) == 0) || all(U(:,perm(1))-U(:,perm(3)) == 0)) - continue; - end - if (all(u(:,perm(1))-u(:,perm(2)) == 0) || all(u(:,perm(2))-u(:,perm(3)) == 0) || all(u(:,perm(1))-u(:,perm(3)) == 0)) - continue; - end - - motlist = resec3pts(imagedata([],u(:,perm)),structure(U(:,perm))); - P_ransac = getcameras(motlist); - - % Go through all found cameras and count inliers - for k = 1:length(P_ransac) - U_proj = P_ransac{k}*U_h; - inFront = (U_proj(3,:) > 0); - U_proj = pflat(U_proj); - currInliers = inFront & (sum((U_proj(1:2,:)-u).^2,1) < tol*tol); - - if (nnz(currInliers) > nnz(inliers)) - inliers = currInliers; - P = P_ransac{k}; - end - end - - if (mod(iter, 250) == 0) - % display(['Finished iteration ', num2str(iter), '!']); - end - end -end - diff --git a/visionary/ransac3views.m b/visionary/ransac3views.m deleted file mode 100644 index c2d3ebf..0000000 --- a/visionary/ransac3views.m +++ /dev/null @@ -1,135 +0,0 @@ -function [bestmot,ind,bestd]=ransac3views(imdata,iteration,threshold) -% [mot,ind]=ransac3views(imdata,iteration,threshold) -% Calculate the motion of three views using ransac -% Input: -% imdata - cell of IMAGEDATA -% iteration - iterations in RANSAC (optional) -% threshold - for inliers in pixels (optional) -% Output: -% mot - motion -% ind - indices of used points -% bestd - reprojection errors -% Default: iteration=100, threshold=1; - -if nargin<2, - iteration=100; -end; -if nargin<3 - threshold=1; -end - -swarn=warning; -warning('off'); - -imT1=getnormtrans(imdata{1}); -imT2=getnormtrans(imdata{2}); -imT3=getnormtrans(imdata{3}); -invT1=inv(imT1); -invT2=inv(imT2); -invT3=inv(imT3); - - -x1orig=getpoints(imdata{1}); -x2orig=getpoints(imdata{2}); -x3orig=getpoints(imdata{3}); - - -x1all=psphere(imT1*x1orig); -x2all=psphere(imT2*x2orig); -x3all=psphere(imT3*x3orig); - -imseqnew={imagedata([],x1all),imagedata([],x2all),imagedata([],x3all)}; - - -n1=size(x1all,2); -consistent=0; - - -M=zeros(9,7); - - -for i=1:iteration, - - Per=randperm(n1); - Per=Per(1:6); - - [scell,mcell]=sm6points(imseqnew,Per); - - for j=1:length(scell); - - P1=getcameras(mcell{j},1); - P2=getcameras(mcell{j},2); - P3=getcameras(mcell{j},3); - - tmp1=svd(P1); - tmp2=svd(P2); - tmp3=svd(P3); - if tmp1(3)>1e-8 & tmp2(3)>1e-8 & tmp3(3)>1e-8, - - - M(1:3,1:4)=P1/norm(P1); - M(4:6,1:4)=P2/norm(P2); - M(7:9,1:4)=P3/norm(P3); - - P1orig=invT1*P1; - P2orig=invT2*P2; - P3orig=invT3*P3; - - - p1a=P1orig(1,:); - p2a=P1orig(2,:); - p3a=P1orig(3,:); - p1b=P2orig(1,:); - p2b=P2orig(2,:); - p3b=P2orig(3,:); - p1c=P3orig(1,:); - p2c=P3orig(2,:); - p3c=P3orig(3,:); - d=zeros(1,n1); - - for qq=1:n1; - if all(Per-qq) - M(1:3,5)=x1all(:,qq); - M(4:6,6)=x2all(:,qq); - M(7:9,7)=x3all(:,qq); - [u,s,v]=svd(M); - X=v(1:4,7)/v(4,7); - - for rr=1:2, %two bundlesteps - p3aX=p3a*X;p3bX=p3b*X;p3cX=p3c*X; - p1aX=p1a*X/p3aX;p2aX=p2a*X/p3aX; - p1bX=p1b*X/p3bX;p2bX=p2b*X/p3bX; - p1cX=p1c*X/p3cX;p2cX=p2c*X/p3cX; - f=[p1aX-x1orig(1,qq);p2aX-x1orig(2,qq); - p1bX-x2orig(1,qq);p2bX-x2orig(2,qq); - p1cX-x3orig(1,qq);p2cX-x3orig(2,qq)]; - fgrad=[p1a(1:3)/p3aX-p1aX/p3aX*p3a(1:3); - p2a(1:3)/p3aX-p2aX/p3aX*p3a(1:3); - p1b(1:3)/p3bX-p1bX/p3bX*p3b(1:3); - p2b(1:3)/p3bX-p2bX/p3bX*p3b(1:3); - p1c(1:3)/p3cX-p1cX/p3cX*p3c(1:3); - p2c(1:3)/p3cX-p2cX/p3cX*p3c(1:3)]; - X(1:3)=X(1:3)-inv(fgrad'*fgrad+0.0001*eye(3))*fgrad'*f; - end - p3aX=p3a*X;p3bX=p3b*X;p3cX=p3c*X; - d(qq)=(p1a*X/p3aX-x1orig(1,qq))^2+(p2a*X/p3aX-x1orig(2,qq))^2+... - (p1b*X/p3bX-x2orig(1,qq))^2+(p2b*X/p3bX-x2orig(2,qq))^2+... - (p1c*X/p3cX-x3orig(1,qq))^2+(p2c*X/p3cX-x3orig(2,qq))^2; - end - end - newconsistent=sum(d<15*threshold); - if newconsistent>consistent; - consistent=newconsistent; - ind=find(d<15*threshold); - bestd=d; - bestmot=motion({P1orig/norm(P1orig),P2orig/norm(P2orig),P3orig/norm(P3orig)}); - end - - end %if singular values of camera matrices... - end %loop over the "3" solutions - -end - - - -warning(swarn); diff --git a/visionary/ransacfocallength.m b/visionary/ransacfocallength.m deleted file mode 100644 index 23ab25f..0000000 --- a/visionary/ransacfocallength.m +++ /dev/null @@ -1,64 +0,0 @@ -function [bestmot, ind, bestd] = ransacfocallength(imseq, iteration, threshold) -% [mot,ind]=ransacfocallength(imdata,iteration,threshold) -% Calculate the motion of two views using ransac based on the 6pts in 2 -% views minimal case for cameras with a common unknown focallength. -% Input: -% imseq - cell of IMAGEDATA -% iteration - iterations in RANSAC (optional) -% threshold - for inliers in relative magnitude (optional) -% Output: -% mot - motion -% ind - indices of used points -% bestd - reprojection errors -% Default: iteration = 100, threshold = 0.01 (relative); - -if nargin < 2, - iteration = 100; -end; -if nargin < 3 - threshold = 0.01; -end - -xx1 = pflat(getpoints(imseq{1})); -xx2 = pflat(getpoints(imseq{2})); - -x1 = xx1(1:2,:); -x2 = xx2(1:2,:); -scale = mean(abs([x1(:); x2(:)])); -threshold = scale * threshold; - -n1 = size(xx1,2); -consistent = 0; - -for i = 1 : iteration, - % draw a random permutation and select the first 6 points - per=randperm(n1); - x1 = xx1(:, per(1 : 6)); - x2 = xx2(:, per(1 : 6)); - - % solve mincase - mm = focallength6pt({imagedata([], x1), imagedata([], x2)}); - - for j = 1 : length(mm) - mot = mm{j}; - % triangulate & reproject - str = intsec2views(mot, imseq); - im1 = project(str, mot, 1); - y1 = getpoints(im1); - r1 = sum((y1 - xx1).^2); - im2 = project(str, mot, 2); - y2 = getpoints(im2); - r2 = sum((y2 - xx2).^2); - res = r1 + r2; - - % find inliers - index = find(res < threshold^2); - - if length(index) > consistent, - consistent = length(index); - ind = index; - bestd = sum(res); - bestmot = mot; - end - end -end \ No newline at end of file diff --git a/visionary/readpgm.m b/visionary/readpgm.m deleted file mode 100644 index aaa8ef0..0000000 --- a/visionary/readpgm.m +++ /dev/null @@ -1,66 +0,0 @@ -function image = readpgm(filename) ; - -% READPGM Reads a grayscale image file in pgm-format. -% READPGM('filename.pgm') returns a matrix containing the -% pixel values in the file PLUS ONE, since pgm-files -% contains numbers 0..255 and Matlab works best with -% numbers in the range 1..256. -% -% -% -% See also SAVEPGM. - -if nargin < 1, error('Too few input arguments.'); end -if ~isstr(filename), error('Argument must be a string.'); end - - -frid = fopen(filename,'r'); -if frid == -1, error('Can not open file.'); end -idline = fgetl(frid) ; -if idline == -1, error('File is empty.'); end -if idline(1) ~= 'P', error('This is not a pbm, pgm or ppm-file.') ; end -if idline(2) ~= '5' & idline(2) ~= '2', error('This is not a pgm-file.') ; end - -%disp('Warning, new version, see "help readpgm".'); - -whline = fgetl(frid); -if length(whline) == 0, whline(1) = '#'; end -while whline(1) == '#', - whline = fgetl(frid); - if length(whline) == 0, whline(1) = '#'; end -end -[vek, count] = sscanf(whline,' %d',2); -width = vek(1); - -%special fix by KAHL! -if count==1, - whline = fgetl(frid); - if length(whline) == 0, whline(1) = '#'; end - while whline(1) == '#', - whline = fgetl(frid); - if length(whline) == 0, whline(1) = '#'; end - end - [vek, count] = sscanf(whline,' %d',1); - height = vek(1); -else %normal case - height = vek(2); -end - - - -whline = fgetl(frid); -if length(whline) == 0, whline(1) = '#'; end -while whline(1) == '#', - whline = fgetl(frid); - if length(whline) == 0, whline(1) = '#'; end -end -[range, count] = sscanf(whline,' %d',1); - -if idline(2) == '5', - [image,count] = fread(frid,[width height],'uchar') ; -elseif idline(2) == '2', - [image,count] = fscanf(frid,' %d ',[width height]); -end - -image = image'+1; % Add one to avoid zeroes in Matlab -fclose(frid); diff --git a/visionary/reproject.m b/visionary/reproject.m deleted file mode 100644 index ca76ba7..0000000 --- a/visionary/reproject.m +++ /dev/null @@ -1,55 +0,0 @@ -function reproject(s,m,imseq,v,option); -% REPROJECT reproject(s,m,imseq,v,option) reprojects structure s -% with motion m and compares to imseq -% v - view . If not specified, the whole sequence is displayed -% option: -% 'numbered' - points are numbered -% -% See also RMSPOINTS - -if ~isa(s,'structure'), - s=structure(s); -end -if ~isa(m,'motion'), - m=motion(m); -end - -if nargin <=3 | isempty(v), - vs = 1:size(m); -else - vs = v; -end -if nargin<5, - option=''; -end - - -for i=vs; - imreproj=project(s,m,i); - clf; - if isa(imseq{i},'imagedata'), - imdata=imseq{i}; - else - imdata=imagedata([],imseq{i}); - end - plot(imdata); - [p,l,c]=getcommonfeatures({imdata,imreproj}); - imreproj=imagedata([],getpoints(imreproj,p),getlines(imreproj,l),getconics(imreproj,c)); - hold on; zoom on; - plot(imreproj,'ro','r-.','r-.'); - if strcmp(option,'numbered'), - pts=pflat(getpoints(imreproj)); - ax=axis; - for pp=1:size(pts,2);dev=0.01*ax(2); - h=text(pts(1,pp)-dev,pts(2,pp)-dev,num2str(p(pp))); - set(h,'Fontsize',11);set(h,'Color','b'); - end - end - if i ~= vs(length(vs)), - disp(['View ' num2str(i) '. Press space.']); - pause; - else - disp(['View ' num2str(i) '.']); - end - hold off; -end diff --git a/visionary/resec3pts.m b/visionary/resec3pts.m deleted file mode 100644 index 7850891..0000000 --- a/visionary/resec3pts.m +++ /dev/null @@ -1,351 +0,0 @@ -function m=resec3pts(imdata,s); -% RESEC3PTS m=resec3pts(imseq,s) resection, 3-method (4 solutions) -% INPUT: -% imdata - imagedata object -% s - structure -% OUTPUT: -% m - motion object, i.e. resected cameras - -u=getpoints(imdata); -U=pflat(s); - -t=mean(U(1:3,:)')'; -scale = max(abs(U(:))); - -T = diag([1/scale,1/scale,1/scale,1]);T(1:3,4)=-t/scale; - -Up = T*U; - - -[llist,pdata]=resection(u,Up); - -for ii = 1:length(pdata), - pdata{ii} = pdata{ii}*T*scale; -end - -m=motion(pdata); - - - - -function [llist,pdata]=resection(u,U); -% [llist,pdata,pindex]=resection(u,U); -% Ber�knar djup lambda och kameramatris P -% givet 3 punkter som kolonner i en matris U -% och 3 bildpunkter som kolonner i en matris u. -% Endast de l�sningar som ger positiva och reella djup returneras. -% Resultatet best�r av en matris llist d�r varje rad inneh�ller -% en m�jlig djupvektor lambda. pdata,pindex �r en lista -% med motsvarande projektionsmatriser P, s� att -% u*diag(lambda)=P*U; -% -% Copyright Kalle �str�m 1996-12-13. - -%[pdata,pindex]=nyml; -pdata=cell(0,1); - -if 0, - % exempel p� slumpm�ssigt genererad indata. - U=rand(3,3); - U=pextend(U); - [u,s,v]=svd(rand(3,3)); - if det(u)<0, u=u*diag([1 1 -1]); end; - P=[u rand(3,1)]; - [u,l]=psphere(P*U); - l1=l(1) - l2=l(2) - l3=l(3) -end; - -Udiff=[U(1:3,2:3)-U(1:3,[1 1])]; -G=Udiff'*Udiff; %Ber�kna grammianen f�r objektspunkterna. -G11=G(1,1); G12=G(1,2); G22=G(2,2); %Tre invarianta tal. -ug=u'*u; %Ber�kna grammianen f�r bildpunkterna. -u11=ug(1,1); -u22=ug(2,2); -u33=ug(3,3); -u23=ug(2,3); -u12=ug(1,2); -u13=ug(1,3); - -% Ans�tt djupen l1, l2 och l3 som obekanta. -% F�ljande ekvationer skall vara uppfyllda -% t0 = l2^2*u22+l1^2*u11-2*l1*l2*u12-G11 -% t0 = l3^2*u33+l1^2*u11-2*l1*l3*u13-G22 -% t0 = l3*l2*u23+l1^2*u11-l1*l2*u12-l1*l3*u13-G12 - -% eliminera l2 och l3 s� f�r man ett 8-gradspolynom -% med endast j�mna termer l1^8, l1^6, l1^4, l1^2, 1. -l1poly=zeros(1,5); -t1 = u11^2; -t2 = t1^2; -t3 = u23^2; -t4 = t3^2; -t6 = u22^2; -t8 = u13^2; -t9 = t8^2; -t11 = t1*u11; -t13 = t8*t3; -t15 = t1*u22; -t21 = t8*u33; -t23 = u12^2; -t48 = u33^2; -t55 = t23^2; -l1poly(1) = t2*t4+t1*t6*t9+2*t11*u22*t13-4*t15*t8*u13*u12*u23-2*t11*t6*t21+2*t15*t21*t23-4*t11*t3*u23*u12*u13-2*t2*t3*u33*u22+2*t11*t3*u33*t23+4*t1*t23*t13+4*t11*u12*u13*u23*u33*u22-4*t1*t23*u12*u13*u23*u33+t48*t6*t2-2*t48*u22*t11*t23+t48*t1*t55; -t1 = u11^2; -t2 = t1*u12; -t3 = u23^2; -t4 = t3*u23; -t10 = u13^2; -t11 = t10*u13; -t23 = u12^2; -t30 = t10^2; -t34 = t1*u11; -t36 = t3*u33; -t37 = t36*u22; -t39 = t10*G11; -t40 = t23^2; -t43 = u22^2; -t45 = t10*u33; -t48 = t3^2; -t49 = t34*t48; -t54 = G12*u22; -t58 = t23*u11; -t61 = u33^2; -t62 = t61*u22; -t63 = t1*G11; -t66 = t23*u12; -t69 = u23*u33; -t73 = u11*u22*t10; -t75 = u33*t23*G12; -t82 = G22*u12*u13; -t88 = t1*u22; -t91 = -2*t49*G11+8*t30*t23*t54+2*t34*G11*t37-12*t39*t58*t3-2*t62*t63*t23+4*u11*t66*u13*t69*G12-16*t73*t75+6*t73*u33*G11*t23+8*t1*t4*t82+4*t34*t3*u33*u22*G12-2*t88*t39*t3; -t102 = t61*u11; -t112 = G22*t66; -t117 = u11*G12; -t124 = 2*u11*t43*t30*G22-4*t61*t43*t34*G12+8*t62*t1*t23*G12-4*t102*t40*G12+2*t102*t40*G11+8*t11*G11*t66*u23-4*t82*t69*t88+4*t112*u13*t69*u11-4*t30*t43*t117-4*G22*t40*t45-4*G22*t23*t30*u22; -t158 = 8*t112*t11*u23+8*t10*t40*G12*u33+16*t10*t23*t117*t3-16*t11*t66*G12*u23-4*t63*u23*u12*u33*u22*u13-2*G22*t1*t36*t23+8*t2*u13*t4*G11-12*t58*t10*t3*G22-4*t2*u13*t69*t54-2*G22*t43*t45*t1-4*t1*t3*t75; -l1poly(2) = -4*t2*G12*t4*u13+4*u11*G11*u23*u12*t11*u22+4*t11*u22*u11*G12*u12*u23+6*G22*u22*t10*u33*u11*t23-4*t10*u22*t1*G12*t3-4*t30*G11*t23*u22+2*G22*t34*t37-4*t39*t40*u33+8*t1*t43*t45*G12-2*t49*G22+t91+t124+t158; -t1 = G22*G11; -t3 = u13^2; -t4 = t3*u13; -t5 = u12*t4; -t10 = u23^2; -t11 = t10*u23; -t12 = t11*u13; -t15 = u11*u12; -t19 = u33^2; -t20 = u22^2; -t22 = u11^2; -t23 = G12^2; -t28 = u12^2; -t30 = u33*t28*G12; -t32 = t3^2; -t35 = G22^2; -t39 = G22*t20; -t42 = G22*u22; -t47 = t35*u22; -t53 = t15*u13; -t54 = u23*u33; -t58 = -4*t1*u23*t5*u22+4*G22*u12*G12*t12*u11+4*t15*G12*t12*G11+6*t19*t20*t22*t23+4*G22*u11*t10*t30+4*t32*t20*t23+4*t35*t28*t10*t3-4*t39*t32*G12+12*t42*t4*G12*u12*u23-4*t47*t5*u23+2*t47*t3*u11*t10+4*t53*t54*u22*t23; -t62 = t3*u22; -t63 = t23*u33; -t68 = u33*G11; -t73 = G12*G11; -t74 = t73*t10; -t80 = t28*u12; -t85 = t10^2; -t87 = G11^2; -t91 = t28^2; -t93 = t19*t91; -t97 = G22*t28; -t98 = t3*G11; -t105 = -12*t53*t11*G22*G11+12*t62*t63*t28+8*t42*u11*u13*t68*u23*u12-8*t3*t28*t74+4*u11*t28*t10*t23*u33-8*u13*t80*t63*u23+t35*t22*t85+t22*t87*t85+t19*t87*t91+4*t93*t23+t35*t20*t32+12*t97*t98*t10-4*u13*t87*t80*u33*u23; -t108 = u33*u22; -t109 = t108*G12; -t113 = t11*u12*u13; -t115 = u11*t87; -t130 = t42*t3; -t143 = -4*t98*t28*t109-4*t35*u11*t113-4*t115*t113-2*t1*t10*t108*t22-4*G22*t80*u13*t54*G11-4*t22*G11*t10*t109-8*t97*G12*t10*t3-2*t130*t68*t28-4*t130*G12*u11*t10+2*t115*t10*u33*t28-2*t22*t10*t108*t23-4*t93*t73; -t146 = t19*u22; -t153 = t23*u11; -t184 = 4*t62*u11*t74+4*t146*u11*t73*t28+4*t3*t87*t28*t10+4*t62*t153*t10+4*t22*t85*t1-10*u11*t20*t3*u33*t23-8*t4*u22*t23*u12*u23-4*t130*t30-4*u11*G11*t10*t30-10*t146*t153*t28+12*u13*G11*t80*u33*G12*u23-4*G22*t22*t10*t109+4*t39*t3*u33*u11*G12; -l1poly(3) = t58+t105+t143+t184; -t1 = G22*u22; -t6 = u33*G11*u23*u12; -t8 = u12^2; -t10 = G12^2; -t11 = u23^2; -t15 = G22*u12; -t17 = t11*u23; -t19 = t17*u13*G11; -t21 = u13^2; -t23 = t10*G11; -t26 = u33^2; -t27 = t26*u22; -t28 = t10*G12; -t31 = u13*u22; -t38 = G11^2; -t39 = G22*t38; -t45 = u33*u22; -t46 = t45*t10; -t53 = -8*t1*u13*G12*t6-4*G22*t8*t10*t11*u33-4*t15*G12*t19-4*t21*u22*t23*t11+4*t27*t28*t8-4*t31*t28*u33*u12*u23+4*t31*t10*t6+4*t39*t17*u12*u13+2*u11*G11*t11*t46+2*G22*u11*t11*t46-2*t27*t23*t8; -t54 = G22^2; -t59 = u22^2; -t75 = G22*G11*t11; -t93 = t11^2; -t99 = -2*t54*u22*t21*G11*t11+4*t21*t59*t28*u33+4*t54*u12*t19+4*t1*t21*G12*G11*t11+4*t15*u13*u23*u33*u22*t10+4*t75*u33*t8*G12-2*t39*t11*u33*t8+4*t75*t45*u11*G12-2*G22*t59*t21*u33*t10-4*t26*t59*u11*t28-2*u11*t38*t93*G22-2*t54*u11*t93*G11; -l1poly(4) = t53+t99; -t1 = G22^2; -t2 = G11^2; -t4 = u23^2; -t5 = t4^2; -t7 = u33^2; -t8 = u22^2; -t10 = G12^2; -t11 = t10^2; -l1poly(5) = t1*t2*t5+t7*t8*t11-2*G22*G11*t4*u33*u22*t10; - -%L�s fj�rdegradspolynomer i (l1^2) -l1list=roots(l1poly); -l1list=l1list(find(imag(l1list)==0)); -l1list=l1list(find(l1list>0)); -l1list=sqrt(l1list); -l1list=l1list'; -l2list=[]; -l3list=[]; - -% �tersubstituera -% L�s l2. -for l1=l1list; -t1 = l1^2; -t2 = t1^2; -t3 = t2*u11; -t4 = u13^2; -t6 = t4*u13*u22; -t8 = u11^2; -t9 = t2*t8; -t10 = u23^2; -t14 = t4*u12*u23; -t16 = u33*u22; -t19 = u12^2; -t23 = t1*u11; -t25 = u13*G11*t10; -t27 = t23*u33; -t29 = u22*u13*G12; -t35 = G11*u23*u12; -t37 = t1*G22; -t41 = u13*t1; -t46 = u33*G11*t19; -t49 = G11*t10; -t51 = u13*u33; -t52 = G12^2; -t53 = u22*t52; -t58 = G12*t1; -t68 = -2*t37*t14+t41*G22*u11*t10-t41*t46-u13*G22*t49-t51*t53+2*u33*u12*t52*u23-2*t58*t6-2*t58*u13*u11*t10+4*t58*t14+2*G12*u13*t49-2*G12*u33*t35; -t71 = u23*t2; -t75 = t10*u23; -t78 = u12*u13*t10; -t87 = u23*t1; -t95 = u22*u11; -t103 = t2*u12; -t114 = t1*u12; -t118 = t71*u22*t4*u11+t9*t75-4*t3*t78-t71*t16*t8+t71*u33*u11*t19-t23*G11*t75-t87*G22*u22*t4-t37*u11*t75+2*t37*t78+2*t87*u33*t95*G12-t87*t46+G22*G11*t75-u23*u33*t53-2*t103*t6+4*t2*t19*t4*u23+2*t103*u33*t95*u13-2*t2*t19*u12*t51+2*t114*t25-2*t114*u33*t29; -l2 = -l1*(t3*t6+t9*u13*t10-2*t3*t14-t9*t16*u13+t3*u33*t19*u13-t23*t25+2*t27*t29-2*t27*u12*G12*u23+2*t27*t35+t37*t6+t68)/t118; - -% �tersubstituera -% L�s l3. -t1 = l1^2; -t2 = t1^2; -t3 = t2*u11; -t4 = u13^2; -t7 = u11^2; -t8 = t2*t7; -t9 = u23^2; -t14 = t1*u11; -t18 = t1*t4; -t21 = u11*t9; -t24 = t1*u13; -t29 = u33*u22; -t33 = G12^2; -t36 = u12^2; -t42 = u33*G11; -t45 = t3*u22*t4+t8*t9-2*t3*u12*u13*u23-t14*G11*t9-G22*u22*t18-G22*t1*t21+2*G22*u12*t24*u23+G22*G11*t9-t29*t8+2*t29*t14*G12-t29*t33+u33*t2*u11*t36-2*u33*t36*t1*G12+t42*t36*t1; -t52 = u12*u23; -t54 = t1*u33; -l3 = -t45/l1/(-t1*t4*u13*u22-t24*t21+2*t18*t52+t54*u22*u11*u13-t54*t36*u13+u13*G11*t9-t29*u13*G12+u33*u12*G12*u23-t42*t52)/2; - -l2list=[l2list l2]; -l3list=[l3list l3]; - -% Ber�kna R och t s� att -% R*U+t=u*diag([l1 l2 l3]); -% Eventuellt genom att transformera till en kanonisk bas -%%% -ut=u*diag([l1 l2 l3]); -Ut=U(1:3,:); -t1=Ut(1:3,1); -t2=ut(1:3,1); -Ut=Ut(1:3,:)-t1*ones(1,3); -ut=ut-t2*ones(1,3); -[R1,s,v]=svd(Ut(1:3,2)); -if det(R1)<0, R1=R1*diag([1 1 -1]); end; -[R2,s,v]=svd(ut(1:3,2)); -if det(R2)<0, R2=R2*diag([1 1 -1]); end; -if norm((R1'*Ut(1:3,2)-R2'*ut(1:3,2)))>0.001, - R2=R2*diag([-1 -1 1]); -end -Ut=R1'*Ut; -ut=R2'*ut; -[R11,s,v]=svd(Ut(2:3,3)); -if det(R11)<0, R11=R11*diag([1 -1]); end; %corrected by Kahl -[R22,s,v]=svd(ut(2:3,3)); -if det(R22)<0, R22=R22*diag([1 -1]); end; %corrected by Kahl -if norm((R11'*Ut(2:3,3)-R22'*ut(2:3,3)))>0.001, - R22=R22*diag([-1 -1]); -end -R11=[[1 0 0]; [0;0] R11]; -R22=[1 0 0; [0;0] R22]; -Ut=R11'*Ut; -ut=R22'*ut; -T1=[eye(3) (-t1);zeros(1,3) 1]; -RR=[R2*R22*R11'*R1' zeros(3,1); zeros(1,3) 1]; -T2=[eye(3) (t2);zeros(1,3) 1]; -T=T2*RR*T1; -PP=T(1:3,:); -pdata{end+1}=PP; - -end; - -llist=[l1list' l2list' l3list']; - - - -% maplekod. -%ekv1 := l2^2*u22+l1^2*u11-2*l1*l2*u12 - G11; -%ekv2 := l3^2*u33+l1^2*u11-2*l1*l3*u13 - G22; -%ekv3 := l3*l2*u23+l1^2*u11-l1*l2*u12-l1*l3*u13 - G12; -%sols := solve({ekv1,ekv2,ekv3},{l1,l2,l3}); -%l1f :=sols[3]: -%l1p :=op(2,l1f): -%l1p := op(1,l1p): -%l1p := subs(_Z=l1,l1p); -%readlib(C); -%fortran(coeff(l1p,l1,8),optimized); -%fortran(coeff(l1p,l1,7)); -%fortran(coeff(l1p,l1,6)); -%fortran(coeff(l1p,l1,5)); -%fortran(coeff(l1p,l1,4)); -%fortran(coeff(l1p,l1,3)); -%fortran(coeff(l1p,l1,2)); -%fortran(coeff(l1p,l1,1)); -%fortran(coeff(l1p,l1,0)); -%l2f :=sols[1]: -%l2f := subs(%1=l1,l2f): -%l2p := op(2,l2f): -%fortran(l2p,optimized); -%l3f :=sols[4]: -%l3f := subs(%1=l1,l3f): -%l3p := op(2,l3f): -%fortran(l3p,optimized); diff --git a/visionary/reseccam3.m b/visionary/reseccam3.m deleted file mode 100644 index a66f0b0..0000000 --- a/visionary/reseccam3.m +++ /dev/null @@ -1,372 +0,0 @@ -function P3=reseccam3(mot,imseq,index,ptsindex,option); -% P3=reseccam3(mot,imseq,index,ptsindex,option) -% Given two cameras and point correspondences across three views, -% the third camera is calculated linearly -% Input: -% mot - MOTION (with at least two cameras) -% imseq - cell of IMAGEDATA (with at least three views) -% index - the indices of the three view (optional) -% ptsindex - the indices of points to use (optional) -% option: -% 'nocoordtrans' - No image coordinate transformation -% Output: -% P3 - third camera matrix -% Default: 'index=[1 2 3]'. - -if nargin<3, - index=[1,2,3]; -end -if nargin<4, - ptsindex=1:size(imseq{1},1); -end - -if nargin<5, - option=''; -end - -if strcmp(option,'nocoordtrans'), - T1=eye(3); - T2=eye(3); - T3=eye(3); -else - T1=getnormtrans(imseq{index(1)}); - T2=getnormtrans(imseq{index(2)}); - T3=getnormtrans(imseq{index(3)}); -end - -pts1=sqrt(3)*psphere(T1*getpoints(imseq{index(1)},ptsindex)); -pts2=sqrt(3)*psphere(T2*getpoints(imseq{index(2)},ptsindex)); -pts3=sqrt(3)*psphere(T3*getpoints(imseq{index(3)},ptsindex)); - -goodpts=find(~isnan(pts1(1,:)+pts1(2,:)+pts1(3,:))); - -P1=T1*getcameras(mot,index(1));P1=sqrt(2)*P1/norm(P1); -P2=T2*getcameras(mot,index(2));P2=sqrt(2)*P2/norm(P2); - - - -p11=P1(1,1);p12=P1(1,2);p13=P1(1,3);p14=P1(1,4); -p21=P1(2,1);p22=P1(2,2);p23=P1(2,3);p24=P1(2,4); -p31=P1(3,1);p32=P1(3,2);p33=P1(3,3);p34=P1(3,4); -q11=P2(1,1);q12=P2(1,2);q13=P2(1,3);q14=P2(1,4); -q21=P2(2,1);q22=P2(2,2);q23=P2(2,3);q24=P2(2,4); -q31=P2(3,1);q32=P2(3,2);q33=P2(3,3);q34=P2(3,4); - -N=zeros(9*length(goodpts),12); - -for i=1:length(goodpts); - - x11=pts1(1,goodpts(i));x12=pts1(2,goodpts(i));x13=pts1(3,goodpts(i)); - x21=pts2(1,goodpts(i));x22=pts2(2,goodpts(i));x23=pts2(3,goodpts(i)); - x31=pts3(1,goodpts(i));x32=pts3(2,goodpts(i));x33=pts3(3,goodpts(i)); - - j=(i-1)*9+1; - - - t1 = p22*p33; - t2 = x11*x22; - t3 = t2*q14; - t5 = p12*p23; - t6 = x13*x22; - t7 = t6*q14; - t9 = p12*q23; - t10 = x12*x21; - t11 = t10*p34; - t13 = q22*p33; - t14 = t10*p14; - t16 = p22*q23; - t17 = x13*x21; - t18 = t17*p14; - t20 = q12*p33; - t21 = x12*x22; - t22 = t21*p14; - t24 = p22*q13; - t25 = t2*p34; - t27 = p32*p23; - t28 = x11*x21; - t29 = t28*q24; - t31 = p32*q13; - t32 = t2*p24; - t34 = q22*p23; - t35 = t28*p34; - t38 = p32*p13; - t39 = t21*q14; - t41 = p32*q23; - t42 = t28*p24; - t44 = t10*q24; - t46 = q22*p13; - t49 = t17*p24; - t52 = t1*t3+t5*t7-t9*t11-t13*t14-t16*t18+t20*t22-t24*t25+t27*t29+t31*t32-t34*t35-t31*t22+t38*t39-t41*t42-t38*t44+t46*t11+t13*t42-t46*t49-t27*t3; - t53 = q12*p23; - t54 = t6*p14; - t59 = p22*p13; - t60 = t17*q24; - t66 = p12*q13; - t67 = t6*p24; - t71 = p12*p33; - t74 = q12*p13; - t75 = t21*p34; - t80 = -t53*t54+t24*t54+t9*t49+t16*t35+t59*t60-t5*t60-t1*t29-t20*t32-t59*t7-t66*t67+t34*t18+t41*t14-t71*t39+t71*t44-t74*t75+t66*t75+t53*t25+t74*t67; - t81 = t52+t80; - t82 = x32*t81; - t83 = p11*q13; - t85 = q11*p33; - t87 = p11*p33; - t89 = p31*p13; - t91 = p11*q23; - t93 = q11*p23; - t95 = q21*p23; - t99 = q21*p13; - t101 = p31*p23; - t103 = p31*q23; - t106 = p31*q13; - t108 = q21*p33; - t111 = p21*q13; - t113 = p21*p33; - t115 = -t83*t67+t85*t22-t87*t39+t89*t39-t91*t11-t93*t54+t95*t18-t95*t35-t85*t32-t99*t49-t101*t3-t103*t42+t101*t29+t106*t32+t108*t42-t108*t14-t111*t25-t113*t29; - t117 = p21*p13; - t119 = p11*p23; - t129 = p21*q23; - t131 = q11*p13; - t138 = t113*t3+t117*t60+t119*t7-t119*t60+t91*t49+t83*t75-t106*t22+t87*t44+t99*t11+t103*t14+t93*t25-t129*t18+t131*t67-t131*t75+t111*t54-t89*t44+t129*t35-t117*t7; - t139 = t115+t138; - t140 = x32*t139; - t141 = p32*q21; - t143 = p12*p21; - t146 = q12*p31; - t148 = q12*p21; - t150 = p22*p31; - t152 = p22*p11; - t155 = q22*p21; - t157 = p12*p31; - t159 = q12*p11; - t162 = q22*p11; - t164 = p32*q11; - t168 = p32*p21; - t170 = p32*p11; - t172 = -t141*t42+t143*t7+t141*t14+t146*t22-t148*t54-t150*t29-t152*t7-t146*t32-t155*t35+t157*t44+t159*t67+t150*t3-t162*t49-t164*t22+t162*t11-t159*t75+t168*t29-t170*t44; - t173 = p12*q21; - t177 = q22*p31; - t182 = p12*q11; - t186 = p22*q11; - t189 = p22*q21; - t196 = -t173*t11+t173*t49+t148*t25+t177*t42-t177*t14-t168*t3-t157*t39-t182*t67+t182*t75-t143*t60+t186*t54+t155*t18+t189*t35+t170*t39-t186*t25+t152*t60-t189*t18+t164*t32; - t197 = t172+t196; - t198 = x32*t197; - t199 = t21*p11; - t201 = t28*q21; - t203 = x11*p31; - t204 = t203*x22; - t206 = x12*p31; - t207 = t206*x22; - t209 = t10*q21; - t212 = x11*q11*x22; - t214 = t17*p11; - t217 = x11*p21; - t218 = t217*x22; - t220 = t6*p11; - t223 = x12*q11*x22; - t225 = x13*p21; - t226 = t225*x21; - t228 = t10*p11; - t231 = x13*q11*x22; - t233 = t217*x21; - t235 = t206*x21; - t238 = t203*x21; - t240 = t20*t199+t27*t201+t53*t204-t74*t207+t71*t209+t1*t212-t16*t214-t31*t199-t20*t218+t24*t220-t71*t223+t9*t226-t13*t228+t5*t231+t13*t233-t9*t235+t34*t214-t34*t238; - t241 = t225*x22; - t256 = t17*q21; - t261 = t74*t241-t66*t241+t16*t238-t38*t209-t53*t220+t31*t218+t41*t228-t41*t233-t46*t226+t66*t207-t24*t204-t1*t201+t46*t235+t38*t223-t5*t256-t59*t231-t27*t212+t59*t256; - t262 = t240+t261; - t263 = x32*t262; - t264 = x31*t81; - t265 = x31*t139; - t266 = x31*t197; - t267 = x31*t262; - t268 = x33*t81; - t269 = x33*t139; - t270 = x33*t197; - t271 = x33*t262; - t272 = t10*q34; - t274 = x12*x23; - t275 = t274*p34; - t277 = q32*p33; - t279 = t28*q34; - t281 = p32*q33; - t283 = p12*q33; - t285 = x13*x23; - t286 = t285*p14; - t288 = q32*p23; - t291 = x11*x23; - t292 = t291*p34; - t294 = t291*p24; - t296 = t274*p14; - t298 = p22*q33; - t300 = q32*p13; - t304 = t291*q14; - t307 = t38*t272+t74*t275-t277*t42+t1*t279-t281*t14+t283*t11-t24*t286+t288*t35-t288*t18-t53*t292+t20*t294+t31*t296+t298*t18+t300*t49+t53*t286-t31*t294+t27*t304+t277*t14; - t308 = t17*q34; - t311 = t274*q14; - t316 = t285*q14; - t318 = t285*p24; - t330 = t5*t308-t59*t308+t71*t311-t1*t304-t298*t35-t66*t275+t59*t316+t66*t318-t27*t279+t24*t292-t71*t272-t283*t49-t5*t316-t20*t296-t300*t11+t281*t42-t38*t311-t74*t318; - t331 = t307+t330; - t332 = x32*t331; - t333 = p31*q33; - t339 = q31*p23; - t341 = q31*p33; - t343 = p11*q33; - t355 = -t333*t14+t83*t318-t101*t279-t83*t275+t93*t286-t339*t18+t341*t14+t343*t11+t119*t308-t343*t49-t87*t272+t106*t296+t333*t42-t89*t311+t101*t304-t111*t286+t87*t311-t117*t308; - t359 = p21*q33; - t367 = q31*p13; - t376 = t117*t316+t113*t279+t111*t292-t359*t35+t359*t18+t89*t272+t131*t275-t119*t316-t85*t296-t341*t42+t367*t49-t367*t11-t113*t304-t131*t318-t106*t294+t85*t294+t339*t35-t93*t292; - t377 = t355+t376; - t378 = x32*t377; - t380 = p32*q31; - t384 = q32*p21; - t387 = q32*p31; - t398 = p22*q31; - t401 = t186*t292-t380*t14-t159*t318+t159*t275-t384*t18+t384*t35+t387*t14+t143*t308-t182*t275+t150*t279-t150*t304-t164*t294+t380*t42+t164*t296-t146*t296+t170*t272-t398*t35+t148*t286; - t402 = q32*p11; - t413 = p12*q31; - t422 = t402*t49-t402*t11+t168*t304+t146*t294-t168*t279-t170*t311-t152*t308-t387*t42-t157*t272+t182*t318-t413*t49+t413*t11-t143*t316+t157*t311-t148*t292-t186*t286+t152*t316+t398*t18; - t423 = t401+t422; - t424 = x32*t423; - t427 = t291*p21; - t430 = t274*p11; - t432 = t291*q11; - t434 = t10*q31; - t436 = t285*p21; - t438 = t274*q11; - t442 = t274*p31; - t444 = t291*p31; - t446 = t285*q11; - t449 = t285*p11; - t453 = -t300*t226-t288*t238+t31*t427+t288*t214-t31*t430+t1*t432-t38*t434-t66*t436-t71*t438+t283*t226-t298*t214+t66*t442-t24*t444-t59*t446+t298*t238+t24*t449+t277*t233-t277*t228; - t454 = t17*q31; - t460 = t28*q31; - t474 = -t5*t454-t283*t235+t71*t434+t5*t446+t281*t228-t1*t460+t20*t430-t53*t449+t27*t460+t53*t444+t74*t436-t74*t442-t20*t427-t27*t432-t281*t233+t38*t438+t300*t235+t59*t454; - t475 = t453+t474; - t476 = x32*t475; - t477 = x31*t331; - t478 = x31*t377; - t479 = x31*t423; - t480 = x31*t475; - t481 = x33*t331; - t482 = x33*t377; - t483 = x33*t423; - t484 = x33*t475; - t486 = t291*q24; - t488 = t274*q24; - t491 = t2*q34; - t493 = t6*q34; - t495 = t285*q24; - t501 = t21*q34; - t509 = -t300*t75-t1*t486+t71*t488+t9*t318+t1*t491-t59*t493+t59*t495+t298*t54-t16*t286-t5*t495+t283*t75-t71*t501-t9*t275-t281*t22+t16*t292+t38*t501-t277*t32+t46*t275; - t528 = -t34*t292-t288*t54-t41*t294+t281*t32-t38*t488+t277*t22+t300*t67+t41*t296+t5*t493-t46*t318-t283*t67+t34*t286-t27*t491+t27*t486-t13*t296+t13*t294+t288*t25-t298*t25; - t529 = t509+t528; - t530 = x32*t529; - t549 = t343*t75-t339*t54-t113*t486+t117*t495-t89*t488-t103*t294-t333*t22+t339*t25-t129*t286+t113*t491+t359*t54+t95*t286-t95*t292-t108*t296+t99*t275+t129*t292-t341*t32+t101*t486; - t568 = -t117*t493-t99*t318-t101*t491-t359*t25+t108*t294-t87*t501+t89*t501-t367*t75+t103*t296-t91*t275-t343*t67+t119*t493+t367*t67-t119*t495+t87*t488+t91*t318+t341*t22+t333*t32; - t569 = t549+t568; - t570 = x32*t569; - t589 = t402*t67-t380*t22-t170*t488-t384*t54+t413*t75+t157*t488+t384*t25-t141*t294+t170*t501-t402*t75-t152*t493-t398*t25+t398*t54-t150*t486+t150*t491-t168*t491+t155*t286+t177*t294; - t608 = -t162*t318+t162*t275-t189*t286+t189*t292+t168*t486+t141*t296-t155*t292-t387*t32+t387*t22-t177*t296-t157*t501+t143*t493-t143*t495-t173*t275-t413*t67+t152*t495+t380*t32+t173*t318; - t609 = t589+t608; - t610 = x32*t609; - t611 = t285*q21; - t619 = t6*q31; - t623 = t274*q21; - t626 = t21*q31; - t630 = t2*q31; - t634 = t5*t611+t46*t436-t277*t199+t13*t430+t300*t207+t34*t444-t34*t449-t5*t619-t9*t436+t288*t220-t71*t623-t13*t427+t71*t626+t59*t619+t298*t204-t1*t630-t300*t241+t16*t449; - t637 = t291*q21; - t654 = -t46*t442+t9*t442-t27*t637-t59*t611-t16*t444+t1*t637-t298*t220-t281*t218-t38*t626-t288*t204+t41*t427+t38*t623+t281*t199-t41*t430+t283*t241-t283*t207+t277*t218+t27*t630; - t655 = t634+t654; - t656 = x32*t655; - t657 = x31*t529; - t658 = x31*t569; - t659 = x31*t609; - t660 = x31*t655; - t661 = x33*t529; - t662 = x33*t569; - t663 = x33*t609; - t664 = x33*t655; - N(j,1) = t82; - N(j,2) = -t140; - N(j,3) = -t198; - N(j,4) = -t263; - N(j,5) = -t264; - N(j,6) = t265; - N(j,7) = t266; - N(j,8) = t267; - N(j+1,1) = t268; - N(j+1,2) = -t269; - N(j+1,3) = -t270; - N(j+1,4) = -t271; - N(j+1,9) = -t264; - N(j+1,10) = t265; - N(j+1,11) = t266; - N(j+1,12) = t267; - N(j+2,5) = t268; - N(j+2,6) = -t269; - N(j+2,7) = -t270; - N(j+2,8) = -t271; - N(j+2,9) = -t82; - N(j+2,10) = t140; - N(j+2,11) = t198; - N(j+2,12) = t263; - N(j+3,1) = -t332; - N(j+3,2) = t378; - N(j+3,3) = t424; - N(j+3,4) = -t476; - N(j+3,5) = t477; - N(j+3,6) = -t478; - N(j+3,7) = -t479; - N(j+3,8) = t480; - N(j+4,1) = -t481; - N(j+4,2) = t482; - N(j+4,3) = t483; - N(j+4,4) = -t484; - N(j+4,9) = t477; - N(j+4,10) = -t478; - N(j+4,11) = -t479; - N(j+4,12) = t480; - N(j+5,5) = -t481; - N(j+5,6) = t482; - N(j+5,7) = t483; - N(j+5,8) = -t484; - N(j+5,9) = t332; - N(j+5,10) = -t378; - N(j+5,11) = -t424; - N(j+5,12) = t476; - N(j+6,1) = -t530; - N(j+6,2) = t570; - N(j+6,3) = t610; - N(j+6,4) = -t656; - N(j+6,5) = t657; - N(j+6,6) = -t658; - N(j+6,7) = -t659; - N(j+6,8) = t660; - N(j+7,1) = -t661; - N(j+7,2) = t662; - N(j+7,3) = t663; - N(j+7,4) = -t664; - N(j+7,9) = t657; - N(j+7,10) = -t658; - N(j+7,11) = -t659; - N(j+7,12) = t660; - N(j+8,5) = -t661; - N(j+8,6) = t662; - N(j+8,7) = t663; - N(j+8,8) = -t664; - N(j+8,9) = t530; - N(j+8,10) = -t570; - N(j+8,11) = -t610; - N(j+8,12) = t656; - -end; - -[U,D,V]=svd(N); - -P3=inv(T3)*reshape(V(:,12),4,3)';P3=P3/norm(P3); diff --git a/visionary/reseclinear.m b/visionary/reseclinear.m deleted file mode 100644 index b855ae8..0000000 --- a/visionary/reseclinear.m +++ /dev/null @@ -1,65 +0,0 @@ -function m=reseclinear(imseq,s,v,option); -% RESECLINEAR m=reseclinear(imseq,s,v,option) resection, linear method using points -% INPUT: -% imseq - imagedata array -% s - structure -% v - views (If not specified all views are resected) -% option: 'nocoordtrans' - No coordinate transformation -% OUTPUT: -% m - motion, i.e. resected cameras - -if nargin <=2 | isempty(v), - v=1:length(imseq); -end -if nargin <=3, - option = ''; -elseif ~strcmp('nocoordtrans',option), - error('Unknown option'); -end -m=motion; % create empty motion object -U = psphere(getpoints(s)); -good=isfinite(U(1,:)); - -for i=v; - if strcmp('nocoordtrans',option), - T=eye(3); - else - T = getnormtrans(imseq{i}); - end - u = psphere(T*getpoints(imseq{i})); - good2=isfinite(u(1,:)); - index=find(good & good2); - p=inv(T)*camerafromuandU(u(:,index),U(:,index)); - p=sign(det(p(:,1:3)))*p/norm(p); - m=addcameras(m,p); -end; - -%end of main function - -function p=camerafromuandU(u,U); -% p=camerafromuandU(u,U) - calculates p -% given 3D object points in homogeneous coordinates U -% and 2D image points in homogeneous coordinates u - -nbrpoints=size(U,2); - -M=[]; -j=0; -for i=1:nbrpoints; - if sum(~isfinite(u(:,i)))==0, - Up=[U(:,i)',zeros(1,8);zeros(1,4),U(:,i)',zeros(1,4);zeros(1,8),U(:,i)']; - M=[M,zeros(j*3,1);Up,zeros(3,j),u(:,i)]; - j=j+1; - end -end - -[uq,sq,vq]=svd(M); -w=vq(1:12,12+j); -p = [w(1:4)';w(5:8)';w(9:12)']; - - - - - - - diff --git a/visionary/rmslines.m b/visionary/rmslines.m deleted file mode 100644 index 1eb9799..0000000 --- a/visionary/rmslines.m +++ /dev/null @@ -1,46 +0,0 @@ -function tmp=rmslines(s,m,imseq,v); -% RMSLINES rmslines(s,m,imseq,v) reproject structure s -% with motion m and calculate Root Mean Square (RMS) error for lines in images -% Residuals are distance of project 3D end points to line. -% INPUT: -% s - structure -% m - motion -% imseq - cell list of imagedata objects -% v - view . If not specified, the whole sequence is displayed -% -% See also REPROJECT - -if nargin <=3, - vs = 1:size(m); -else - vs = v; -end -nbrvs=length(vs); -rms=zeros(1,nbrvs); -nbr=zeros(1,nbrvs); -for i=vs; - imreproj=project(s,m,i); - lll=gethomogeneouslines(imseq{i}); - reendpts=getlines(imreproj); - reendpts(1:3,:)=pflat(reendpts(1:3,:)); - reendpts(4:6,:)=pflat(reendpts(4:6,:)); - lindex=find(~isnan(lll(1,:)) & ~isnan(reendpts(1,:))); - nbrlll=length(lindex); - err=zeros(1,2*length(lindex)); - for jj=1:length(lindex); - err(2*jj-1:2*jj)=lll(:,lindex(jj))'/norm(lll(1:2,lindex(jj)))*[reendpts(1:3,lindex(jj)),reendpts(4:6,lindex(jj))]; - end - - rms(i)=sum(err.^2); - tmp=sqrt(rms(i)/(2*nbrlll)); - if nargout==0, - disp(['View ',num2str(i),', RMS ',num2str(tmp),' --- Number of lines: ',num2str(nbrlll)]); - end - nbr(i)=nbrlll; -end -tmp=sqrt(sum(rms)/(2*sum(nbr))); -if nargout ==0, - disp(['Total RMS ',num2str(tmp),' --- Number of lines: ',num2str(sum(nbr))]); -end - - diff --git a/visionary/rmspoints.m b/visionary/rmspoints.m deleted file mode 100644 index 4df3aec..0000000 --- a/visionary/rmspoints.m +++ /dev/null @@ -1,54 +0,0 @@ -function tmp=rmspoints(s,m,imseq,v); -% RMSPOINTS rmspoints(s,m,imseq,v) reproject structure s -% with motion m and calculate Root Mean Square (RMS) error for points in images -% INPUT: -% s - structure -% m - motion -% imseq - cell list of imagedata objects -% v - view . If not specified, the whole sequence is displayed -% -% See also REPROJECT - -if ~isa(s,'structure'), - s=structure(s); -end -if ~isa(m,'motion'), - m=motion(m); -end - -if nargin <=3 | isempty(v), - vs = 1:size(m); -else - vs = v; -end -nbrvs=length(vs); -rms=zeros(1,nbrvs); -nbr=zeros(1,nbrvs); -res=[]; -for i=vs; - imreproj=project(s,m,i); - if isa(imseq{i},'imagedata'), - imdata=imseq{i}; - else - imdata=imagedata([],imseq{i}); - end - - pts=pflat(imdata); - repts=pflat(imreproj); - pindex=find(~isnan(pts(1,:)) & ~isnan(repts(1,:))); - nbrpts=length(pindex); - err=repts(1:2,pindex)-pts(1:2,pindex); - res=[res;err(:)]; - - rms(i)=sum(sum(err.^2)); - tmp=sqrt(rms(i)/(2*nbrpts)); - if nargout==0, - disp(['View ',num2str(i),', RMS ',num2str(tmp),' --- Number of points: ',num2str(nbrpts)]); - end - nbr(i)=nbrpts; -end -tmp=sqrt(sum(rms)/(2*sum(nbr))); -if nargout ==0, - disp(['Total RMS ',num2str(tmp),' --- Number of points: ',num2str(sum(nbr))]); - clf;plot(res,'.'); -end diff --git a/visionary/rot2quat.m b/visionary/rot2quat.m deleted file mode 100644 index ab47d31..0000000 --- a/visionary/rot2quat.m +++ /dev/null @@ -1,18 +0,0 @@ -function q=rot2quat(R); -% function q=rot2quat(R); -% Rotation matrix R to unit quaternion q - -[slask,index]=max(diag(R)); -if index==1, - ii=[1,2,3]; -elseif index==2, - ii=[2,3,1]; -else - ii=[3,1,2]; -end -r=sqrt(1+sum(diag(R))); -q=zeros(4,1); -q(1)=r/2; -q(ii(1)+1)=(R(ii(3),ii(2))-R(ii(2),ii(3)))/2/r; -q(ii(2)+1)=(R(ii(1),ii(3))-R(ii(3),ii(1)))/2/r; -q(ii(3)+1)=(R(ii(2),ii(1))-R(ii(1),ii(2)))/2/r; diff --git a/visionary/rq.m b/visionary/rq.m deleted file mode 100644 index 29d2593..0000000 --- a/visionary/rq.m +++ /dev/null @@ -1,19 +0,0 @@ -function [r,q]=rq(a) -% RQ [r,q]=rq(a) factorises a such a=rq where r upper tri. and q unit matrix -% If a is not square, then q is equal q=[q1 q2] where q1 is unit matrix - -[m,n]=size(a); -e=eye(m); -p=e(:,[m:-1:1]); -[q0,r0]=qr(p*a(:,1:m)'*p); - -r=p*r0'*p; -q=p*q0'*p; - -fix=diag(sign(diag(r))); -r=r*fix; -q=fix*q; - -if n>m - q=[q, inv(r)*a(:,m+1:n)]; -end diff --git a/visionary/skew.m b/visionary/skew.m deleted file mode 100644 index 541aa04..0000000 --- a/visionary/skew.m +++ /dev/null @@ -1,5 +0,0 @@ -function A=skew(a); -% A=skew(a) - returns skew matrix y such that a x v = Av for any v -% - -A=[0,-a(3),a(2);a(3),0,-a(1);-a(2),a(1),0]; diff --git a/visionary/sm6points.m b/visionary/sm6points.m deleted file mode 100644 index 140b7fc..0000000 --- a/visionary/sm6points.m +++ /dev/null @@ -1,113 +0,0 @@ -function [scell,mcell]=sm6points(imseq,index); -% [scell,mcell]=sm6points(imseq,index); -% Calculates the real solutions (1 or 3) of the minimal case -% of 6 points in 3 views -% Input: -% imseq - cell of 3 IMAGEDATA objects -% index - indices of points to use -% Output: -% scell - cell of structures -% mcell - cell of motions - -if nargin<2, - index=1:6; -end; - - -% change image coordinate systems - -b1 = getpoints(imseq{1},index(1:4))'; -b2 = getpoints(imseq{2},index(1:4))'; -b3 = getpoints(imseq{3},index(1:4))'; - - -M0=zeros(12,4);M0(1)=-1;M0(18)=-1;M0(35)=-1;M0(40)=-1;M0(44)=-1;M0(48)=-1; - - -M1=zeros(12,9);M1(1:4,1:3)=b1;M1(5:8,4:6)=b1;M1(9:12,7:9)=b1; -M2=zeros(12,9);M2(1:4,1:3)=b2;M2(5:8,4:6)=b2;M2(9:12,7:9)=b2; -M3=zeros(12,9);M3(1:4,1:3)=b3;M3(5:8,4:6)=b3;M3(9:12,7:9)=b3; - -v=null([M1 M0]); -imT1=reshape(v(1:9),3,3)'; -v=null([M2 M0]); -imT2=reshape(v(1:9),3,3)'; -v=null([M3 M0]); -imT3=reshape(v(1:9),3,3)'; - -tmp1=svd(imT1); -tmp2=svd(imT2); -tmp3=svd(imT3); - -scell=cell(0); -mcell=cell(0); - -if tmp1(3)>1e-8 & tmp2(3)>1e-8 & tmp3(3)>1e-8, -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - - -c1=psphere(imT1*getpoints(imseq{1},index(5:6))); -c2=psphere(imT2*getpoints(imseq{2},index(5:6))); -c3=psphere(imT3*getpoints(imseq{3},index(5:6))); - - -N=[c1(1,1)*(c1(3,2)-c1(2,2)),c1(2,1)*(c1(1,2)-c1(3,2)),... - c1(3,1)*(c1(2,2)-c1(1,2)),c1(3,2)*(c1(2,1)-c1(1,1)),... - c1(2,2)*(c1(1,1)-c1(3,1));... - c2(1,1)*(c2(3,2)-c2(2,2)),c2(2,1)*(c2(1,2)-c2(3,2)),... - c2(3,1)*(c2(2,2)-c2(1,2)),c2(3,2)*(c2(2,1)-c2(1,1)),... - c2(2,2)*(c2(1,1)-c2(3,1));... - c3(1,1)*(c3(3,2)-c3(2,2)),c3(2,1)*(c3(1,2)-c3(3,2)),... - c3(3,1)*(c3(2,2)-c3(1,2)),c3(3,2)*(c3(2,1)-c3(1,1)),... - c3(2,2)*(c3(1,1)-c3(3,1))]; -v=null(N); - -poly=[v(1,2)*v(2,2)*v(5,2)-v(2,2)*v(3,2)*v(5,2)-v(2,2)*v(4,2)*v(5,2)+v(2,2)*v(3,2)*v(4,2)-v(1,2)*v(3,2)*v(4,2)+v(3,2)*v(4,2)*v(5,2),... -v(1,1)*v(2,2)*v(5,2)+v(1,2)*v(2,2)*v(5,1)+v(1,2)*v(2,1)*v(5,2)-v(2,1)*v(3,2)*v(5,2)-v(2,2)*v(3,1)*v(5,2)-v(2,2)*v(3,2)*v(5,1)-v(2,2)*v(4,1)*v(5,2)-v(2,1)*v(4,2)*v(5,2)-v(1,1)*v(3,2)*v(4,2)-v(2,2)*v(4,2)*v(5,1)-v(1,2)*v(3,1)*v(4,2)+v(2,2)*v(3,1)*v(4,2)+v(2,1)*v(3,2)*v(4,2)-v(1,2)*v(3,2)*v(4,1)+v(2,2)*v(3,2)*v(4,1)+v(3,2)*v(4,1)*v(5,2)+v(3,1)*v(4,2)*v(5,2)+v(3,2)*v(4,2)*v(5,1),... -v(1,1)*v(2,1)*v(5,2)-v(2,1)*v(3,1)*v(5,2)+v(1,2)*v(2,1)*v(5,1)+v(1,1)*v(2,2)*v(5,1)-v(2,1)*v(4,1)*v(5,2)-v(2,2)*v(3,1)*v(5,1)-v(2,1)*v(3,2)*v(5,1)-v(2,2)*v(4,1)*v(5,1)-v(2,1)*v(4,2)*v(5,1)-v(1,1)*v(3,1)*v(4,2)+v(2,1)*v(3,1)*v(4,2)-v(1,2)*v(3,1)*v(4,1)-v(1,1)*v(3,2)*v(4,1)+v(2,1)*v(3,2)*v(4,1)+v(2,2)*v(3,1)*v(4,1)+v(3,1)*v(4,2)*v(5,1)+v(3,1)*v(4,1)*v(5,2)+v(3,2)*v(4,1)*v(5,1),... -v(3,1)*v(4,1)*v(5,1)-v(2,1)*v(4,1)*v(5,1)+v(2,1)*v(3,1)*v(4,1)-v(2,1)*v(3,1)*v(5,1)+v(1,1)*v(2,1)*v(5,1)-v(1,1)*v(3,1)*v(4,1)]; - -alfa=roots(poly); -if log(min([svd(imT1);svd(imT2);svd(imT3)]))>-10 & log(min(svd(N)))>-10, -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -for i=1:length(alfa), - if imag(alfa(i))==0, - t=v(:,1)+alfa(i)*v(:,2); - if t(1)~=t(2) & t(1)~=t(3) & t(2)~=t(3), - X=[(t(4)-t(5))/(t(2)-t(3));t(4)/(t(1)-t(3));t(5)/(t(1)-t(2));1]; - - A= [ c1(3,1),0,-c1(1,1),c1(3,1)-c1(1,1);... - 0,c1(3,1),-c1(2,1),c1(3,1)-c1(2,1);... - c1(3,2)*X(1),0,-c1(1,2)*X(3),c1(3,2)-c1(1,2);... - 0,c1(3,2)*X(2),-c1(2,2)*X(3),c1(3,2)-c1(2,2)]; - [u,s,w]=svd(A); - P1=[diag(w(1:3,4)),w(4,4)*ones(3,1)]; - - A= [ c2(3,1),0,-c2(1,1),c2(3,1)-c2(1,1);... - 0,c2(3,1),-c2(2,1),c2(3,1)-c2(2,1);... - c2(3,2)*X(1),0,-c2(1,2)*X(3),c2(3,2)-c2(1,2);... - 0,c2(3,2)*X(2),-c2(2,2)*X(3),c2(3,2)-c2(2,2)]; - [u,s,w]=svd(A); - P2=[diag(w(1:3,4)),w(4,4)*ones(3,1)]; - - A= [ c3(3,1),0,-c3(1,1),c3(3,1)-c3(1,1);... - 0,c3(3,1),-c3(2,1),c3(3,1)-c3(2,1);... - c3(3,2)*X(1),0,-c3(1,2)*X(3),c3(3,2)-c3(1,2);... - 0,c3(3,2)*X(2),-c3(2,2)*X(3),c3(3,2)-c3(2,2)]; - [u,s,w]=svd(A); - P3=[diag(w(1:3,4)),w(4,4)*ones(3,1)]; - mcell={mcell{:},motion({inv(imT1)*P1,inv(imT2)*P2,inv(imT3)*P3})}; - scell={scell{:},structure([eye(4),ones(4,1),X])}; - end %if t(1) - end -end; -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -end %if log... - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -end %if tmp1(3)>1e-8 & tmp2(3)>1e-8 & tmp3(3)>1e-8, diff --git a/visionary/smaffine.m b/visionary/smaffine.m deleted file mode 100644 index 48ccbcf..0000000 --- a/visionary/smaffine.m +++ /dev/null @@ -1,367 +0,0 @@ -function [s,m]=smaffine(imseq,option); -% [s,m]=smaffine(imseq,option) solves the structure and motion with affine cameras -% for points, lines and conics with factorisation. -% NB: Only features that are visible in all images are used. -% INPUT: -% imseq - cell list containing imagedata objects -% option: -% 'conic' - the quadrics are disk-quadrics, i.e. conics -% OUTPUT: -% s - structure -% m - motion -% -% See also: smaffineclos - -if nargin<=1, - option =''; -end - -nbrimages=length(imseq); -[p0,l0,c0]=getcommonfeatures(imseq); - -nbrpoints=length(p0); -nbrlines=length(l0); -nbrconics=length(c0); -nbrpc=nbrpoints+nbrconics; - -% Check for minimality conditions (NB: This is not done strictly) -if nbrpc<5, - if nbrpc<3 | nbrlines < 3; - error(['Too few visible features: ', ... - num2str(nbrpoints),' points ', ... - num2str(nbrlines),' lines ', ... - num2str(nbrconics),' conics']); - end -end - -trans=cell(1,nbrimages); -for i=1:nbrimages; - if isempty(strmatch('nocoordtrans',option)), - trans{i}=getnormtrans(imseq{i},'affine'); - else - trans{i}=eye(3); - end -end - -Utilde=[]; %notation according to Tomasi-Kanade -Vtilde=[]; -centroid = zeros(2,nbrimages); -%scale = ones(2,nbrimages); - -for i=1:nbrimages, - im=changecsystem(imseq{i},trans{i}); - - %points - if nbrpoints>0 - pts=pflat(getpoints(im,p0)); - pts = pts(1:2,:); - end - - %conics - if nbrconics>0, - u = getconics(im,c0); - u = pflat(u(4:6,:)); - if nbrpoints>0, - pts = [pts,u(1:2,:)]; - else - pts = u(1:2,:); - end - end - - %relative coordinates - centroid(:,i)=mean(pts')'; - utilde = pts(1,:) - centroid(1,i); - vtilde = pts(2,:) - centroid(2,i); -% scale(:,i) = [max(abs(utilde));max(abs(vtilde))]; -% utilde = utilde/scale(1,i); -% vtilde = vtilde/scale(2,i); - - %lines - if nbrlines>0, - uline = pflat(gethomogeneouslines(im,l0)); -% uline = diag([scale(:,i)',1])*pflat(gethomogeneouslines(im,l0)); - udir = normc([uline(2,:);-uline(1,:)]); - utilde = [utilde,udir(1,:)]; - vtilde = [vtilde,udir(2,:)]; - end - Utilde = [Utilde;utilde]; - Vtilde = [Vtilde;vtilde]; -end -if nbrlines>0, - %retrieve direction scale-factor - depth=rescalelines(Utilde,Vtilde,nbrimages,nbrpc,nbrlines); - index=nbrpc+[1:nbrlines]; - Utilde(:,index)=Utilde(:,index).*depth; - Vtilde(:,index)=Vtilde(:,index).*depth; -end - -W= [Utilde;Vtilde]; -[O1,sv,O2]=svd(W); -sv3=sv(1:3,1:3); -Rtilde = O1(:,1:3)*sqrt(sv3); -Stilde = sqrt(sv3)*O2(:,1:3)'; - -m = motion; -for i=1:nbrimages, - -% T = diag(scale(:,i)); -% pcam = [ T*[Rtilde(i,:);Rtilde(i+nbrimages,:)],... -% centroid(:,i);zeros(1,3),1 ]; -% pcam = inv(trans{i})*[ T*[Rtilde(i,:);Rtilde(i+nbrimages,:)],... -% centroid(:,i);zeros(1,3),1 ]; - pcam = inv(trans{i})*[ [Rtilde(i,:);Rtilde(i+nbrimages,:)],... - centroid(:,i);zeros(1,3),1 ]; - - m = addcameras(m,pcam); -end -tmp=NaN*ones(4,size(imseq{1},1)); -tmp(:,p0)=[Stilde(:,1:nbrpoints);ones(1,nbrpoints)]; -s=structure(tmp); -s=intseclinear(imseq,m,option,s); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% end of main-function -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function depth=rescalelines(Utilde,Vtilde,nbrimages,nbrpc,nbrlines) - -M=zeros(15*nbrimages,nbrimages,nbrlines); -total=nbrpc+nbrlines; - -for bildnr=1:nbrimages, - ind1=bildnr; - ind2=mod(ind1,nbrimages)+1; - ind3=mod(ind2,nbrimages)+1; - - Utemp=Utilde([ind1,ind2,ind3],:); - Vtemp=Vtilde([ind1,ind2,ind3],:); - [A0,A1,A2]=tritensor20(Utemp,Vtemp,nbrpc,nbrlines); - a=A0(1,1);b=A0(1,2);c=A0(1,3); - d=A0(2,1);e=A0(2,2);f=A0(2,3); - g=A1(1,1);h=A1(1,2);i=A1(1,3); - j=A1(2,1);k=A1(2,2);l=A1(2,3); - m=A2(1,1);n=A2(1,2);o=A2(1,3); - p=A2(2,1);q=A2(2,2);r=A2(2,3); - - for linenr=1:nbrlines, - u1=Utemp(1,nbrpc+linenr); - u2=Vtemp(1,nbrpc+linenr); - v1=Utemp(2,nbrpc+linenr); - v2=Vtemp(2,nbrpc+linenr); - w1=Utemp(3,nbrpc+linenr); - w2=Vtemp(3,nbrpc+linenr); - - row=bildnr*15-14; - col1=bildnr; - col2=mod(col1,nbrimages)+1; - col3=mod(col2,nbrimages)+1; - - M(row,col1,linenr)=a*h*u2*l-a*k*u2*i-d*h*u1*l+d*k*u1*i-g*b*u2*l+g*e*u1*l+g*k*c*u2-g*k* ... -u1*f+j*b*u2*i-j*e*u1*i-j*h*c*u2+j*h*u1*f; - M(row,col2,linenr)=a*e*i*v2-a*e*v1*l-a*h*f*v2+a*k*f*v1-d*b*i*v2+d*b*v1*l+d*h*c*v2-d*k*c ... -*v1+g*b*f*v2-g*e*c*v2-j*b*f*v1+j*e*c*v1; - - M(row+1,col1,linenr)=a*h*u2*o-a*n*u2*i-d*h*u1*o+d*n*u1*i-g*b*u2*o+g*e*u1*o+g*n*c*u2-g*n* ... -u1*f+m*b*u2*i-m*e*u1*i-m*h*c*u2+m*h*u1*f; - M(row+1,col2,linenr)=-a*e*v1*o+a*n*f*v1+d*b*v1*o-d*n*c*v1-m*b*f*v1+m*e*c*v1; - M(row+1,col3,linenr)=a*e*i*w1-a*h*f*w1-d*b*i*w1+d*h*c*w1+g*b*f*w1-g*e*c*w1; - - M(row+2,col1,linenr)=a*h*u2*r-a*q*u2*i-d*h*u1*r+d*q*u1*i-g*b*u2*r+g*e*u1*r+g*q*c*u2-g*q* ... -u1*f+p*b*u2*i-p*e*u1*i-p*h*c*u2+p*h*u1*f; - M(row+2,col2,linenr)=-a*e*v1*r+a*q*f*v1+d*b*v1*r-d*q*c*v1-p*b*f*v1+p*e*c*v1; - M(row+2,col3,linenr)=a*e*i*w2-a*h*f*w2-d*b*i*w2+d*h*c*w2+g*b*f*w2-g*e*c*w2; - - M(row+3,col1,linenr)=a*k*u2*o-a*n*u2*l-d*k*u1*o+d*n*u1*l-j*b*u2*o+j*e*u1*o+j*n*c*u2-j*n* ... -u1*f+m*b*u2*l-m*e*u1*l-m*k*c*u2+m*k*u1*f; - M(row+3,col2,linenr)=-a*e*v2*o+a*n*f*v2+d*b*v2*o-d*n*c*v2-m*b*f*v2+m*e*c*v2; - M(row+3,col3,linenr)=a*e*l*w1-a*k*f*w1-d*b*l*w1+d*k*c*w1+j*b*f*w1-j*e*c*w1; - - M(row+4,col1,linenr)=a*k*u2*r-a*q*u2*l-d*k*u1*r+d*q*u1*l-j*b*u2*r+j*e*u1*r+j*q*c*u2-j*q* ... -u1*f+p*b*u2*l-p*e*u1*l-p*k*c*u2+p*k*u1*f; - M(row+4,col2,linenr)=-a*e*v2*r+a*q*f*v2+d*b*v2*r-d*q*c*v2-p*b*f*v2+p*e*c*v2; - M(row+4,col3,linenr)=a*e*l*w2-a*k*f*w2-d*b*l*w2+d*k*c*w2+j*b*f*w2-j*e*c*w2; - - M(row+5,col1,linenr)=a*n*u2*r-a*q*u2*o-d*n*u1*r+d*q*u1*o-m*b*u2*r+m*e*u1*r+m*q*c*u2-m*q* ... -u1*f+p*b*u2*o-p*e*u1*o-p*n*c*u2+p*n*u1*f; - M(row+5,col3,linenr)=a*e*o*w2-a*e*w1*r-a*n*f*w2+a*q*f*w1-d*b*o*w2+d*b*w1*r+d*n*c*w2-d*q*c ... -*w1+m*b*f*w2-m*e*c*w2-p*b*f*w1+p*e*c*w1; - - M(row+6,col1,linenr)=-g*k*u1*o+g*n*u1*l+j*h*u1*o-j*n*u1*i-m*h*u1*l+m*k*u1*i; - M(row+6,col2,linenr)=-a*h*v2*o+a*k*v1*o+a*n*i*v2-a*n*v1*l+g*b*v2*o-g*n*c*v2-j*b*v1*o+j*n* ... -c*v1-m*b*i*v2+m*b*v1*l+m*h*c*v2-m*k*c*v1; - M(row+6,col3,linenr)=a*h*l*w1-a*k*i*w1-g*b*l*w1+g*k*c*w1+j*b*i*w1-j*h*c*w1; - - M(row+7,col1,linenr)=-g*k*u1*r+g*q*u1*l+j*h*u1*r-j*q*u1*i-p*h*u1*l+p*k*u1*i; - M(row+7,col2,linenr)=-a*h*v2*r+a*k*v1*r+a*q*i*v2-a*q*v1*l+g*b*v2*r-g*q*c*v2-j*b*v1*r+j*q* ... -c*v1-p*b*i*v2+p*b*v1*l+p*h*c*v2-p*k*c*v1; - M(row+7,col3,linenr)=a*h*l*w2-a*k*i*w2-g*b*l*w2+g*k*c*w2+j*b*i*w2-j*h*c*w2; - - M(row+8,col1,linenr)=-g*n*u1*r+g*q*u1*o+m*h*u1*r-m*q*u1*i-p*h*u1*o+p*n*u1*i; - M(row+8,col2,linenr)=a*n*v1*r-a*q*v1*o-m*b*v1*r+m*q*c*v1+p*b*v1*o-p*n*c*v1; - M(row+8,col3,linenr)=a*h*o*w2-a*h*w1*r-a*n*i*w2+a*q*i*w1-g*b*o*w2+g*b*w1*r+g*n*c*w2-g*q*c ... -*w1+m*b*i*w2-m*h*c*w2-p*b*i*w1+p*h*c*w1; - - M(row+9,col1,linenr)=-j*n*u1*r+j*q*u1*o+m*k*u1*r-m*q*u1*l-p*k*u1*o+p*n*u1*l; - M(row+9,col2,linenr)=a*n*v2*r-a*q*v2*o-m*b*v2*r+m*q*c*v2+p*b*v2*o-p*n*c*v2; - M(row+9,col3,linenr)=a*k*o*w2-a*k*w1*r-a*n*l*w2+a*q*l*w1-j*b*o*w2+j*b*w1*r+j*n*c*w2-j*q*c ... -*w1+m*b*l*w2-m*k*c*w2-p*b*l*w1+p*k*c*w1; - - M(row+10,col1,linenr)=-g*k*u2*o+g*n*u2*l+j*h*u2*o-j*n*u2*i-m*h*u2*l+m*k*u2*i; - M(row+10,col2,linenr)=-d*h*v2*o+d*k*v1*o+d*n*i*v2-d*n*v1*l+g*e*v2*o-g*n*f*v2-j*e*v1*o+j*n* ... -f*v1-m*e*i*v2+m*e*v1*l+m*h*f*v2-m*k*f*v1; - M(row+10,col3,linenr)=d*h*l*w1-d*k*i*w1-g*e*l*w1+g*k*f*w1+j*e*i*w1-j*h*f*w1; - - M(row+11,col1,linenr)=-g*k*u2*r+g*q*u2*l+j*h*u2*r-j*q*u2*i-p*h*u2*l+p*k*u2*i; - M(row+11,col2,linenr)=-d*h*v2*r+d*k*v1*r+d*q*i*v2-d*q*v1*l+g*e*v2*r-g*q*f*v2-j*e*v1*r+j*q* ... -f*v1-p*e*i*v2+p*e*v1*l+p*h*f*v2-p*k*f*v1; - M(row+11,col3,linenr)=d*h*l*w2-d*k*i*w2-g*e*l*w2+g*k*f*w2+j*e*i*w2-j*h*f*w2; - - M(row+12,col1,linenr)=-g*n*u2*r+g*q*u2*o+m*h*u2*r-m*q*u2*i-p*h*u2*o+p*n*u2*i; - M(row+12,col2,linenr)=d*n*v1*r-d*q*v1*o-m*e*v1*r+m*q*f*v1+p*e*v1*o-p*n*f*v1; - M(row+12,col3,linenr)=d*h*o*w2-d*h*w1*r-d*n*i*w2+d*q*i*w1-g*e*o*w2+g*e*w1*r+g*n*f*w2-g*q*f ... -*w1+m*e*i*w2-m*h*f*w2-p*e*i*w1+p*h*f*w1; - - M(row+13,col1,linenr)=-j*n*u2*r+j*q*u2*o+m*k*u2*r-m*q*u2*l-p*k*u2*o+p*n*u2*l; - M(row+13,col2,linenr)=d*n*v2*r-d*q*v2*o-m*e*v2*r+m*q*f*v2+p*e*v2*o-p*n*f*v2; - M(row+13,col3,linenr)=d*k*o*w2-d*k*w1*r-d*n*l*w2+d*q*l*w1-j*e*o*w2+j*e*w1*r+j*n*f*w2-j*q*f ... -*w1+m*e*l*w2-m*k*f*w2-p*e*l*w1+p*k*f*w1; - - M(row+14,col2,linenr)=g*n*v2*r-g*q*v2*o-j*n*v1*r+j*q*v1*o-m*h*v2*r+m*k*v1*r+m*q*i*v2-m*q* ... -v1*l+p*h*v2*o-p*k*v1*o-p*n*i*v2+p*n*v1*l; - M(row+14,col3,linenr)=g*k*o*w2-g*k*w1*r-g*n*l*w2+g*q*l*w1-j*h*o*w2+j*h*w1*r+j*n*i*w2-j*q*i ... -*w1+m*h*l*w2-m*k*i*w2-p*h*l*w1+p*k*i*w1; - end -end - -depth=zeros(nbrimages,nbrlines); -for linenr=1:nbrlines, - [U,S,V]=svd(M(:,:,linenr)); - depth(:,linenr)=V(:,nbrimages); -end - -function [A0,A1,A2]=tritensor20(Utilde,Vtilde,nbrpc,nbrlines); -%reconstuct A0,A1,A2 from three views - -M=zeros(15*nbrpc+nbrlines,20); -R=zeros(15,20); - -for i=1:nbrpc, - x1=Utilde(1,i); - x2=Vtilde(1,i); - y1=Utilde(2,i); - y2=Vtilde(2,i); - z1=Utilde(3,i); - z2=Vtilde(3,i); - - R(1,9) = y2; - R(1,10) = -y1; - R(1,13) = x2; - R(1,14) = -x1; - R(2,1) = x2; - R(2,5) = -x1; - R(2,9) = z1; - R(2,11) = -y1; - R(3,2) = x2; - R(3,6) = -x1; - R(3,9) = z2; - R(3,12) = -y1; - R(4,3) = x2; - R(4,7) = -x1; - R(4,10) = z1; - R(4,11) = -y2; - R(5,4) = x2; - R(5,8) = -x1; - R(5,10) = z2; - R(5,12) = -y2; - R(6,11) = z2; - R(6,12) = -z1; - R(6,17) = x2; - R(6,18) = -x1; - R(7,1) = -y2; - R(7,3) = y1; - R(7,13) = z1; - R(7,15) = -x1; - R(8,2) = -y2; - R(8,4) = y1; - R(8,13) = z2; - R(8,16) = -x1; - R(9,1) = z2; - R(9,2) = -z1; - R(9,17) = y1; - R(9,19) = -x1; - R(10,3) = z2; - R(10,4) = -z1; - R(10,17) = y2; - R(10,20) = -x1; - R(11,5) = -y2; - R(11,7) = y1; - R(11,14) = z1; - R(11,15) = -x2; - R(12,6) = -y2; - R(12,8) = y1; - R(12,14) = z2; - R(12,16) = -x2; - R(13,5) = z2; - R(13,6) = -z1; - R(13,18) = y1; - R(13,19) = -x2; - R(14,7) = z2; - R(14,8) = -z1; - R(14,18) = y2; - R(14,20) = -x2; - R(15,15) = z2; - R(15,16) = -z1; - R(15,19) = y2; - R(15,20) = -y1; - M(i*15-14:i*15,:)=R; -end - -linec=zeros(1,8); -for i=1:nbrlines, - u1=Utilde(1,nbrpc+i); - u2=Vtilde(1,nbrpc+i); - v1=Utilde(2,nbrpc+i); - v2=Vtilde(2,nbrpc+i); - w1=Utilde(3,nbrpc+i); - w2=Vtilde(3,nbrpc+i); - - linec(1) = -u2*v2*w2; - linec(2) = u2*v2*w1; - linec(3) = u2*v1*w2; - linec(4) = -u2*v1*w1; - linec(5) = u1*v2*w2; - linec(6) = -u1*v2*w1; - linec(7) = -u1*v1*w2; - linec(8) = u1*v1*w1; - - M(nbrpc*15+i,1:8)=linec; -end -[u,d,v]=svd(M); - -param20=v(:,20)/v(1,20); - -A0=zeros(2,3); -A1=zeros(2,3); -A2=zeros(2,3); -A0(1,1)=1; -A1(1,2)=1; -A2(1,3)=1; - -A0(2,1)=param20(5); -A0(2,2)=param20(11); -A0(2,3)=-param20(9); -A1(2,1)=-param20(15); -A1(2,2)=param20(3); -A1(2,3)=param20(13); -A2(2,1)=param20(19); -A2(2,2)=-param20(17); -A2(2,3)=param20(2); - -%change to good coordinate system -[u,s,v]=svd([A0;A1;A2]); -t=v*diag(1./diag(s)); -A0=A0*t; -A1=A1*t; -A2=A2*t; - - diff --git a/visionary/smaffineclos.m b/visionary/smaffineclos.m deleted file mode 100644 index e4e47b8..0000000 --- a/visionary/smaffineclos.m +++ /dev/null @@ -1,304 +0,0 @@ -function [s,m]=smaffineclos(imseq,option); -% [s,m]=smaffineclos(imseq,option) solves the structure and motion with affine cameras -% for points, lines and conics with closure constraints -% Missing data is handled automaticly -% INPUT: -% imseq - cell list containing imagedata objects -% option: -% 'conic' - the quadrics are disk-quadrics, i.e. conics -% 'nocoordtrans' - No coordinate transformation -% OUTPUT: -% s - structure -% m - motion -% -% See also: smaffine -if nargin<=1, - option =''; -end - -nbrimages=length(imseq); - -if nbrimages<3, - error('Too few images'); -end - -trans=cell(1,nbrimages); -for i=1:nbrimages; - if isempty(strmatch('nocoordtrans',option)), - trans{i}=getnormtrans(imseq{i},'affine'); - else - trans{i}=eye(3); - end -end -centers=zeros(6,0); -samecenter=zeros(1,nbrimages); -T=zeros(0,20); -i=0; -while i1, - if size(pi,2)==size(pilast,2) & size(ci,2)==size(cilast,2), - if sum(abs(pi-pilast))+sum(abs(ci-cilast))==0, - samecenter(i)=1; - end - end - else - pifirst=pi; - cifirst=ci; - end - - % Check for minimality conditions (NB: This is not done strictly) - if nbrpc<5, - if nbrpc<3 | nbrlines < 3; - if i<(nbrimages-1) %closed sequence???? - error(['Too few visible features: ', ... - num2str(nbrpoints),' points ', ... - num2str(nbrlines),' lines ', ... - num2str(nbrconics),' conics']); - else - i=nbrimages+1; % no closed sequence - end - end - end - if i<=nbrimages; - % get points and centers of conics and lines - if nbrpoints==0, - tmp=getconics(imseq{i},ci); - pts1=pflat(trans{i}*tmp(4:6,:)); - tmp=getconics(imseq{iplus1},ci); - pts2=pflat(trans{iplus1}*tmp(4:6,:)); - tmp=getconics(imseq{iplus2},ci); - pts3=pflat(trans{iplus2}*tmp(4:6,:)); - else - pts1=pflat(trans{i}*getpoints(imseq{i},pi)); - pts2=pflat(trans{iplus1}*getpoints(imseq{iplus1},pi)); - pts3=pflat(trans{iplus2}*getpoints(imseq{iplus2},pi)); - if nbrconics>0, - tmp=getconics(imseq{i},ci); - pts1=[pts1,pflat(trans{i}*tmp(4:6,:))]; - tmp=getconics(imseq{iplus1},ci); - pts2=[pts2,pflat(trans{iplus1}*tmp(4:6,:))]; - tmp=getconics(imseq{iplus2},ci); - pts3=[pts3,pflat(trans{iplus2}*tmp(4:6,:))]; - end - end - lines1=psphere(inv(trans{i})'*gethomogeneouslines(imseq{i},li)); - lines2=psphere(inv(trans{iplus1})'*gethomogeneouslines(imseq{iplus1},li)); - lines3=psphere(inv(trans{iplus2})'*gethomogeneouslines(imseq{iplus2},li)); - [t,c1,c2,c3]=pointslines2tri(pts1,pts2,pts3,lines1,lines2,lines3); - centers=[centers,[c1(1:2);c2(1:2);c3(1:2)]]; - T=[T;t]; - end %END of if i<=nbrimages - pilast=pi; - cilast=ci; -end %END of while - -% compare first and last centers -if size(pi,2)==size(pifirst,2) & size(ci,2)==size(cifirst,2), - if sum(abs(pi-pifirst))+sum(abs(ci-cifirst))==0, - samecenter(1)=1; - end -end - - -P=tri2p(T,nbrimages); - -A=cell(1,nbrimages); -for i=1:nbrimages; - index=2*i-1; - A{i}=P(index:index+1,:); -end - -%calculate last column in camera matrices -M=[]; -cindex=2+2*size(centers,2); -sq=1; -changed=0; -for i=[2:size(centers,2),1]; - if samecenter(i)==0 & i~=2, - cindex=cindex+3; - changed=1; - end - iplus1=mod(i,nbrimages)+1; - iplus2=mod(i+1,nbrimages)+1; - M(sq:sq+5,1)=-centers(:,i); - M(sq:sq+1,2*i:2*i+1)=eye(2); - M(sq+2:sq+3,2*iplus1:2*iplus1+1)=eye(2); - M(sq+4:sq+5,2*iplus2:2*iplus2+1)=eye(2); - if changed==1, - M(sq:sq+1,cindex:cindex+2)=A{i}; - M(sq+2:sq+3,cindex:cindex+2)=A{iplus1}; - M(sq+4:sq+5,cindex:cindex+2)=A{iplus2}; - end - - sq=sq+6; -end -[u,ss,v]=svd(M); -%tmp=abs(v(1,cindex-1:cindex+2)); -%bindex=cindex-2+find(max(tmp)==tmp); -%b=v(:,bindex)/v(1,bindex); - -b=v(:,size(v,2));b=b/b(1); - -m=motion; -for i=1:nbrimages; - P=[A{i},b(2*i:2*i+1);0 0 0 1]; - m=addcameras(m,inv(trans{i})*P); -end - -s=intseclinear(imseq,m,option); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% end of main-function -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [T,c1,c2,c3]=pointslines2tri(I1,I2,I3,L1,L2,L3); - -% [T,c1,c2,c3]=pointslines2tri(I1,I2,I3,L1,L2,L3); -% Calculates the third order affine quasi-tensor -% from point and line correspondences -c1=mean(I1')'; -c2=mean(I2')'; -c3=mean(I3')'; - -nbrpoints=size(I1,2); -I1=I1-c1*ones(1,nbrpoints); -I2=I2-c2*ones(1,nbrpoints); -I3=I3-c3*ones(1,nbrpoints); -nbrlines=size(L1,2); - -nofconstr=15*nbrpoints+nbrlines; -M=zeros(nofconstr,20); -pointnr=1; -constrnr=1; -while pointnr<=nbrpoints - M(constrnr,[11,5,2,1])=... - [I1(1,pointnr) -I1(2,pointnr) I2(1,pointnr) -I2(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[16,10,4,3])=... - [I1(1,pointnr) -I1(2,pointnr) I3(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[20,19,18,17])=... - [I2(1,pointnr) -I2(2,pointnr) I3(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[12,6,3,1])=... - [I1(1,pointnr) -I1(2,pointnr) I2(1,pointnr) -I3(1,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[13,7,4,1])=... - [I1(1,pointnr) -I1(2,pointnr) I2(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[14,8,3,2])=... - [I1(1,pointnr) -I1(2,pointnr) I2(2,pointnr) -I3(1,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[15,9,4,2])=... - [I1(1,pointnr) -I1(2,pointnr) I2(2,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[17,8,6,5])=... - [I1(1,pointnr) -I2(1,pointnr) I2(2,pointnr) -I3(1,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[18,9,7,5])=... - [I1(1,pointnr) -I2(1,pointnr) I2(2,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[19,10,7,6])=... - [I1(1,pointnr) -I2(1,pointnr) I3(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[20,10,9,8])=... - [I1(1,pointnr) -I2(2,pointnr) I3(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[17,14,12,11])=... - [I1(2,pointnr) -I2(1,pointnr) I2(2,pointnr) -I3(1,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[18,15,13,11])=... - [I1(2,pointnr) -I2(1,pointnr) I2(2,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[19,16,13,12])=... - [I1(2,pointnr) -I2(1,pointnr) I3(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - M(constrnr,[20,16,15,14])=... - [I1(2,pointnr) -I2(2,pointnr) I3(1,pointnr) -I3(2,pointnr)]; - constrnr=constrnr+1; - pointnr=pointnr+1; -end -linenr=1; -while linenr<=nbrlines -% M(constrnr,[6,7,8,9,12,13,14,15])=... -% [L1(2,linenr)*L2(2,linenr)*L3(2,linenr)... -% -L1(2,linenr)*L2(2,linenr)*L3(1,linenr)... -% -L1(2,linenr)*L2(1,linenr)*L3(2,linenr)... -% L1(2,linenr)*L2(1,linenr)*L3(1,linenr)... -% -L1(1,linenr)*L2(2,linenr)*L3(2,linenr)... -% L1(1,linenr)*L2(2,linenr)*L3(1,linenr)... -% L1(1,linenr)*L2(1,linenr)*L3(2,linenr)... -% -L1(1,linenr)*L2(1,linenr)*L3(1,linenr)]; - M(constrnr,[6,7,8,9,12,13,14,15])=... - [L1(1,linenr)*L2(1,linenr)*L3(1,linenr)... - L1(1,linenr)*L2(1,linenr)*L3(2,linenr)... - L1(1,linenr)*L2(2,linenr)*L3(1,linenr)... - L1(1,linenr)*L2(2,linenr)*L3(2,linenr)... - L1(2,linenr)*L2(1,linenr)*L3(1,linenr)... - L1(2,linenr)*L2(1,linenr)*L3(2,linenr)... - L1(2,linenr)*L2(2,linenr)*L3(1,linenr)... - L1(2,linenr)*L2(2,linenr)*L3(2,linenr)]; - constrnr=constrnr+1; - linenr=linenr+1; -end -[U,S,V]=svd(M); -[m,n]=size(V); -T=V(:,n)'; - -function P=tri2p(T,nbrimages); - -% P=tri2p(T,nbrimages); -% calculate camera matrices from the third order affine quasi-tensors -% between views i,i+1 and i+2 using the affine closure constraints - -noftri=12*size(T,1); -M=zeros(noftri,2*nbrimages); -imnr=1; -trinr=1; -while trinr 0)) - % Depths are positive so this is the right one. - P2 = PP{i}; - P1 = P; - break; - end -end - -% uncalibrate cameras -P1 = K1*P1; -P2 = K2*P2; - -mot = motion({P1, P2}); - -% triangulate -str = intsec2views(mot, imseq); - -function [f1,f2]=focallengths_inner(F,pp1,pp2); -%focal lengths from F given pp1,pp2 -%NB: Degenerate when principal axes intersect or equivalently -% pp1 and pp2 are corresponding points <=> pp1'*F*pp2=0 - -pp1 = pextend(pp1); -pp2 = pextend(pp2); - -%epipoles -e1=null(F'); -e2=null(F); - -E1=skew(e1); -E2=skew(e2); - -I2=diag([1,1,0]); - -numer1 = -pp2'*E2*I2*F'*pp1*pp1'*F*pp2; -denom1 = pp2'*E2*I2*F'*I2*F*pp2; -f1= sqrt(numer1/denom1); - -numer2 = -pp1'*E1*I2*F*pp2*pp2'*F'*pp1; -denom2 = pp1'*E1*I2*F*I2*F'*pp1; -f2 = sqrt(numer2/denom2); \ No newline at end of file diff --git a/visionary/smproj8p.m b/visionary/smproj8p.m deleted file mode 100644 index 417a2f2..0000000 --- a/visionary/smproj8p.m +++ /dev/null @@ -1,89 +0,0 @@ -function [s,m]=smproj8p(imseq,index); -% [s,m]=smproj8p(imseq,index) solves the projective structure and motion -% problem for image-point correspondences. -% INPUT: -% imseq - cell list containing imagedata objects -% index - reference to 2 view (optional) -% OUTPUT: -% s - structure -% m - motion -% NB: At least 8 points must be visible in all images - -if nargin<2, - index=[1,2]; -end - -% normalize coordinates for numerical conditioning -H1 = getnormtrans(imseq{1}); -H2 = getnormtrans(imseq{2}); - -p0=getcommonfeatures(imseq(index)); -nbrpoints=length(p0); - -if nbrpoints<8, - error('Too few points visible in all images'); -end - -x1 = getpoints(imseq{index(1)},p0); -x2 = getpoints(imseq{index(2)},p0); -u1 = H1*x1; -u2 = H2*x2; -[m,s]=sm3(u1,u2); -F = ptobi(m); -F = H1 * F * H2; % undo coordinate normalization -m = bitop(F); - -function [m,s]=sm3(u1,u2); -% [P1,P2,U,l1,l2,u1t,u2t]=sm3(u1,u2); -% Calculate structure and motion given -% at least 8 corresponding image points in two images -% Input: u1, u2 matrices of type 3x6. Each column represents -% a point. Corresponding columns in u1 and u2 are -% the image points of the same point in image 1 and 2. -% Output: P1, P2 - 3x4 camera matrices (camera position and orientation) -% U - 4x6 matrix representing the position of the six points. -% l1, l2 - depth vectors. -% u1t, u2t - reprojected points. -% We have u1t*diag(l1) = P1*U and -% and we hope to have u1t approximately equal to u1, -% and similarily for image 2. -% -% Copyright Fredrik Kahl, Kalle Åström 2006. - - -% The corresponding points -% give linear constraints on the -% 3x3 fundamental Matrix F - -% We are now going to construct a matrix ev -% containing these linear constraints on the -% parameters of the fundamental matrix F -% such that ev*F(:)=0. -ev=[]; -nr_of_points=size(u1,2); -if nr_of_points<8, - error('The routine sm3 requires at least 8 points'); -end; -for i=1:nr_of_points - v=u1(:,i)*u2(:,i)'; - ev=[ev v(:)]; -end; -[u,s,v]=svd(ev); -% Here one ought to test if the matrix ev has 8 sufficiently -% large singular values and that the last singular is sufficiently -% small -% If so then the last column of u in the singular value -% decomposition gives the vector uu that minimized norm(ev*u,'fro'); -F=reshape(u(:,9),3,3); - -% Make sure that F has rank 2 -[u s v] = svd(F); -s(end) = 0; -F = u*s*v'; - -% Also renormalize so that F has unit norm in the frobenius. -F=F/norm(F,'fro'); - -% compute motion and structure objects -m = bitop(F); -s = intsec2views(m, {imagedata([],u1), imagedata([],u2)}); \ No newline at end of file diff --git a/visionary/smshape.m b/visionary/smshape.m deleted file mode 100644 index a4c3235..0000000 --- a/visionary/smshape.m +++ /dev/null @@ -1,64 +0,0 @@ -function [s,m]=smshape(imseq); -% [s,m]=smshape(imseq) solves the structure and motion -% problem for points with a shape based factorisation method. -% INPUT: -% imseq - cell list containing imagedata objects -% OUTPUT: -% s - structure -% m - motion -% NB: All points must be visible in all images - -nbrimages=length(imseq); -p0=getcommonfeatures(imseq); -nbrpoints=length(p0); - -if nbrpoints<6, - error('Too few points visible in all images'); -end - -udata = zeros(nbrpoints,3*nbrimages); -for i=1:nbrimages; - u=getpoints(imseq{i},p0)'; - [u,s,v]=svd(u,0); - udata(:,(3*i-2):3*i)=u; -end; - -E=eye(nbrpoints); - -for ii=1:30; - %% Adjust U - [us,ss,vs]=svd(udata,0); - U=us(:,1:4); - %% Adjust depth - for jj=1:nbrimages; - ui=udata(:,(3*jj-2):3*jj); - Qu=E-U*U'; - M=[]; - for i=1:size(ui,2); - M=[M;Qu*diag(ui(:,i))]; - end; - [us,ss,vs]=svd(M'*M,0); - q=vs(:,nbrpoints); - q=q/sum(q); - q=sqrt(size(q,1))*q/norm(q); - - ui=diag(q)*ui; - [ui,ss,vs]=svd(ui,0); - - udata(:,(3*jj-2):3*jj)=ui; - end; -end; - -U=U'; -%WARNING This has to be fixed properly! -U=pflat(U([2 3 4 1],:)); -tmp=NaN*ones(4,size(imseq{1},1)); -tmp(:,p0)=U; -s=structure(tmp); -m=reseclinear(imseq,s); -s=intseclinear(m,imseq,[],s); %intersect remaining points and other features - -% end of main function - - - diff --git a/visionary/test_bundler.m b/visionary/test_bundler.m deleted file mode 100644 index a7fed75..0000000 --- a/visionary/test_bundler.m +++ /dev/null @@ -1,21 +0,0 @@ - - -%test-script -bibdir='C:\bundler\bundler-v0.3-binary\examples\ET'; - -[str,mot,Kmot,f,pp,kk, imseq0, imundist]=bundler_import(bibdir); - -figure(1);plot(imseq0{1},'numbered') -figure(2);plot(imseq0{2},'numbered') - -figure(3); -plot(mot,str);axis equal; - - -figure(4); -rmspoints(str,mot,imundist); -figure(5); -reproject(str,mot,imundist); - -% Do calibrated bundle adjustment -%[ss,mm]=bundleplc(str,mot, imundist, {'autocalib=11111'}); diff --git a/visionary/test_visualize.m b/visionary/test_visualize.m deleted file mode 100644 index 8f45a73..0000000 --- a/visionary/test_visualize.m +++ /dev/null @@ -1,61 +0,0 @@ - -%Test program for exporting files to visualizer -%There are two cases, first second example is synthetic example, -% the first example shows how to import from bundler and then export - -% CASE 1: import a scene from bundler and then export - -bibdir='C:\bundler\bundler-v0.3-binary\examples\ET'; -[str,mot,Kmot,f,pp,kk, imseq0, imundist, rgb]=bundler_import(bibdir); - -nbrcameras=size(mot); -imfiles=cell(1,nbrcameras); - -for ii=1:nbrcameras, - tmp=getfilename(imseq0{ii}); - imfiles{ii}=tmp(length(bibdir)+2:end); -end -visualize_export([bibdir,'\ET.out'],str,Kmot,imfiles,rgb); - -% CASE 2: generate a synthetic scene as well as images and then export -filename='synt.out'; %export filename -imagename='syntim'; %image filename - -nbrcameras=10; -nbrpoints=50; %random points -background=0.8; %background color -sx=300; %size of images -sy=200; - -rgb=rand(3,nbrpoints); -pp=[sx/2;sy/2]; %principal point -margin=2; %size of projected point in image - -[str,mot,imseq]=randomscene(nbrcameras,nbrpoints,0,0,{'noise=0','zeroprincipal'}); -T=diag([1/100,1/100,1/100,1]); -mot=changecsystem(mot,T); -str=changecsystem(str,T); - -T=diag([1/5,1/5,1]); -T(1:2,3)=pp; -mot=changecsystem(mot,T); -imnames=cell(1,nbrcameras); -for ii=1:nbrcameras, - imseq{ii}=changecsystem(imseq{ii},T); - im=background*ones(sy,sx,3); - - pt=round(pflat(project(str,mot,ii))); - index=find(pt(1,:)>margin & pt(1,:)margin & pt(2,:) 0); - proj(:,k) = pflat(U_proj); - inliers(k) = (norm(xlist(:,k)-proj(:,k)) < threshold) & inFront; - end - - if (nnz(inliers) > bestNumInliers) - bestInliers = inliers; - bestNumInliers = nnz(bestInliers); - bestU = U; - end - end - - U = bestU; - inliers = bestInliers; -end - diff --git a/visionary/trifocallinear.m b/visionary/trifocallinear.m deleted file mode 100644 index 51bb105..0000000 --- a/visionary/trifocallinear.m +++ /dev/null @@ -1,109 +0,0 @@ -function T=trifocallinear(imdata,index,points); -% function T=trifocallinear(imdata,index,points); -% Determines the trifofal tensor linearly using point correspondences -% -% Input: -% imdata - cell of IMAGEDATA -% index - indices of images to use -% points - indices of points to use -% Output: -% T - trifocal tensor - -if nargin<2, - index = [ 1 2 3]; -end; - -if nargin<3, - points=getcommonfeatures(imdata(index)); -end; - -imT1=getnormtrans(imdata{index(1)}); -imT2=getnormtrans(imdata{index(2)}); -imT3=getnormtrans(imdata{index(3)}); - -x1=imT1*getpoints(imdata{index(1)},points); -x2=imT2*getpoints(imdata{index(2)},points); -x3=imT3*getpoints(imdata{index(3)},points); - - -%kb jb kp jp k j -J=[1 1 3 3 2 2 1 - 1 1 3 2 2 3 -1 - 1 2 3 3 2 1 -1 - 1 2 3 1 2 3 1 - 1 3 3 1 2 2 -1 - 1 3 3 2 2 1 1 - - 2 1 1 3 3 2 1 - 2 1 1 2 3 3 -1 - 2 2 1 3 3 1 -1 - 2 2 1 1 3 3 1 - 2 3 1 1 3 2 -1 - 2 3 1 2 3 1 1 - - 3 1 2 3 1 2 1 - 3 1 2 2 1 3 -1 - 3 2 2 3 1 1 -1 - 3 2 2 1 1 3 1 - 3 3 2 1 1 2 -1 - 3 3 2 2 1 1 1 - - - 1 1 2 3 3 2 -1 - 1 1 2 2 3 3 1 - 1 2 2 3 3 1 1 - 1 2 2 1 3 3 -1 - 1 3 2 1 3 2 1 - 1 3 2 2 3 1 -1 - - 2 1 3 3 1 2 -1 - 2 1 3 2 1 3 1 - 2 2 3 3 1 1 1 - 2 2 3 1 1 3 -1 - 2 3 3 1 1 2 1 - 2 3 3 2 1 1 -1 - - 3 1 1 3 2 2 -1 - 3 1 1 2 2 3 1 - 3 2 1 3 2 1 1 - 3 2 1 1 2 3 -1 - 3 3 1 1 2 2 1 - 3 3 1 2 2 1 -1 - ]; - - - -M=zeros(9*size(points,2),27); -for n=1:size(points,2) - -for m=1:36 -kb=J(m,1);jb=J(m,2);kp=J(m,3);jp=J(m,4);k=J(m,5);j=J(m,6); -for i=1:3 - M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)=M(9*(n-1)+3*(jb-1)+kb,9*(i-1)+3*(j-1)+k)+x1(i,n)*x2(jp,n)*x3(kp,n)*J(m,7); - end - end - end - - [u,s,v]=svd(M); - t=v(:,27); - - -for i=1:3 - for j=1:3 - for k=1:3 - - T(i,j,k)=t(9*(i-1)+3*(j-1)+k); - - end - end -end - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% i ursprungliga bildkoordinater -m=tritop(T); -P1=getcameras(m,1); -P2=getcameras(m,2); -P3=getcameras(m,3); -m=motion({inv(imT1)*P1,inv(imT2)*P2,inv(imT3)*P3}); -T=ptotri(m); - - diff --git a/visionary/tritop.m b/visionary/tritop.m deleted file mode 100644 index 8b481a3..0000000 --- a/visionary/tritop.m +++ /dev/null @@ -1,64 +0,0 @@ -function m=tritop(T) -% m=tritop(T) -% Determines three possible camera matrices from the trifocal tensor T -% Input: T=trifocal tensor -% Output: [P1,P2,P3]=two camera matrices normalised such that -% P1=[I 0] P2(1,:)=[0 0 0 1] -% The 3 cameras are returned as MOTION m - -F12=tritobi(T); -[E12,E21]=bitoone(F12); -E2=E12./E12(1); - -F13=tritobi1(T,1,3); -[E13,E31]=bitoone(F13); -E3=E13./E13(1); - -P1=[1 0 0 0;0 1 0 0;0 0 1 0]; -P2=[... -0 0 0 1;... -F12(1,3) F12(2,3) F12(3,3) E2(2);... --F12(1,2) -F12(2,2) -F12(3,2) E2(3)]; - -P3=[... -T(1,1,1) T(2,1,1) T(3,1,1) E3(1);... -T(1,1,2) T(2,1,2) T(3,1,2) E3(2);... -T(1,1,3) T(2,1,3) T(3,1,3) E3(3)]; - -M=[... -T(1,2,1) P2(2,1)*P3(1,4) -P2(2,4)*P3(1,1);... -T(1,2,2) P2(2,1)*P3(2,4) -P2(2,4)*P3(2,1);... -T(1,2,3) P2(2,1)*P3(3,4) -P2(2,4)*P3(3,1);... -T(1,3,1) P2(3,1)*P3(1,4) -P2(3,4)*P3(1,1);... -T(1,3,2) P2(3,1)*P3(2,4) -P2(3,4)*P3(2,1);... -T(1,3,3) P2(3,1)*P3(3,4) -P2(3,4)*P3(3,1);... -T(2,2,1) P2(2,2)*P3(1,4) -P2(2,4)*P3(1,2);... -T(2,2,2) P2(2,2)*P3(2,4) -P2(2,4)*P3(2,2);... -T(2,2,3) P2(2,2)*P3(3,4) -P2(2,4)*P3(3,2);... -T(2,3,1) P2(3,2)*P3(1,4) -P2(3,4)*P3(1,2);... -T(2,3,2) P2(3,2)*P3(2,4) -P2(3,4)*P3(2,2);... -T(2,3,3) P2(3,2)*P3(3,4) -P2(3,4)*P3(3,2);... -T(3,2,1) P2(2,3)*P3(1,4) -P2(2,4)*P3(1,3);... -T(3,2,2) P2(2,3)*P3(2,4) -P2(2,4)*P3(2,3);... -T(3,2,3) P2(2,3)*P3(3,4) -P2(2,4)*P3(3,3);... -T(3,3,1) P2(3,3)*P3(1,4) -P2(3,4)*P3(1,3);... -T(3,3,2) P2(3,3)*P3(2,4) -P2(3,4)*P3(2,3);... -T(3,3,3) P2(3,3)*P3(3,4) -P2(3,4)*P3(3,3) -]; - -[U,S,V]=svd(M); -t=V(:,3); - -s=t(3)/t(2); -E3=E3./s; - -P3=[... -T(1,1,1) T(2,1,1) T(3,1,1) E3(1);... -T(1,1,2) T(2,1,2) T(3,1,2) E3(2);... -T(1,1,3) T(2,1,3) T(3,1,3) E3(3)]; - - -m=motion({P1,P2,P3}); - - - diff --git a/visionary/undist_radial.m b/visionary/undist_radial.m deleted file mode 100644 index 398d0aa..0000000 --- a/visionary/undist_radial.m +++ /dev/null @@ -1,49 +0,0 @@ -function imseq2 = undist_radial(imseq1,f,k) -%undistort all points in an image sequence -% INPUT: imseq1 - cell of imagedata objects OR cell of 2xN matrices -% f - vector of focal lengths -% k - 2xn matrix of radial distortion parameter -% OUTPUT: imseq2 - cell of imagedata objects -% -% MODEL: (x',y')=r(x,y)*(x,y) where r(x,y)=1+k(1)*(x^2+y^2)+k(2)*(x^2+y^2)^2 - -n=length(imseq1); -for ii=1:n, - if isa(imseq1{ii},'imagedata'), - tmp=pflat(getpoints(imseq1{ii}));tmp(3,:)=[]; - else - tmp=imseq1{ii}; - end - for jj=find(~isnan(tmp(1,:))); - tmp(:,jj)=calibrate(tmp(:,jj),f(ii),k(:,ii)); - end - imseq2{ii}=imagedata([],tmp); -end - - -function xc = calibrate(loc,f,k) - -% loc is the point location, f is the focal length, k are the radial distortion parameters. - -rdist = norm(loc/f); - -pol = [k(2) 0 k(1) 0 1 -rdist]; - -rt = roots(pol); - -rt = rt(abs(imag(rt)) 0); - -bb = abs(rt-rdist); - -[m,g] = min(bb); - -rt = rt(g); -if isempty(rt), - scaling=1; -else - scaling = rdist/rt; -end -xc = [loc/scaling/f]; - diff --git a/visionary/v2m.m b/visionary/v2m.m deleted file mode 100644 index e4c42e1..0000000 --- a/visionary/v2m.m +++ /dev/null @@ -1,18 +0,0 @@ -function m=v2m(v); -% V2M m=v2m(v) returns symmetric matrix of conic/quadric vector - -if size(v,1)==10, - m = [ v(1),v(2),v(4),v(7);... - v(2),v(3),v(5),v(8);... - v(4),v(5),v(6),v(9);... - v(7),v(8),v(9),v(10)]; - -elseif size(v,1)==6, - m = [ v(1),v(2),v(4);... - v(2),v(3),v(5);... - v(4),v(5),v(6)]; - -else - error('Wrong vector dimension'); -end - diff --git a/visionary/visualize_export.m b/visionary/visualize_export.m deleted file mode 100644 index 3800bd5..0000000 --- a/visionary/visualize_export.m +++ /dev/null @@ -1,107 +0,0 @@ -function visualize_export(filename,str,mot,imfiles,rgb,surface) -% function visualize_export(filename,str,mot,imfiles,rgb,surface) -% Export results to Visualize -% INPUT: -% filename - filename of exported file. -% Should always have extension '.out' -% str - structure -% mot - motion -% imfiles - filenames of images OR cell of imagedata objects -% rgb - 3xn matrix for point colors where n is the number of points -% Should be between 0-255. -% surface - triangulated 3D-surface -% - surface.vertices (nx3) containes a set of 3D-points -% - surface.faces (mx3) containes three indices (into the vertic list) -% for each triangle. -% - surface.facevertexcdata (mx3) containds the color (rgb) -% for each triangle -% -% See "test_visualize" for examples -% NB: The scale should be set so the maximum distance in 3D is around 10 units - -nbrcameras=size(mot); -nbrpoints=size(str,1); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%compute closest cameras -%ORDER: left,down,forward,right,up,backward -distthreshold=1; %maximum distance for a camera to be called "close" - -Pmot=getcameras(mot); -C=pflat(focalpoints(mot));C(4,:)=[]; -camindex=-ones(nbrcameras,6); -for ii=1:nbrcameras, - [K,P]=rq(Pmot{ii}); - R=P(:,1:3); - vC=C-C(:,ii)*ones(1,nbrcameras); - dist=sum(vC.^2); - %determine which axis (with sign) it is closest to: - tmp=R*vC; - [maxc,maxindex]=max(abs(tmp)); - negindex=find(maxc>max(tmp)); - maxindex(negindex)=maxindex(negindex)+3; - maxindex(ii)=NaN; - - %find closest camera in each six categories - for jj=1:6, - index=find(maxindex==jj); - [slask,minindex]=min(dist(index)); - theindex=index(minindex); - if length(theindex)==1 & sqrt(slask) 5 - %Triangulerad yta - fprintf(fid,'\n'); - fprintf(fid,'%d\n', size(surface.vertices,1)); - %punkter - for i = 1:size(surface.vertices,1); - fprintf(fid,'%f %f %f\n',surface.vertices(i,:)); - end - %triangelhörn index och färg - fprintf(fid,'\n'); - fprintf(fid,'%d\n', size(surface.faces,1)); - for i = 1:size(surface.faces,1); - fprintf(fid,'%d %d %d ',surface.faces(i,:)); - fprintf(fid,'%d %d %d\n',surface.facevertexcdata(i,:)); - end -end - -fclose(fid); diff --git a/visionary/visualize_import.m b/visionary/visualize_import.m deleted file mode 100644 index 8fc05ac..0000000 --- a/visionary/visualize_import.m +++ /dev/null @@ -1,65 +0,0 @@ -function [str,mot,imseq,imfiles,rgb]=visualize_import(bib,filename) -% function [str,mot,imseq,imfiles,rgb]=visualize_import(bib,filename) -% Import results from Visualize -% INPUT: -% bib - directory -% filename - filename of imported file (without directory). -% Should always have extension '.out' -% Hence, full filename is given by [bib,filename] -% OUTPUT: -% str - structure -% mot - motion -% imfiles - filenames of images -% rgb - 3xn matrix for point colors where n is the number of points -% - - -%input file -fid = fopen([bib,filename]); -nbrcameras=fscanf(fid,'%d'); -imfiles=cell(1,nbrcameras); -imseq=cell(1,nbrcameras); -mot=motion; -for ii=1:nbrcameras, - imfiles{ii}=fgetl(fid); - imseq{ii}=imagedata([bib,imfiles{ii}]); - - tline=fgetl(fid); - r1=sscanf(tline,'%f'); - tline=fgetl(fid); - r2=sscanf(tline,'%f'); - tline=fgetl(fid); - r3=sscanf(tline,'%f'); - tline=fgetl(fid); - t=sscanf(tline,'%f'); - tline=fgetl(fid); - tmp=sscanf(tline,'%f'); - focal=tmp(1); - pp=tmp(2:3); - K=eye(3); - K(1,1)=focal; - K(2,2)=focal; - K(1:2,3)=pp; - P=K*[[r1,r2,r3]',t]; - - mot=addcameras(mot,P); - - %Closest cameras - tline=fgetl(fid); -end -%empty line -tline=fgetl(fid); -tline=fgetl(fid); -nbrpoints=sscanf(tline,'%d'); -X=zeros(3,nbrpoints); -rgb=zeros(3,nbrpoints); -for ii=1:nbrpoints, - tline=fgetl(fid); - X(:,ii)=sscanf(tline,'%f'); - tline=fgetl(fid); - rgb(:,ii)=sscanf(tline,'%d'); -end -str=structure(X); -fclose(fid); - -