├── pymulticamselfcal ├── __init__.py ├── visualization.py └── align.py ├── strawlab └── test-data │ ├── DATA20090709_111010 │ ├── Res.dat │ └── camera_order.txt │ ├── DATA20100906_134124 │ ├── Res.dat │ ├── camera_order.txt │ ├── multicamselfcal.cfg │ └── no-global-iterations.cfg │ └── caldata20130726_122220 │ ├── Res.dat │ ├── camera_order.txt │ ├── original_cam_centers.dat │ ├── basename1.rad │ ├── basename3.rad │ ├── basename4.rad │ ├── basename2.rad │ ├── multicamselfcal.cfg │ └── IdMat.dat ├── CalTechCal ├── check_active_images.m ├── go_calib_optim_iter.m ├── README.txt ├── preparedata.m ├── comp_error_calib.m ├── begin-attempt-initialize-rad.patch ├── normalize.m ├── comp_distortion_oulu.m ├── extract_parameters.m ├── rigid_motion.m ├── apply_distortion.m ├── gorad.m ├── comp_ext_calib.m ├── goradf.m └── compute_extrinsic_refine.m ├── MultiCamSelfCal ├── Ransac │ ├── p2e.m │ ├── fFDs.mexglx │ ├── run_mkoct.sh │ ├── fslcm.mexglx │ ├── lin_fm.mexglx │ ├── rroots.mexglx │ ├── seig.m │ ├── nsamples.m │ ├── FDs.m │ ├── fu2F7.m │ ├── normu.m │ ├── mfFDs.m │ ├── lin_fm.c │ ├── lin_fm.cc │ ├── u2F.m │ ├── fFDs.cc │ ├── fFDs.c │ ├── rroots.c │ ├── rEG.m │ ├── rroots.cc │ ├── fslcm.cc │ └── fslcm.c ├── MartinecPajdla │ ├── utils │ │ ├── comb.m │ │ ├── kill_spaces.m │ │ ├── random_int.m │ │ ├── tiles_set.m │ │ ├── combfirst.m │ │ ├── eucl_dist.m │ │ ├── file_exists.m │ │ ├── str_cut.m │ │ ├── normalize_cut.m │ │ ├── p2e.m │ │ ├── k2i.m │ │ ├── combnext.m │ │ ├── diff_rand_ints.m │ │ ├── dist.m │ │ ├── normalize_mp.m │ │ └── eucl_dist_only.m │ ├── test_.m │ ├── setpaths.m │ ├── fill_mm_test │ │ ├── nhom.m │ │ ├── fill_mm_bundle.m │ │ └── eval_y_and_dy.m │ ├── startup.m │ └── fill_mm │ │ ├── spread_depths_col.m │ │ ├── normu.m │ │ ├── subseq_longest.m │ │ ├── depth_estimation.m │ │ ├── M2Fe.m │ │ ├── u2FI.m │ │ ├── L2depths.m │ │ ├── fill_mm_sub.m │ │ ├── create_nullspace__run_one_trial.oct │ │ ├── balance_triplets.m │ │ └── approximate.m ├── CoreFunctions │ ├── align3d.m │ ├── findoutl.m │ ├── rq.m │ ├── reprerror.m │ ├── estsimt.m │ ├── estimateLambda.m │ ├── findinl.m │ ├── estimatePX.m │ └── selfcalib.m ├── FindingPoints │ ├── atlantic.pm │ ├── virooms4.pm │ ├── mycorr2.m │ ├── virooms.pm │ ├── atlanticall.pm │ └── im2pmultiproc.pl ├── LocalAlignments │ ├── planefit.m │ ├── nfi2r.m │ ├── g9.m │ ├── planarmove.m │ ├── planarcams.m │ ├── erlangen.m │ └── joinoscars.m ├── OutputFunctions │ ├── dispcamstats.m │ ├── showimg.m │ ├── drawcloud.m │ ├── drawobject.m │ ├── drawscene.m │ ├── evalreprerror.m │ └── savecalpar.m ├── BlueCLocal │ ├── align_existing_camera_centers.m │ ├── humdra.m │ ├── strawlab_test.m │ ├── bluecrz.m │ └── bluechoengg.m └── changes.txt ├── MultiCamValidation ├── Ransac │ ├── p2e.m │ ├── fFDs.mexglx │ ├── fslcm.mexglx │ ├── lin_fm.mexglx │ ├── rroots.mexglx │ ├── seig.m │ ├── nsamples.m │ ├── FDs.m │ ├── fu2F7.m │ ├── normu.m │ ├── lin_fm.c │ ├── u2F.m │ ├── fFDs.c │ ├── rroots.c │ ├── rEG.m │ └── fslcm.c ├── FindingPoints │ ├── atlantic.pm │ ├── virooms4.pm │ ├── mycorr2.m │ ├── virooms.pm │ └── im2pmultiproc.pl ├── CoreFunctions │ ├── P2KRtC.m │ ├── isptnorm.m │ ├── rq.m │ ├── uP2X.m │ ├── workvolume.m │ ├── findinl.m │ └── estimateX.m └── InputOutputFunctions │ ├── showimg.m │ ├── drawcloud.m │ ├── drawobject.m │ └── drawscene.m ├── RansacM ├── nsamples.m ├── Fsampson.m ├── pointnormiso.m ├── u2Fdlt.m └── rEG.m ├── configurations └── strawlab_test ├── RadialDistortions ├── isptnorm.m ├── undoradial.m ├── readradfile.m ├── undoheikk.m └── comp_distortion_oulu.m ├── CommonCfgAndIO ├── expname.m ├── ConvertOldConfigToNew.m └── loaddata.m ├── python └── test │ ├── test_mcsc.py │ └── test_align.py ├── .github └── workflows │ └── test.yml ├── pyproject.toml └── BlueCFindingPoints ├── findpointsBlueC └── localconfig.m /pymulticamselfcal/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /strawlab/test-data/DATA20090709_111010/Res.dat: -------------------------------------------------------------------------------- 1 | 752 480 2 | 752 480 3 | 752 480 4 | -------------------------------------------------------------------------------- /CalTechCal/check_active_images.m: -------------------------------------------------------------------------------- 1 | ind_active = 1; 2 | n_ima = 1; 3 | active_images = 1; -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/p2e.m: -------------------------------------------------------------------------------- 1 | function e = p2e (u) 2 | e = u(1:2,:) ./ ([1;1] * u(3,:)); 3 | return -------------------------------------------------------------------------------- /strawlab/test-data/DATA20100906_134124/Res.dat: -------------------------------------------------------------------------------- 1 | 752 480 2 | 752 480 3 | 752 480 4 | 752 480 5 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/Res.dat: -------------------------------------------------------------------------------- 1 | 659 494 2 | 659 494 3 | 659 494 4 | 659 494 5 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/p2e.m: -------------------------------------------------------------------------------- 1 | function e = p2e (u) 2 | e = u(1:2,:) ./ ([1;1] * u(3,:)); 3 | return -------------------------------------------------------------------------------- /strawlab/test-data/DATA20090709_111010/camera_order.txt: -------------------------------------------------------------------------------- 1 | sericomyia_0 2 | sericomyia_1 3 | sericomyia_2 4 | -------------------------------------------------------------------------------- /CalTechCal/go_calib_optim_iter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/CalTechCal/go_calib_optim_iter.m -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fFDs.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamSelfCal/Ransac/fFDs.mexglx -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/run_mkoct.sh: -------------------------------------------------------------------------------- 1 | mkoctfile fFDs.cc 2 | mkoctfile fslcm.cc 3 | mkoctfile lin_fm.cc 4 | mkoctfile rroots.cc 5 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fslcm.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamSelfCal/Ransac/fslcm.mexglx -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/lin_fm.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamSelfCal/Ransac/lin_fm.mexglx -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/rroots.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamSelfCal/Ransac/rroots.mexglx -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/fFDs.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamValidation/Ransac/fFDs.mexglx -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/fslcm.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamValidation/Ransac/fslcm.mexglx -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/lin_fm.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamValidation/Ransac/lin_fm.mexglx -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/rroots.mexglx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/MultiCamSelfCal/HEAD/MultiCamValidation/Ransac/rroots.mexglx -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/camera_order.txt: -------------------------------------------------------------------------------- 1 | Basler_21275576 2 | Basler_21275577 3 | Basler_21283674 4 | Basler_21283677 5 | -------------------------------------------------------------------------------- /CalTechCal/README.txt: -------------------------------------------------------------------------------- 1 | Source codes by courtesy of Jean-Yves Bouguet at jean-yves.bouguet@intel.com 2 | 3 | http://www.vision.caltech.edu/bouguetj/calib_doc/ 4 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/seig.m: -------------------------------------------------------------------------------- 1 | %seig sorted eigenvalues 2 | function [V,d] = seig(M) 3 | [V,D] = eig(M); 4 | [d,s] = sort(diag(D)); 5 | V = V(:,s); 6 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/seig.m: -------------------------------------------------------------------------------- 1 | %seig sorted eigenvalues 2 | function [V,d] = seig(M) 3 | [V,D] = eig(M); 4 | [d,s] = sort(diag(D)); 5 | V = V(:,s); 6 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/comb.m: -------------------------------------------------------------------------------- 1 | %returns combination number n over k 2 | function r = comb(n,k) 3 | 4 | r=1; 5 | for i=1:k 6 | r=r*(n-i+1)/i; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /strawlab/test-data/DATA20100906_134124/camera_order.txt: -------------------------------------------------------------------------------- 1 | sericomyia-mobile.local_0 2 | sericomyia-mobile.local_1 3 | sericomyia-mobile.local_2 4 | sericomyia-mobile.local_3 5 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/kill_spaces.m: -------------------------------------------------------------------------------- 1 | %kill_spaces Remove all spaces from given string. 2 | 3 | function r = kill_spaces(s) 4 | 5 | r = s(setdiff(1:length(s),strfind(s,' '))); 6 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/align3d.m: -------------------------------------------------------------------------------- 1 | function [P,X] = align3d(inP,inX,simT); 2 | 3 | T = [simT.s*simT.R;0 0 0]; 4 | T = [T, [simT.t(:);1]]; 5 | 6 | X = T*inX; 7 | 8 | P = inP*inv(T); 9 | 10 | return; 11 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/random_int.m: -------------------------------------------------------------------------------- 1 | %random_int Returns random integer is specified range. 2 | % 3 | % y = random_int(from, to) 4 | 5 | function y = random_int(from, to) 6 | 7 | y = floor(from + (1 + to - from)*rand); -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/tiles_set.m: -------------------------------------------------------------------------------- 1 | %tiles_set Set global tiles.x/y to the specified values. 2 | % 3 | % tiles_set(x, y) 4 | 5 | function tiles_set(x, y) 6 | 7 | global tiles; 8 | 9 | tiles.x = x; 10 | tiles.y = y; 11 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/test_.m: -------------------------------------------------------------------------------- 1 | global scene; 2 | scene.ID = 2; 3 | M = load_scene; if isempty(M), return; end; 4 | 5 | options.no_factorization = 0; 6 | options.create_nullspace.trial_coef = 10; 7 | [ P,X, u1,u2, info ] = fill_mm(M, options); 8 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/original_cam_centers.dat: -------------------------------------------------------------------------------- 1 | 0.38877566795033641 -0.25224323017973899 0.54269896014519803 2 | 0.43992018849672399 0.19838405533526099 0.5530006979363804 3 | -0.36633996302366301 0.17534422404396213 0.49834434355408341 4 | -0.35485027557235815 -0.16901931968931816 0.50765063358276696 5 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/basename1.rad: -------------------------------------------------------------------------------- 1 | K11 = 422.202325 2 | K12 = 0.000000 3 | K13 = 330.145038 4 | K21 = 0.000000 5 | K22 = 424.180871 6 | K23 = 210.309616 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.280971 12 | kc2 = 0.074959 13 | kc3 = 0.000404 14 | kc4 = -0.000104 15 | 16 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/basename3.rad: -------------------------------------------------------------------------------- 1 | K11 = 397.684777 2 | K12 = 0.000000 3 | K13 = 313.133191 4 | K21 = 0.000000 5 | K22 = 400.068501 6 | K23 = 258.339857 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.282840 12 | kc2 = 0.078460 13 | kc3 = 0.000912 14 | kc4 = -0.000127 15 | 16 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/basename4.rad: -------------------------------------------------------------------------------- 1 | K11 = 389.752453 2 | K12 = 0.000000 3 | K13 = 349.609998 4 | K21 = 0.000000 5 | K22 = 391.514349 6 | K23 = 237.332404 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.271015 12 | kc2 = 0.063892 13 | kc3 = -0.000953 14 | kc4 = 0.000412 15 | 16 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/basename2.rad: -------------------------------------------------------------------------------- 1 | K11 = 402.101953 2 | K12 = 0.000000 3 | K13 = 320.832798 4 | K21 = 0.000000 5 | K22 = 403.409910 6 | K23 = 239.706027 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.293525 12 | kc2 = 0.080576 13 | kc3 = -0.000718 14 | kc4 = -0.001240 15 | 16 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/setpaths.m: -------------------------------------------------------------------------------- 1 | Start_path = './MartinecPajdla'; 2 | 3 | addpath ([Start_path '/fill_mm']) 4 | addpath ([Start_path '/fill_mm_test']) 5 | addpath ([Start_path '/utils']) 6 | 7 | global OutputDir Matlab_data; 8 | Matlab_data = [ str_cut(Start_path) 'Matlab_data/' ]; 9 | OutputDir = [ Matlab_data 'output/' ]; 10 | -------------------------------------------------------------------------------- /MultiCamSelfCal/FindingPoints/atlantic.pm: -------------------------------------------------------------------------------- 1 | $process={"compname"=>"atlantic2","camIds"=>[3,4,5,6,7,8,9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"atlantic2","camIds"=>[11,12,13,14,15,16,17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | -------------------------------------------------------------------------------- /MultiCamValidation/FindingPoints/atlantic.pm: -------------------------------------------------------------------------------- 1 | $process={"compname"=>"atlantic2","camIds"=>[3,4,5,6,7,8,9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"atlantic2","camIds"=>[11,12,13,14,15,16,17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/combfirst.m: -------------------------------------------------------------------------------- 1 | %combfirst Returns the first combination in order of shifting the least left 2 | %number to the right. 3 | % 4 | % function com=combfirst(n, k) 5 | % 6 | %n and k has the meaning of combination number. 7 | function com=combfirst(n, k) 8 | 9 | com=find(1 == ones(1,k-1)); 10 | com=[com k-1]; 11 | 12 | return -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/nsamples.m: -------------------------------------------------------------------------------- 1 | %SampleCnt calculates number of samples needed to be done 2 | 3 | function SampleCnt = nsamples(ni, ptNum, pf, conf) 4 | q = prod ([(ni-pf+1) : ni] ./ [(ptNum-pf+1) : ptNum]); 5 | 6 | if (1 -q) < eps 7 | SampleCnt = 1; 8 | else 9 | SampleCnt = log(1 - conf) / log(1 - q); 10 | end 11 | 12 | if SampleCnt < 1 13 | SampleCnt = 1; 14 | end 15 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/nsamples.m: -------------------------------------------------------------------------------- 1 | %SampleCnt calculates number of samples needed to be done 2 | 3 | function SampleCnt = nsamples(ni, ptNum, pf, conf) 4 | q = prod ([(ni-pf+1) : ni] ./ [(ptNum-pf+1) : ptNum]); 5 | 6 | if (1 -q) < eps 7 | SampleCnt = 1; 8 | else 9 | SampleCnt = log(1 - conf) / log(1 - q); 10 | end 11 | 12 | if SampleCnt < 1 13 | SampleCnt = 1; 14 | end 15 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/eucl_dist.m: -------------------------------------------------------------------------------- 1 | %eucl_dist Return Eucledian norm of the difference between two scenes. 2 | % 3 | % er = eucl_dist(M0, M [,I]) 4 | % 5 | % Note: Perspective cameras are assumed. 6 | 7 | function er = eucl_dist(M0, M, I) 8 | 9 | if nargin < 3, I = ~isnan(M0(1:3:end,:)) & ~isnan(M(1:3:end,:)); end 10 | 11 | er = eucl_dist_only(normalize_cut(M0), normalize_cut(M), I); 12 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/file_exists.m: -------------------------------------------------------------------------------- 1 | %file_exists Detection, whether given file exists. 2 | % 3 | % ex = file_exists(name) 4 | % 5 | % name .... file name 6 | % ex .... logical value, if given file exist (0/1) 7 | 8 | function [ex] = file_exists(name); 9 | 10 | fid = fopen(name,'r'); 11 | if fid < 0 12 | ex = 0; 13 | else 14 | fclose(fid); 15 | ex = 1; 16 | end 17 | 18 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/FDs.m: -------------------------------------------------------------------------------- 1 | %FDs error trem for fundamentel m. - squares of Sampson's distances 2 | %function Ds = FDs(F,u) 3 | %where u are corespondences and F is fundamental matrix 4 | 5 | function Ds = FDs(F,u) 6 | rx1 = (F(:,1)' * u(1:3,:)).^2; 7 | ry1 = (F(:,2)' * u(1:3,:)).^2; 8 | rx2 = (F(1,:) * u(4:6,:)).^2; 9 | ry2 = (F(2,:) * u(4:6,:)).^2; 10 | r = Fr(F,u); 11 | Ds = r ./ (rx1 + ry1 + rx2 + ry2); 12 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/str_cut.m: -------------------------------------------------------------------------------- 1 | %str_cut Cut off last piece according to a delimeter from a string. 2 | % 3 | % [ head, tail ] = str_cut(s, delim) 4 | 5 | function [ head, tail ] = str_cut(s, delim) 6 | 7 | if nargin < 2, delim = '/'; end 8 | 9 | idcs = strfind(s,delim); 10 | if isempty(idcs), 11 | head = s; 12 | tail = []; 13 | else 14 | head = s(1:idcs(end)); 15 | tail = s(length(head)+1:end); 16 | end -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/FDs.m: -------------------------------------------------------------------------------- 1 | %FDs error trem for fundamentel m. - squares of Sampson's distances 2 | %function Ds = FDs(F,u) 3 | %where u are corespondences and F is fundamental matrix 4 | 5 | function Ds = FDs(F,u) 6 | rx1 = (F(:,1)' * u(1:3,:)).^2; 7 | ry1 = (F(:,2)' * u(1:3,:)).^2; 8 | rx2 = (F(1,:) * u(4:6,:)).^2; 9 | ry2 = (F(2,:) * u(4:6,:)).^2; 10 | r = Fr(F,u); 11 | Ds = r ./ (rx1 + ry1 + rx2 + ry2); 12 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/normalize_cut.m: -------------------------------------------------------------------------------- 1 | % 2 | % Function Mnorm=normalize_cut(M,I) 3 | % 4 | % normalizes homogenous coordinates and cuts the last coordinate 5 | % (which then equals to 1). 6 | 7 | function Mnorm=normalize_cut(M,I) 8 | 9 | m=size(M,1)/3; 10 | 11 | if nargin < 2, Mnorm=normalize_mp(M); 12 | else Mnorm=normalize_mp(M,I); end 13 | 14 | Mnorm=Mnorm(union((1:m)*3-2,(1:m)*3-1),:); 15 | 16 | return 17 | -------------------------------------------------------------------------------- /RansacM/nsamples.m: -------------------------------------------------------------------------------- 1 | % nsamples calculate the number of samples yet needed 2 | % 3 | % N = nsamples(no_i,ptNum,s,conf) 4 | % no_i ... current number of inliers 5 | % ptNum ... total number of points 6 | % s ... sample size 7 | % conf ... confidence value 8 | % 9 | % $Id: nsamples.m,v 1.1 2005/05/23 16:15:59 svoboda Exp $ 10 | 11 | function N = nsamples(no_i,ptNum,s,conf) 12 | 13 | outl = 1-no_i/ptNum; 14 | N = log(1-conf) / log(1-(1-outl)^s+eps); 15 | 16 | return; -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/planefit.m: -------------------------------------------------------------------------------- 1 | % planefit ... fit a plane to a point cloud 2 | % algebraic minimization applied 3 | % 4 | % n = planefit(X); 5 | % X ... Nx3 matrix with 3D points 6 | % 7 | % n ... 3x1 normal of the fitted plane 8 | % 9 | % $Id: planefit.m,v 1.1 2003/07/03 15:34:43 svoboda Exp $ 10 | 11 | function n = planefit(X); 12 | 13 | X = X-repmat(mean(X),size(X,1),1); 14 | 15 | [u,s,v] =svd(X); 16 | 17 | n = v(:,3); 18 | n = n./norm(n); 19 | 20 | return 21 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm_test/nhom.m: -------------------------------------------------------------------------------- 1 | function x = nhom(x) 2 | %nhom Projective to euclidean coordinates. 3 | % x = nhom(x_) Computes euclidean coordinates by dividing 4 | % all rows but last by the last row. 5 | % x_ ... Size (dim+1,N) Projective coordinates. 6 | % x ... Size (dim,N). Euclidean coordinates. 7 | % (N can be arbitrary.) 8 | if isempty(x) 9 | x = []; 10 | return; 11 | end 12 | d = size(x,1) - 1; 13 | x = x(1:d,:)./(ones(d,1)*x(end,:)); 14 | return 15 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/p2e.m: -------------------------------------------------------------------------------- 1 | function x = p2e(x_) 2 | 3 | %P2E Projective to euclidean coordinates. 4 | % x = p2e(x_) Computes euclidean coordinates by dividing 5 | % all rows but last by the last row. 6 | % x_ ... Size (dim+1,N) Projective coordinates. 7 | % x ... Size (dim,N). Euclidean coordinates. 8 | % (N can be arbitrary.) 9 | 10 | if isempty(x_) 11 | x = []; 12 | return; 13 | end 14 | dim = size(x_,1) - 1; 15 | x = x_(1:dim,:) ./ (ones(dim,1)*x_(dim+1,:)); 16 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/k2i.m: -------------------------------------------------------------------------------- 1 | %k2i Compute indices of matrix rows corresponding to views k [with some step]. 2 | % 3 | % i = k2i(k [,step]) 4 | % 5 | % If k is scalar, it is i = [1:step]+step*(k-1). 6 | % 7 | % Default: step = 3 8 | % 9 | % E.g., for REC.u, REC.P, REC.X use k2i(k), 10 | % for REC.q use k2i(k,2). 11 | 12 | function i = k2i(k,step) 13 | 14 | if nargin<2 15 | step = 3; 16 | end 17 | 18 | k = k(:)'; 19 | i = [1:step]'*ones(size(k)) + step*(ones(step,1)*k-1); 20 | i = i(:); 21 | -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/P2KRtC.m: -------------------------------------------------------------------------------- 1 | % [K,R,t,C] = P2KRtC(P) 2 | % decompose the euclidean 3x4 projection matrix P into the 3 | % 4 | % K ... 3x3 upper triangular calibration matrix 5 | % R ... 3x3 rotation matrix 6 | % t ... 3x1 translation vector 7 | % C ... 3x1 position od the camera center 8 | % 9 | % $Id: P2KRtC.m,v 2.0 2003/06/19 12:07:08 svoboda Exp $ 10 | 11 | function [K,R,t,C] = P2KRtC(P) 12 | 13 | P = P./norm(P(3,1:3)); 14 | 15 | [K,R] = rq(P(:,1:3)); 16 | t = inv(K)*P(:,4); 17 | C = -R'*t; 18 | 19 | return; 20 | -------------------------------------------------------------------------------- /CalTechCal/preparedata.m: -------------------------------------------------------------------------------- 1 | % preparedata 2 | % 3 | % reads BlueC *.rad file and transform data 4 | % for the CalTech Camera calibration toolbox 5 | 6 | function [X,x] = preparedata(filename); 7 | 8 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 9 | 10 | if Octave, 11 | datamat = load(filename); 12 | if isstruct(datamat) 13 | datamat = datamat.corresp; 14 | end 15 | else 16 | datamat = load(filename,'-ASCII'); 17 | end 18 | 19 | X = datamat(:,1:3)'; % 3D points 20 | x = datamat(:,5:6)'; % 2D projections 21 | 22 | return; 23 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fu2F7.m: -------------------------------------------------------------------------------- 1 | function Fs = fu2F7(u) 2 | 3 | Z = lin_fm(u); 4 | 5 | NullSp = null(Z); 6 | if size(NullSp,2) > 2 7 | Fs = []; 8 | return; %degenerated sample 9 | end 10 | 11 | F1 = reshape(NullSp(:,1),3,3); 12 | F2 = reshape(NullSp(:,2),3,3); 13 | p = fslcm(F1,F2); 14 | aroots = rroots(p); 15 | 16 | %xr = o_fslcm(F1,F2) 17 | 18 | %aroots == xr 19 | 20 | for i = 1:length(aroots) 21 | l = aroots(i); 22 | Ft = F1 * l + F2 * (1-l); 23 | % Ft = Ft /norm(Ft,2); 24 | Fs(:,:,i) = Ft; 25 | end 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/startup.m: -------------------------------------------------------------------------------- 1 | MATLAB_PATH = '/usr/local/matlab/'; 2 | 3 | global Start_path; 4 | Start_path = '/home.dokt/martid1/export/svoboda/Matlab'; 5 | setpaths; 6 | 7 | screen = get(0, 'ScreenSize'); 8 | xwidth = screen(4)/2; 9 | ywidth = screen(4)/2; 10 | set(0, 'defaultfigureposition', [screen(3)-xwidth*1.1 5 xwidth*1.1-4 ywidth*0.9]); 11 | clear screen xwidth ywidth 12 | 13 | set(0,'FormatSpacing', 'compact',... 14 | 'DefaultFigureColormap' , (0:1/63:1)'*[1 1 1],... 15 | 'DefaultFigureMenu', 'none' ); 16 | close 17 | 18 | flops(0); 19 | -------------------------------------------------------------------------------- /MultiCamSelfCal/FindingPoints/virooms4.pm: -------------------------------------------------------------------------------- 1 | $process={"compname"=>"viroom00","camIds"=>[0],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"viroom00","camIds"=>[1],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | $process={"compname"=>"viroom03","camIds"=>[2],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 6 | push(@processes,$process); 7 | $process={"compname"=>"viroom03","camIds"=>[3],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 8 | push(@processes,$process); 9 | -------------------------------------------------------------------------------- /MultiCamValidation/FindingPoints/virooms4.pm: -------------------------------------------------------------------------------- 1 | $process={"compname"=>"viroom00","camIds"=>[0],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"viroom00","camIds"=>[1],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | $process={"compname"=>"viroom03","camIds"=>[2],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 6 | push(@processes,$process); 7 | $process={"compname"=>"viroom03","camIds"=>[3],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 8 | push(@processes,$process); 9 | -------------------------------------------------------------------------------- /RansacM/Fsampson.m: -------------------------------------------------------------------------------- 1 | function errs = Fsampson(F,u); 2 | % FSAMPSON ... first order geometrical error (Sampson Distance) 3 | % errs = Fsampson(F,u); 4 | % F ... 3x3 Fundamental matrix 5 | % u ... 6xN point pairs homogenous 6 | % 7 | % errs ... 1xN error for each point pair 8 | % 9 | % $Id: Fsampson.m,v 1.1 2005/05/23 16:15:59 svoboda Exp $ 10 | 11 | N = size(u,2); 12 | 13 | u1 = u(1:3,:); 14 | u2 = u(4:6,:); 15 | 16 | errs = zeros(1,N); 17 | for i=1:N 18 | Fu1 = F*u1(:,i); 19 | Fu2 = F'*u1(:,i); 20 | errs(i) = (u2(:,i)'*F*u1(:,i))^2 / (sum([Fu1(1:2)'.^2,Fu2(1:2)'.^2])); 21 | end 22 | -------------------------------------------------------------------------------- /strawlab/test-data/DATA20100906_134124/multicamselfcal.cfg: -------------------------------------------------------------------------------- 1 | [Files] 2 | Basename: cam 3 | Image-Extension: jpg 4 | 5 | [Images] 6 | Subpix: 0.5 7 | 8 | [Calibration] 9 | Num-Cameras: 4 10 | Num-Projectors: 0 11 | Nonlinear-Parameters: 50 0 1 0 0 0 12 | Nonlinear-Update: 1 0 1 0 0 0 13 | Initial-Tolerance: 10 14 | Do-Global-Iterations: 1 15 | Global-Iteration-Threshold: 0.5 16 | Global-Iteration-Max: 5 17 | Num-Cameras-Fill: 2 18 | Do-Bundle-Adjustment: 1 19 | Undo-Radial: 0 20 | Min-Points-Value: 30 21 | N-Tuples: 3 22 | Square-Pixels: 1 23 | Use-Nth-Frame: 2 24 | 25 | -------------------------------------------------------------------------------- /strawlab/test-data/DATA20100906_134124/no-global-iterations.cfg: -------------------------------------------------------------------------------- 1 | [Files] 2 | Basename: cam 3 | Image-Extension: jpg 4 | 5 | [Images] 6 | Subpix: 0.5 7 | 8 | [Calibration] 9 | Num-Cameras: 4 10 | Num-Projectors: 0 11 | Nonlinear-Parameters: 50 0 1 0 0 0 12 | Nonlinear-Update: 1 0 1 0 0 0 13 | Initial-Tolerance: 10 14 | Do-Global-Iterations: 0 15 | Global-Iteration-Threshold: 0.5 16 | Global-Iteration-Max: 5 17 | Num-Cameras-Fill: 2 18 | Do-Bundle-Adjustment: 1 19 | Undo-Radial: 0 20 | Min-Points-Value: 30 21 | N-Tuples: 3 22 | Square-Pixels: 1 23 | Use-Nth-Frame: 2 24 | 25 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/dispcamstats.m: -------------------------------------------------------------------------------- 1 | function ret = dispcamstats(cam,inliers); 2 | % DISPCAMSTATS display statistics about cameras to the command window 3 | % 4 | % auxiliary function for the main gocal script 5 | % with a minor modification is may also write the statistics to a file 6 | % I never needed it. 7 | % 8 | % $Id: dispcamstats.m,v 2.1 2005/05/23 16:21:50 svoboda Exp $ 9 | 10 | fprintf(1,'CamId std mean #inliers \n'); 11 | for i=1:size(cam,2), 12 | fprintf(1,'%3d %8.2f %8.2f %6d \n',cam(i).camId, cam(i).std2Derr, cam(i).mean2Derr, sum(inliers.IdMat(i,:))); 13 | end 14 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/spread_depths_col.m: -------------------------------------------------------------------------------- 1 | % Mdepthcol is column of JIM with depths which is spread by this 2 | % function to a submatrix with some zeros 3 | function submatrix = spread_depths_col(Mdepthcol,depthsIcol) 4 | 5 | m = size(depthsIcol,1); 6 | n = 1; 7 | 8 | known_depths = find(depthsIcol ~= 0); 9 | 10 | if ~isempty(known_depths) 11 | rows = k2i(known_depths); 12 | submatrix(rows,n) = Mdepthcol(rows); n=n+1; 13 | end 14 | 15 | for i=setdiff(1:m, known_depths) 16 | rows = k2i(i); 17 | submatrix(rows,n) = Mdepthcol(rows); n=n+1; 18 | end 19 | 20 | -------------------------------------------------------------------------------- /configurations/strawlab_test: -------------------------------------------------------------------------------- 1 | [Paths] 2 | Data: ../strawlab/test-data/DATA20090709_111010/ 3 | 4 | [Files] 5 | Basename: basename 6 | Image-Extension: jpg 7 | 8 | [Images] 9 | Subpix: 0.5 10 | 11 | [Calibration] 12 | Num-Cameras: 3 13 | Num-Projectors: 0 14 | Nonlinear-Parameters: 30 0 1 0 0 0 15 | Nonlinear-Update: 1 0 1 0 0 0 16 | Initial-Tolerance: 10 17 | Do-Global-Iterations: 0 18 | Global-Iteration-Threshold: 0.5 19 | Global-Iteration-Max: 100 20 | Num-Cameras-Fill: 2 21 | Do-Bundle-Adjustment: 1 22 | Undo-Radial: 0 23 | Min-Points-Value: 30 24 | N-Tuples: 3 25 | Square-Pixels: 1 26 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/multicamselfcal.cfg: -------------------------------------------------------------------------------- 1 | [Files] 2 | Basename: basename 3 | Image-Extension: jpg 4 | 5 | [Images] 6 | Subpix: 0.5 7 | 8 | [Calibration] 9 | Num-Cameras: 4 10 | Num-Projectors: 0 11 | Nonlinear-Parameters: 50 0 1 0 0 0 12 | Nonlinear-Update: 1 0 1 0 0 0 13 | Initial-Tolerance: 10 14 | Do-Global-Iterations: 0 15 | Global-Iteration-Threshold: 0.5 16 | Global-Iteration-Max: 5 17 | Num-Cameras-Fill: 4 18 | Do-Bundle-Adjustment: 1 19 | Undo-Radial: 1 20 | Min-Points-Value: 30 21 | N-Tuples: 3 22 | Square-Pixels: 1 23 | Use-Nth-Frame: 5 24 | Align-Existing: 1 25 | -------------------------------------------------------------------------------- /MultiCamSelfCal/FindingPoints/mycorr2.m: -------------------------------------------------------------------------------- 1 | % mycorr2 modified version of the 2D correlation 2 | % for the use with im2col and col2im 3 | % see GETPOINT 4 | % 5 | % 6 | % $Id: mycorr2.m,v 2.0 2003/06/19 12:06:52 svoboda Exp $ 7 | 8 | % Note: It written in order to gain speed. The clarity of the code suffers accordingly 9 | 10 | function R = mycorr2(X,G,Gn,Gn2) 11 | 12 | % Gn = G-mean(G); 13 | % Gn2 = sqrt(sum(Gn.^2)); 14 | 15 | mX = repmat(mean(X),size(X,1),1); 16 | mXn = X - mX; 17 | smX = sum(mXn.^2); 18 | 19 | numerator = (mXn'*Gn)'; 20 | denominator = smX*Gn2; 21 | 22 | R = numerator./denominator; 23 | 24 | return -------------------------------------------------------------------------------- /MultiCamValidation/FindingPoints/mycorr2.m: -------------------------------------------------------------------------------- 1 | % mycorr2 modified version of the 2D correlation 2 | % for the use with im2col and col2im 3 | % see GETPOINT 4 | % 5 | % 6 | % $Id: mycorr2.m,v 2.0 2003/06/19 12:07:11 svoboda Exp $ 7 | 8 | % Note: It written in order to gain speed. The clarity of the code suffers accordingly 9 | 10 | function R = mycorr2(X,G,Gn,Gn2) 11 | 12 | % Gn = G-mean(G); 13 | % Gn2 = sqrt(sum(Gn.^2)); 14 | 15 | mX = repmat(mean(X),size(X,1),1); 16 | mXn = X - mX; 17 | smX = sum(mXn.^2); 18 | 19 | numerator = (mXn'*Gn)'; 20 | denominator = smX*Gn2; 21 | 22 | R = numerator./denominator; 23 | 24 | return -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/fu2F7.m: -------------------------------------------------------------------------------- 1 | function Fs = fu2F7(u) 2 | 3 | % Z = []; 4 | % for j = 1:7 5 | % Z(j,:) = reshape(u(1:3,j)*u(4:6,j)',1,9); 6 | % end 7 | 8 | Z = lin_fm(u); 9 | 10 | NullSp = null(Z); 11 | if size(NullSp,2) > 2 12 | Fs = []; 13 | return; %degenerated sample 14 | end 15 | 16 | F1 = reshape(NullSp(:,1),3,3); 17 | F2 = reshape(NullSp(:,2),3,3); 18 | p = fslcm(F1,F2); 19 | aroots = rroots(p); 20 | 21 | %xr = o_fslcm(F1,F2) 22 | 23 | %aroots == xr 24 | 25 | for i = 1:length(aroots) 26 | l = aroots(i); 27 | Ft = F1 * l + F2 * (1-l); 28 | % Ft = Ft /norm(Ft,2); 29 | Fs(:,:,i) = Ft; 30 | end 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/normu.m: -------------------------------------------------------------------------------- 1 | function A = normu(u); 2 | 3 | % NORMU Normalizes image points to be used for LS estimation. 4 | % A = NORMU(u) finds normalization matrix A so that mean(A*u)=0 and mean(||A*u||)=sqrt(2). 5 | % (see Hartley: In Defence of 8-point Algorithm, ICCV`95). 6 | % Parameters: 7 | % u ... Size (2,N) or (3,N). Points to normalize. 8 | % A ... Size (3,3). Normalization matrix. 9 | 10 | if size(u,1)==3, u = p2e(u); end 11 | 12 | m = mean(u')'; % <=> mean (u,2) 13 | u = u - m*ones(1,size(u,2)); 14 | distu = sqrt(sum(u.*u)); 15 | r = mean(distu)/sqrt(2); 16 | A = diag([1/r 1/r 1]); 17 | A(1:2,3) = -m/r; 18 | 19 | return 20 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/showimg.m: -------------------------------------------------------------------------------- 1 | %SHOWIMG Pajdla: Shows scaled images in the gray palette 2 | % 3 | % 4 | % function f = showimg(m,f) 5 | % 6 | % m = image matrix 7 | % f = figure handle 8 | % 9 | % See also: IMAGE, IMAGESC, COLORMEN. 10 | 11 | % Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be 12 | % pajdla@vision.felk.cvut.cz 13 | % 03/06/95 ESAT-MI2, KU Leuven 14 | % Documentation: 15 | % Language: Matlab 4.2, (c) MathWorks 16 | % 17 | function f = showimg(m,f); 18 | 19 | if (nargin==2) 20 | figure(f); 21 | else 22 | f=figure; 23 | % colormen; 24 | colormap('gray'); 25 | end 26 | 27 | imagesc(m); 28 | axis('image'); 29 | 30 | return 31 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/normu.m: -------------------------------------------------------------------------------- 1 | function A = normu(u); 2 | 3 | % NORMU Normalizes image points to be used for LS estimation. 4 | % A = NORMU(u) finds normalization matrix A so that mean(A*u)=0 and mean(||A*u||)=sqrt(2). 5 | % (see Hartley: In Defence of 8-point Algorithm, ICCV`95). 6 | % Parameters: 7 | % u ... Size (2,N) or (3,N). Points to normalize. 8 | % A ... Size (3,3). Normalization matrix. 9 | 10 | if size(u,1)==3, u = p2e(u); end 11 | 12 | m = mean(u')'; % <=> mean (u,2) 13 | u = u - m*ones(1,size(u,2)); 14 | distu = sqrt(sum(u.*u)); 15 | r = mean(distu)/sqrt(2); 16 | A = diag([1/r 1/r 1]); 17 | A(1:2,3) = -m/r; 18 | 19 | return 20 | -------------------------------------------------------------------------------- /MultiCamValidation/InputOutputFunctions/showimg.m: -------------------------------------------------------------------------------- 1 | %SHOWIMG Pajdla: Shows scaled images in the gray palette 2 | % 3 | % 4 | % function f = showimg(m,f) 5 | % 6 | % m = image matrix 7 | % f = figure handle 8 | % 9 | % See also: IMAGE, IMAGESC, COLORMEN. 10 | 11 | % Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be 12 | % pajdla@vision.felk.cvut.cz 13 | % 03/06/95 ESAT-MI2, KU Leuven 14 | % Documentation: 15 | % Language: Matlab 4.2, (c) MathWorks 16 | % 17 | function f = showimg(m,f); 18 | 19 | if (nargin==2) 20 | figure(f); 21 | else 22 | f=figure; 23 | % colormen; 24 | colormap('gray'); 25 | end 26 | 27 | imagesc(m); 28 | axis('image'); 29 | 30 | return 31 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/mfFDs.m: -------------------------------------------------------------------------------- 1 | % mfFDs first order geometric error (Sampson distance) 2 | % m-version of the original c-code (mex) which were 3 | % causing some problems 4 | 5 | % $Author: svoboda $ 6 | % $Revision: 2.2 $ 7 | % $Id: mfFDs.m,v 2.2 2004/05/04 16:09:35 svoboda Exp $ 8 | % $State: Exp $ 9 | 10 | function err = mfFDs(F,u); 11 | 12 | % disp('m-version of fFDs') 13 | 14 | Fu1 = F*u(4:6,:); 15 | Fu2 = (F'*u(1:3,:)).^2; 16 | Fu1pow = Fu1.^2; 17 | 18 | denom = Fu1pow(1,:)+Fu1pow(2,:)+Fu2(1,:)+Fu2(2,:); 19 | 20 | errvec = zeros(1,size(u,2)); 21 | for i=1:size(u,2), 22 | xFx = u(1:3,i)'*Fu1(:,i); 23 | errvec(i) = xFx^2/denom(i); 24 | end 25 | 26 | err = errvec; 27 | return 28 | 29 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/drawcloud.m: -------------------------------------------------------------------------------- 1 | % drawcloud ... draw point cloud 2 | % 3 | % [fig] = drawcloud(X,fig,{color}) 4 | % 5 | % X ... 3xn matrix containing the points 6 | % if X is 4xn only the first 3 rows are used 7 | % fig . figure handle 8 | % color color of plotting; defaults to blue 9 | % 10 | % fig . return the figure handle 11 | % 12 | % $Id: drawcloud.m,v 2.0 2003/06/19 12:07:02 svoboda Exp $ 13 | 14 | function [fig] = drawcloud(X,fig,color) 15 | if nargin < 3 16 | color = 'b'; % default color 17 | end 18 | 19 | figure(fig), hold on 20 | plot3(X(1,:),X(2,:),X(3,:),[color,'o']) 21 | 22 | 23 | view([1,1,1]); 24 | axis('equal'); 25 | grid on 26 | rotate3d on 27 | fig=fig; 28 | return 29 | -------------------------------------------------------------------------------- /MultiCamValidation/InputOutputFunctions/drawcloud.m: -------------------------------------------------------------------------------- 1 | % drawcloud ... draw point cloud 2 | % 3 | % [fig] = drawcloud(X,fig,{color}) 4 | % 5 | % X ... 3xn matrix containing the points 6 | % if X is 4xn only the first 3 rows are used 7 | % fig . figure handle 8 | % color color of plotting; defaults to blue 9 | % 10 | % fig . return the figure handle 11 | % 12 | % $Id: drawcloud.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $ 13 | 14 | function [fig] = drawcloud(X,fig,color) 15 | if nargin < 3 16 | color = 'b'; % default color 17 | end 18 | 19 | figure(fig), hold on 20 | plot3(X(1,:),X(2,:),X(3,:),[color,'o']) 21 | 22 | 23 | view([1,1,1]); 24 | axis('equal'); 25 | grid on 26 | rotate3d on 27 | fig=fig; 28 | return 29 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/combnext.m: -------------------------------------------------------------------------------- 1 | %combnext Returns the next combination in order of shifting the least left 2 | %number to the right. 3 | % 4 | % function next=combnext(n, k, com) 5 | % 6 | %n and k has the meaning of combination number. 7 | function next=combnext(n, k, com) 8 | 9 | next=com; 10 | move=k; moved=0; 11 | while ~moved 12 | if next(move) < n-k+move 13 | next(move)=next(move)+1; 14 | for i=move+1:k 15 | next(i)=next(move)+i-move; 16 | end 17 | moved=1; 18 | else 19 | if move>1 20 | move=move-1; 21 | else 22 | disp('Error: this code should have never be called'); 23 | end 24 | end 25 | end 26 | 27 | return 28 | 29 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/lin_fm.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mex.h" 3 | #include "matrix.h" 4 | 5 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 6 | { 7 | double *u = mxGetPr(aSrcP[0]); 8 | double *p, *s; 9 | int len = mxGetN(aSrcP[0]); 10 | int i,k,l,pos; 11 | 12 | aDstP[0] = mxCreateDoubleMatrix(len, 9, mxREAL ); 13 | p = (double *)mxGetData(aDstP[0]); 14 | 15 | s = u; 16 | for (i = 0; i < len; i++) 17 | { 18 | pos = 0; 19 | for (k = 0; k < 3; k++) 20 | { 21 | for (l = 0; l < 3; l++) 22 | { 23 | *(p+pos) = *(s+k+3) * (*(s+l)); 24 | pos += len; 25 | } 26 | } 27 | s += 6; 28 | p++; 29 | } 30 | } 31 | 32 | 33 | -------------------------------------------------------------------------------- /RansacM/pointnormiso.m: -------------------------------------------------------------------------------- 1 | function [u2,T] = pointnormiso(u); 2 | % pointnormiso Isotropic point normalization 3 | % 4 | % [u2,T] = pointnormiso(u); 5 | % u ... 3xN input data homogenous 6 | % 7 | % u2 ... 3xN normalized data homogenous 8 | % T ... 3x3 transformation matrix the does the tranformation 9 | % 10 | % $Id: pointnormiso.m,v 1.1 2005/05/23 16:16:00 svoboda Exp $ 11 | 12 | n=size(u,2); 13 | 14 | xmean = mean(u(1,:)); 15 | ymean = mean(u(2,:)); 16 | 17 | u2 = u; 18 | u2(1:2,:) = u(1:2,:) - repmat([xmean;ymean],1,n); 19 | 20 | scale = sqrt(2)/mean(sqrt(sum(u2(1:2,:).^2))); 21 | 22 | u2(1:2,:) = scale*u2(1:2,:); 23 | 24 | T = diag([scale,scale,1]); 25 | T(1,3) = -scale*xmean; 26 | T(2,3) = -scale*ymean; 27 | 28 | return; 29 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/lin_fm.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mex.h" 3 | #include "matrix.h" 4 | 5 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 6 | { 7 | double *u = mxGetPr(aSrcP[0]); 8 | double *p, *s; 9 | int len = mxGetN(aSrcP[0]); 10 | int i,k,l,pos; 11 | 12 | aDstP[0] = mxCreateDoubleMatrix(len, 9, mxREAL ); 13 | p = (double *)mxGetData(aDstP[0]); 14 | 15 | s = u; 16 | for (i = 0; i < len; i++) 17 | { 18 | pos = 0; 19 | for (k = 0; k < 3; k++) 20 | { 21 | for (l = 0; l < 3; l++) 22 | { 23 | *(p+pos) = *(s+k+3) * (*(s+l)); 24 | pos += len; 25 | } 26 | } 27 | s += 6; 28 | p++; 29 | } 30 | } 31 | 32 | 33 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/normu.m: -------------------------------------------------------------------------------- 1 | %normu Normalize image points to be used for LS estimation. 2 | % A = normu(u) finds normalization matrix A so that mean(A*u)=0 and 3 | % mean(||A*u||)=sqrt(2). (see Hartley: In Defence of 8-point Algorithm, 4 | % ICCV`95). 5 | % 6 | % Parameters: 7 | % u ... Size (2,N) or (3,N). Points to normalize. 8 | % A ... Size (3,3). Normalization matrix. 9 | 10 | function A = normu(u) 11 | 12 | if size(u,1)==3, u = p2e(u); end 13 | 14 | m = mean(u')'; % <=> mean (u,2) 15 | u = u - m*ones(1,size(u,2)); 16 | distu = sqrt(sum(u.*u)); 17 | r = mean(distu)/sqrt(2); 18 | if ~r, A = []; return; end % one of degenarate configurations 19 | A = diag([1/r 1/r 1]); 20 | A(1:2,3) = -m/r; 21 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/diff_rand_ints.m: -------------------------------------------------------------------------------- 1 | %diff_rand_ints Add n random ints in some scope to a given vector. 2 | % 3 | % Add n ints, randomly chosen between from and to inclusive to the list in 4 | % ints. The resulting vector of ints should have all different numbers. 5 | % 6 | % y = diff_rand_ints(ints, n,from,to) 7 | 8 | function y = diff_rand_ints(ints, n,from,to) 9 | 10 | if (to + 1 - from) < n + size(ints,2) 11 | error(sprintf('Not enough room for %d random numbers from %d to %d', n, from, to)); 12 | end 13 | for i = 1:n 14 | x = random_int(from,to-size(ints,2)); 15 | oldx = from - 1; 16 | while oldx < x 17 | temp = x; 18 | x = x + sum((oldx < ints) & (ints <= x)); 19 | oldx = temp; 20 | end 21 | ints = [ints,x]; 22 | end 23 | y = ints; -------------------------------------------------------------------------------- /pymulticamselfcal/visualization.py: -------------------------------------------------------------------------------- 1 | def create_pcd_file_from_points(fname, points, npts=None): 2 | HEADER = \ 3 | "# .PCD v.7 - Point Cloud Data file format\n"\ 4 | "VERSION .7\n"\ 5 | "FIELDS x y z\n"\ 6 | "SIZE 4 4 4\n"\ 7 | "TYPE F F F\n"\ 8 | "COUNT 1 1 1\n"\ 9 | "WIDTH %(npoints)d\n"\ 10 | "HEIGHT 1\n"\ 11 | "VIEWPOINT 0 0 0 1 0 0 0\n"\ 12 | "POINTS %(npoints)d\n"\ 13 | "DATA ascii\n" 14 | 15 | ok = False 16 | if len(points): 17 | if len(points[0]) == 3: 18 | ok = True 19 | if not ok: 20 | raise ValueError("Points must be a list of (x,y,z) tuples") 21 | 22 | with open(fname, 'w') as fd: 23 | fd.write(HEADER % {"npoints":len(points)}) 24 | for pt in points: 25 | fd.write("%f %f %f\n" % tuple(pt)) 26 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/dist.m: -------------------------------------------------------------------------------- 1 | %dist Return distance between image points in homog. coor. in specified metric. 2 | % 3 | % d = dist(M1, M2, metric) 4 | % 5 | % Parameters: 6 | % metric .. x- and y-coordinates are associated with a single image point 7 | % 1 .. square root of sum of squares of x- and y-coordinates 8 | % 2 .. std of x- and y-coordinates 9 | % <=> equivalent to noise type 2 in noise_add 10 | 11 | function d = dist(M1, M2, metric) 12 | 13 | if nargin < 3, metric = 2; end 14 | 15 | switch metric, 16 | case 1, I = ~isnan(M1(1:3:end,:)) &~isnan(M2(1:3:end,:)); 17 | d = eucl_dist(M1, M2, I) / sum(sum(I)); 18 | case 2, D = normalize_cut(M1) - normalize_cut(M2); 19 | i = find(~isnan(D(1:2:end))); 20 | d = std([D(2*i-1) D(2*i)]); 21 | otherwise, error('dist: unknown metric'); 22 | end 23 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/lin_fm.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | DEFUN_DLD (lin_fm, args, , 5 | "[...] = lin_fm (...)\n") 6 | { 7 | Matrix ovu (args(0).matrix_value()); 8 | 9 | double *u = ovu.rep->data; // this is a naughty way to get a pointer to the data 10 | double *p, *s; 11 | int len = ovu.columns(); 12 | int i,k,l,pos; 13 | 14 | Matrix output_matrix(len,9); 15 | p = output_matrix.rep->data; // this is a naughty way to get a pointer to the data 16 | 17 | printf("lin_fm\n"); 18 | 19 | s = u; 20 | for (i = 0; i < len; i++) 21 | { 22 | pos = 0; 23 | for (k = 0; k < 3; k++) 24 | { 25 | for (l = 0; l < 3; l++) 26 | { 27 | *(p+pos) = *(s+k+3) * (*(s+l)); 28 | pos += len; 29 | } 30 | } 31 | s += 6; 32 | p++; 33 | } 34 | return octave_value(output_matrix); 35 | } 36 | 37 | 38 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/u2F.m: -------------------------------------------------------------------------------- 1 | %u2F estimates fundamental matrix using ortogonal LS regression 2 | % F = u2F(u) estimates F from u using NORMU 3 | % F = u2F(u,'nonorm') disables normalization 4 | % see also NORMU, U2FA 5 | 6 | 7 | function F = u2F (u, str) 8 | 9 | if (nargin > 1) & strcmp(str, 'nonorm') 10 | donorm = 0; 11 | else 12 | donorm = 1; 13 | end 14 | 15 | ptNum = size(u,2); 16 | 17 | if donorm 18 | A1 = normu(u(1:3,:)); 19 | A2 = normu(u(4:6,:)); 20 | 21 | u1 = A1*u(1:3,:); 22 | u2 = A2*u(4:6,:); 23 | end 24 | 25 | for i = 1:ptNum 26 | Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); 27 | end 28 | 29 | M = Z'*Z; 30 | V = seig(M); 31 | F = reshape(V(:,1),3,3); 32 | 33 | [uu,us,uv] = svd(F); 34 | [y,i] = min (abs(diag(us))); 35 | us(i,i) = 0; 36 | F = uu*us*uv'; 37 | 38 | if donorm 39 | F = A1'*F*A2; 40 | end 41 | 42 | F = F /norm(F,2); 43 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/u2F.m: -------------------------------------------------------------------------------- 1 | %u2F estimates fundamental matrix using ortogonal LS regression 2 | % F = u2F(u) estimates F from u using NORMU 3 | % F = u2F(u,'nonorm') disables normalization 4 | % see also NORMU, U2FA 5 | 6 | 7 | function F = u2F (u, str) 8 | 9 | if (nargin > 1) & strcmp(str, 'nonorm') 10 | donorm = 0; 11 | else 12 | donorm = 1; 13 | end 14 | 15 | ptNum = size(u,2); 16 | 17 | if donorm 18 | A1 = normu(u(1:3,:)); 19 | A2 = normu(u(4:6,:)); 20 | 21 | u1 = A1*u(1:3,:); 22 | u2 = A2*u(4:6,:); 23 | end 24 | 25 | for i = 1:ptNum 26 | Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); 27 | end 28 | 29 | M = Z'*Z; 30 | V = seig(M); 31 | F = reshape(V(:,1),3,3); 32 | 33 | [uu,us,uv] = svd(F); 34 | [y,i] = min (abs(diag(us))); 35 | us(i,i) = 0; 36 | F = uu*us*uv'; 37 | 38 | if donorm 39 | F = A1'*F*A2; 40 | end 41 | 42 | F = F /norm(F,2); 43 | -------------------------------------------------------------------------------- /RadialDistortions/isptnorm.m: -------------------------------------------------------------------------------- 1 | % isptnorm ISotropic PoinT NORMalization 2 | % 3 | % [xnorm,T] = isptnorm(x); 4 | % x ... [N x dim] coordinates 5 | % 6 | % xnorm ... normalized coordinates 7 | % T ... transformation matrix used 8 | % 9 | % T. Svoboda, 5/2001 10 | 11 | 12 | function [xnorm,T] = isptnorm(x); 13 | 14 | 15 | % data dimension 16 | dim = size(x,2); 17 | N = size(x,1); 18 | 19 | % make homogenous coordinates 20 | x(:,dim+1) = ones(N,1); 21 | 22 | % compute sum of square diferences 23 | for i=1:dim, 24 | ssd(:,i) = (x(:,i)-mean(x(:,i))).^2; 25 | end 26 | 27 | scale = (sqrt(dim)*N) / (sum(sqrt(sum(ssd')))); 28 | 29 | T = zeros(dim+1); 30 | 31 | for i=1:dim, 32 | T(i,i) = scale; 33 | T(i,dim+1) = -scale*mean(x(:,i)); 34 | end 35 | T(dim+1,dim+1) = 1; 36 | 37 | xnorm = T*x'; 38 | xnorm = xnorm'; 39 | % return nonhomogenous part of the points coordinates 40 | xnorm = xnorm(:,1:dim); 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/findoutl.m: -------------------------------------------------------------------------------- 1 | % find outliers in cameras 2 | % 3 | % [outliers, inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL); 4 | % 5 | % $Id: findoutl.m,v 2.1 2005/05/20 11:58:23 svoboda Exp $ 6 | 7 | function [outliers, inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL); 8 | 9 | CAMS = size(cam,2); 10 | 11 | idxoutMat = zeros(size(inliers.IdMat)); 12 | for i=1:CAMS, 13 | if (cam(i).std2Derr > cam(i).mean2Derr) || (cam(i).mean2Derr > INL_TOL) 14 | reprerrs = cam(i).err2d - cam(i).mean2Derr; 15 | idxout = find((reprerrs > 3*cam(i).std2Derr) && reprerrs > INL_TOL); 16 | else 17 | idxout = []; 18 | end 19 | idxoutMat(i,cam(i).idlin(cam(i).visandrec(idxout))) = 1; 20 | end 21 | inliers.IdMat(:,sum(idxoutMat)>0) = 0; % zero all columns with at least one outlier 22 | inliers.idx = find(sum(inliers.IdMat)>=size(inliers.IdMat,1)-NUM_CAMS_FILL); 23 | outliers = sum(sum(idxoutMat)>0); 24 | 25 | return; 26 | 27 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/subseq_longest.m: -------------------------------------------------------------------------------- 1 | %subseq_longest Find the longest continuous subsequences in columns of MM. 2 | % 3 | % Put the initial image of the longest continuous subsequence of known 4 | % points in column ``p'' to ``b(p)''. 5 | % 6 | % [b, len] = subseq_longest(I) 7 | % 8 | % Parameters: 9 | % len ... len(p) is length of the longest continuous 10 | % subsequence of known points in column ``p'' 11 | 12 | function [b, len] = subseq_longest(I) 13 | 14 | [m n] = size(I); 15 | 16 | b(n) = 0; % memory allocation 17 | len(n) = 0; % " " 18 | 19 | for p = 1:n 20 | seq(1:m) = 0; 21 | l = 1; 22 | for i = 1:m 23 | if I(i,p) 24 | seq(l) = seq(l) +1; 25 | else 26 | l = i+1; 27 | end 28 | end 29 | len(p) = max(seq); 30 | best = find(seq == len(p)); 31 | b(p) = best(1); 32 | end 33 | 34 | %b(:) = 1; % debug -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/isptnorm.m: -------------------------------------------------------------------------------- 1 | % isptnorm ISotropic PoinT NORMalization 2 | % 3 | % [xnorm,T] = isptnorm(x); 4 | % x ... [N x dim] coordinates 5 | % 6 | % xnorm ... normalized coordinates 7 | % T ... transformation matrix used 8 | % 9 | % T. Svoboda, 5/2001 10 | 11 | 12 | function [xnorm,T] = isptnorm(x); 13 | 14 | 15 | % data dimension 16 | dim = size(x,2); 17 | N = size(x,1); 18 | 19 | % make homogenous coordinates 20 | x(:,dim+1) = ones(N,1); 21 | 22 | % compute sum of square diferences 23 | for i=1:dim, 24 | ssd(:,i) = (x(:,i)-mean(x(:,i))).^2; 25 | end 26 | 27 | scale = (sqrt(dim)*N) / (sum(sqrt(sum(ssd')))); 28 | 29 | T = zeros(dim+1); 30 | 31 | for i=1:dim, 32 | T(i,i) = scale; 33 | T(i,dim+1) = -scale*mean(x(:,i)); 34 | end 35 | T(dim+1,dim+1) = 1; 36 | 37 | xnorm = T*x'; 38 | xnorm = xnorm'; 39 | % return nonhomogenous part of the points coordinates 40 | xnorm = xnorm(:,1:dim); 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/rq.m: -------------------------------------------------------------------------------- 1 | %RQ Pajdla: Returns a 3x3 upper triangular R and a unitary Q so that X = R*Q 2 | % 3 | % function [R,Q] = rq(X) 4 | % 5 | % X = input matrix, 6 | % Q = unitary matrix 7 | % R = upper triangular matrix 8 | % 9 | % See also QR. 10 | 11 | % Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be 12 | % pajdla@vision.felk.cvut.cz 13 | % 05/28/94 ESAT-MI2, KU Leuven 14 | % Documentation: 15 | % Language: Matlab 4.1, (c) MathWorks 16 | % 17 | function [R,Q] = rq(X) 18 | 19 | [Qt,Rt] = qr(X'); 20 | Rt = Rt'; 21 | Qt = Qt'; 22 | 23 | Qu(1,:) = cross(Rt(2,:),Rt(3,:)); 24 | Qu(1,:) = Qu(1,:)/norm(Qu(1,:)); 25 | 26 | Qu(2,:) = cross(Qu(1,:),Rt(3,:)); 27 | Qu(2,:) = Qu(2,:)/norm(Qu(2,:)); 28 | 29 | Qu(3,:) = cross(Qu(1,:),Qu(2,:)); 30 | 31 | R = Rt * Qu'; 32 | Q = Qu * Qt; 33 | 34 | -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/rq.m: -------------------------------------------------------------------------------- 1 | %RQ Pajdla: Returns a 3x3 upper triangular R and a unitary Q so that X = R*Q 2 | % 3 | % function [R,Q] = rq(X) 4 | % 5 | % X = input matrix, 6 | % Q = unitary matrix 7 | % R = upper triangular matrix 8 | % 9 | % See also QR. 10 | 11 | % Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be 12 | % pajdla@vision.felk.cvut.cz 13 | % 05/28/94 ESAT-MI2, KU Leuven 14 | % Documentation: 15 | % Language: Matlab 4.1, (c) MathWorks 16 | % 17 | function [R,Q] = rq(X) 18 | 19 | [Qt,Rt] = qr(X'); 20 | Rt = Rt'; 21 | Qt = Qt'; 22 | 23 | Qu(1,:) = cross(Rt(2,:),Rt(3,:)); 24 | Qu(1,:) = Qu(1,:)/norm(Qu(1,:)); 25 | 26 | Qu(2,:) = cross(Qu(1,:),Rt(3,:)); 27 | Qu(2,:) = Qu(2,:)/norm(Qu(2,:)); 28 | 29 | Qu(3,:) = cross(Qu(1,:),Qu(2,:)); 30 | 31 | R = Rt * Qu'; 32 | Q = Qu * Qt; 33 | 34 | -------------------------------------------------------------------------------- /CommonCfgAndIO/expname.m: -------------------------------------------------------------------------------- 1 | % EXPeriment NAME 2 | % returns the name of the current experiment 3 | % this name indexes the CONFIGDATA 4 | % 5 | % $Id: expname.m,v 2.7 2005/05/23 16:23:35 svoboda Exp $ 6 | 7 | function name = expname() 8 | 9 | % the name of a calibration fo BlueC must 10 | % contain the following sub-string 'BigBlue' 11 | 12 | % the name for the Hoenggeberg calibration must contain 13 | % the sub-string Hoengg 14 | 15 | % the name of the oscar setup must contain 16 | % the string 'oscar' 17 | 18 | name = 'strawlab_test'; 19 | %name = 'flydra'; 20 | %name = 'mamarama'; 21 | %name = 'humdra'; 22 | % name = '0801BlueCRZ'; 23 | % name = 'BlueCHoengg'; 24 | % name = 'TestData'; 25 | % name = 'OneSideH0105'; 26 | % name = 'OneSideH1416'; 27 | % name = 'BlueCRZ'; 28 | % name = 'ViRoom20030611'; 29 | % name = '2410ViRoom'; 30 | % name = 'Erlangen'; 31 | % name = 'oscar2c1p'; 32 | % name = 'oscardemo'; 33 | % name = 'ViRoom20030724'; 34 | % name = 'extern'; 35 | 36 | return 37 | 38 | 39 | -------------------------------------------------------------------------------- /python/test/test_mcsc.py: -------------------------------------------------------------------------------- 1 | import os, shutil 2 | import pymulticamselfcal.execute 3 | from pymulticamselfcal.execute import MultiCamSelfCal 4 | import tempfile 5 | import pytest 6 | 7 | def _get_src_path(): 8 | """this is a hack to get path to data""" 9 | mydir = os.path.split(__file__)[0] 10 | srcpath = os.path.abspath(os.path.join(mydir,'..','..')) 11 | return srcpath 12 | 13 | def _get_data_path(): 14 | """this is a hack to get path to data""" 15 | datapath = os.path.abspath(os.path.join(_get_src_path(),'strawlab','test-data')) 16 | return datapath 17 | 18 | SRC_PATH = _get_src_path() 19 | DATA_PATH = _get_data_path() 20 | 21 | @pytest.mark.parametrize("path", ['caldata20130726_122220', 'DATA20100906_134124']) 22 | def test_mcsc(path): 23 | calib_dir = os.path.join(DATA_PATH, path ) 24 | mcscdir = os.path.join(SRC_PATH,'MultiCamSelfCal') 25 | mcsc = MultiCamSelfCal(calib_dir, mcscdir=mcscdir ) 26 | target_dir = tempfile.mkdtemp(prefix='result-',dir=calib_dir) 27 | caldir = mcsc.execute() 28 | assert os.path.exists(caldir) 29 | -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/uP2X.m: -------------------------------------------------------------------------------- 1 | % uP2X ... linear reconstruction of 3D points 2 | % from N-perspective views 3 | % 4 | % X = uP2X(Umat,Pmat); 5 | % Umat ... 3*N x n matrix of n homogenous points 6 | % Ps ... 3 x 4*N matrix of projection matrices 7 | % 8 | % X ... 4 x n matrix of homogenous 3D points 9 | % 10 | % Algorithm is based on: Hartley and Zisserman, Multiple 11 | % View Geometry, 2000, pp 297-298 12 | % 13 | % $Id: uP2X.m,v 2.0 2003/06/19 12:07:10 svoboda Exp $ 14 | 15 | function X = uP2X(Umat,Ps); 16 | 17 | N = size(Umat,1)/3; 18 | n = size(Umat,2); 19 | 20 | % reshuffle the Ps matrix 21 | Pmat = []; 22 | for i=1:N; 23 | Pmat = [Pmat;Ps(:,i*4-3:i*4)]; 24 | end 25 | 26 | X = []; 27 | for i=1:n, % for all points 28 | A = []; 29 | for j=1:N, % for all cameras 30 | % create the data matrix 31 | A = [A; Umat(j*3-2,i)*Pmat(j*3,:) - Pmat(j*3-2,:); Umat(j*3-1,i)*Pmat(j*3,:) - Pmat(j*3-1,:)]; 32 | end 33 | [u,s,v] = svd(A); 34 | X = [X,v(:,end)]; 35 | end 36 | % normalize reconstructed points 37 | X = X./repmat(X(4,:),4,1); 38 | 39 | return; 40 | -------------------------------------------------------------------------------- /RadialDistortions/undoradial.m: -------------------------------------------------------------------------------- 1 | % undoradial remove radial distortion 2 | % 3 | % [xl] = undoradial(x,K,kc) 4 | % 5 | % x ... 3xN coordinates of the distorted pixel points 6 | % K ... 3x3 camera calibration matrix 7 | % kc ... 4x1 vector of distortion parameters 8 | % 9 | % xl ... linearized pixel coordinates 10 | % these coordinates should obey the linear pinhole model 11 | % 12 | % It calls comp_distortion_oulu: undistort pixel coordinates. 13 | % function taken from the CalTech camera calibration toolbox 14 | 15 | function [xl] = undoradial(x_kk,K,kc) 16 | 17 | cc(1) = K(1,3); 18 | cc(2) = K(2,3); 19 | fc(1) = K(1,1); 20 | fc(2) = K(2,2); 21 | 22 | % First: Subtract principal point, and divide by the focal length: 23 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 24 | 25 | if norm(kc) ~= 0, 26 | % Third: Compensate for lens distortion: 27 | xn = comp_distortion_oulu(x_distort,kc); 28 | else 29 | xn = x_distort; 30 | end; 31 | 32 | % back to the linear pixel coordinates 33 | xl = K*[xn;ones(size(xn(1,:)))]; 34 | -------------------------------------------------------------------------------- /RadialDistortions/readradfile.m: -------------------------------------------------------------------------------- 1 | % emacs, this is -*-Matlab-*- mode 2 | % 3 | % readradfile reads the BlueC *.rad files 4 | % 5 | % *.rad files contain paprameters of the radial distortion 6 | % [K,kc] = readradfile(name) 7 | % name ... name of the *.rad file with its full path 8 | % 9 | % K ... 3x3 calibration matrix 10 | % kc ... 4x1 vector of distortion parameters 11 | % 12 | % $Id: readradfile.m,v 2.0 2003/06/19 12:07:16 svoboda Exp $ 13 | function [K,kc] = readradfile(name); 14 | 15 | fid = fopen(name,'r'); 16 | if fid<0 17 | error(sprintf('Could not open %s. Missing rad files?',name')) 18 | end 19 | 20 | for i=1:3, 21 | for j=1:3, 22 | buff = fgetl(fid); 23 | str_end = buff(7:end); 24 | if str_end(end)==';' 25 | str_end = str_end(1:end-1); 26 | end 27 | K(i,j) = str2num(str_end); 28 | end 29 | end 30 | 31 | buff = fgetl(fid); 32 | for i=1:4, 33 | buff = fgetl(fid); 34 | str_end = buff(7:end); 35 | if str_end(end)==';' 36 | str_end = str_end(1:end-1); 37 | end 38 | kc(i) = str2num(str_end); 39 | end 40 | 41 | fclose(fid); 42 | 43 | return; 44 | 45 | 46 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/normalize_mp.m: -------------------------------------------------------------------------------- 1 | % 2 | % Function Mnorm=normalize_mp(M,I) 3 | % 4 | % normalizes M by dividing each point by its homogenous coordinate 5 | % (these coordinates equal to ones afterwards). 6 | % 7 | % Parameter I can be omitted for complete scenes. 8 | 9 | function Mnorm=normalize_mp(M,I) 10 | 11 | m=size(M,1)/3; 12 | n=size(M,2); 13 | 14 | if nargin < 2 15 | I=ones(m,n); 16 | end 17 | 18 | Mnorm(1:3*m, 1:n) = M; % There are two reasons for this. (i) Make NaN the 19 | % unknown data thus the whole matrix has to be filled 20 | % by NaNs. (ii) Sometimes it happens that when there 21 | % is a missing data in the last column(s), the 22 | % column(s) disapppears. 23 | 24 | known = find(I); 25 | big_enough = known(find( abs(M(known*3)) > eps )); 26 | if m == 1, big_enough = big_enough'; end 27 | 28 | if ~isempty(big_enough) 29 | div_by = repmat(M(big_enough*3)',3,1); 30 | Mnorm(k2i(big_enough)) = M(k2i(big_enough)) ./ div_by(:); 31 | end 32 | 33 | Mnorm(setdiff(known, big_enough)*3) = 1; 34 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/depth_estimation.m: -------------------------------------------------------------------------------- 1 | %depths_estimation Determine scale factors (proj. depths) of PRMM. 2 | 3 | function [lambda, Ilamb] = depth_estimation(M,F,ep,rows,central) 4 | 5 | [m n] = size(M); m = m/3; 6 | 7 | if central 8 | j = central; 9 | ps = 1:n; 10 | Ilamb(j,:) = ~isnan(M(3*j,:)); 11 | else 12 | j = 1; 13 | b = subseq_longest(~isnan(M(1:3:end,:))); 14 | for p = 1:n 15 | Ilamb(b(p),p) = ~isnan(M(3*b(p),p)); % =1 is sufficient if b(p) is correct 16 | end 17 | end 18 | 19 | lambda = ones(m,n); 20 | 21 | for i = setdiff(1:m, j) 22 | if ~central 23 | j = i-1; 24 | ps = find(b <= j); 25 | end 26 | G = reshape(F(rows(i),rows(j),1:3,1:3),3,3); 27 | epip = reshape(ep(rows(i),rows(j),1:3),3,1); 28 | for p = ps 29 | Ilamb(i,p) = Ilamb(j,p) & ~isnan(M(3*i,p)); 30 | 31 | if Ilamb(i,p) 32 | u = cross(epip,M(3*i-2:3*i,p)); %q(i,p) 33 | v = G*M(3*j-2:3*j,p); %q(j,p) 34 | lambda(i,p) = u'*v/norm(u)^2*lambda(j,p); 35 | else 36 | lambda(i,p) = 1; %so that it's possible to recover scene at the end 37 | end 38 | end 39 | end 40 | -------------------------------------------------------------------------------- /MultiCamSelfCal/FindingPoints/virooms.pm: -------------------------------------------------------------------------------- 1 | $process={"compname"=>"viroom00","camIds"=>[3,4],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"viroom00","camIds"=>[5,6],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | $process={"compname"=>"viroom01","camIds"=>[7,8],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 6 | push(@processes,$process); 7 | $process={"compname"=>"viroom01","camIds"=>[9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 8 | push(@processes,$process); 9 | $process={"compname"=>"viroom02","camIds"=>[11,12],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 10 | push(@processes,$process); 11 | $process={"compname"=>"viroom02","camIds"=>[14],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 12 | push(@processes,$process); 13 | $process={"compname"=>"viroom03","camIds"=>[15,16],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 14 | push(@processes,$process); 15 | $process={"compname"=>"viroom03","camIds"=>[17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 16 | push(@processes,$process); 17 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fFDs.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define f1 F(0) 5 | #define f2 F(1) 6 | #define f3 F(2) 7 | #define f4 F(3) 8 | #define f5 F(4) 9 | #define f6 F(5) 10 | #define f7 F(6) 11 | #define f8 F(7) 12 | #define f9 F(8) 13 | 14 | #define u1 u(uinc+0) 15 | #define u2 u(uinc+1) 16 | #define u4 u(uinc+3) 17 | #define u5 u(uinc+4) 18 | 19 | DEFUN_DLD (fFDs, args, , 20 | "[...] = fFDs (...)\n\ 21 | \n\ 22 | fast FDs routine.") 23 | { 24 | 25 | ColumnVector F (args(0).vector_value()); 26 | ColumnVector u (args(1).vector_value()); 27 | 28 | int len = u.length(); 29 | ColumnVector p (len); 30 | 31 | double rx, ry, rwc, ryc, rxc, r; 32 | int i; 33 | int uinc=0; 34 | 35 | printf("fFDs\n"); 36 | 37 | for (i=0; i"viroom00","camIds"=>[3,4],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"viroom00","camIds"=>[5,6],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | $process={"compname"=>"viroom01","camIds"=>[7,8],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 6 | push(@processes,$process); 7 | $process={"compname"=>"viroom01","camIds"=>[9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 8 | push(@processes,$process); 9 | $process={"compname"=>"viroom02","camIds"=>[11,12],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 10 | push(@processes,$process); 11 | $process={"compname"=>"viroom02","camIds"=>[14],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 12 | push(@processes,$process); 13 | $process={"compname"=>"viroom03","camIds"=>[15,16],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 14 | push(@processes,$process); 15 | $process={"compname"=>"viroom03","camIds"=>[17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 16 | push(@processes,$process); 17 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/drawobject.m: -------------------------------------------------------------------------------- 1 | % drawobject ... draws a (calibration) object of a specific type 2 | % 3 | % [fig] = drawobject(X, ctype, fig, {color}) 4 | % 5 | % X ...... 3xn matrix containg the object's corner points 6 | % if X is 4xn only the first 3 rows are used 7 | % ctype .. a string specifying the type of the object 8 | % according to the type the appropriate drawing functions is called 9 | % supported types are: 10 | % 'cube' - a cube 11 | % 'octagon' - a planar octagon 12 | % 'cloud' - a point cloud 13 | % fig .... figure handle 14 | % color .. color of plotting; defaults to blue 15 | % 16 | % fig .... returns the figure handle 17 | % 18 | % $Id: drawobject.m,v 2.0 2003/06/19 12:07:02 svoboda Exp $ 19 | 20 | function [fig] = drawobject(X,ctype,fig,color) 21 | 22 | if nargin < 4 23 | color = 'b'; % default color 24 | end 25 | 26 | switch ctype 27 | case 'cube', 28 | drawcube(X,fig,color); 29 | case 'octagon', 30 | drawoctagon(X,fig,color); 31 | case 'cloud', 32 | drawcloud(X,fig,color); 33 | otherwise, 34 | error('unknown object type: ', ctype) 35 | end 36 | 37 | return 38 | -------------------------------------------------------------------------------- /MultiCamValidation/InputOutputFunctions/drawobject.m: -------------------------------------------------------------------------------- 1 | % drawobject ... draws a (calibration) object of a specific type 2 | % 3 | % [fig] = drawobject(X, ctype, fig, {color}) 4 | % 5 | % X ...... 3xn matrix containg the object's corner points 6 | % if X is 4xn only the first 3 rows are used 7 | % ctype .. a string specifying the type of the object 8 | % according to the type the appropriate drawing functions is called 9 | % supported types are: 10 | % 'cube' - a cube 11 | % 'octagon' - a planar octagon 12 | % 'cloud' - a point cloud 13 | % fig .... figure handle 14 | % color .. color of plotting; defaults to blue 15 | % 16 | % fig .... returns the figure handle 17 | % 18 | % $Id: drawobject.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $ 19 | 20 | function [fig] = drawobject(X,ctype,fig,color) 21 | 22 | if nargin < 4 23 | color = 'b'; % default color 24 | end 25 | 26 | switch ctype 27 | case 'cube', 28 | drawcube(X,fig,color); 29 | case 'octagon', 30 | drawoctagon(X,fig,color); 31 | case 'cloud', 32 | drawcloud(X,fig,color); 33 | otherwise, 34 | error('unknown object type: ', ctype) 35 | end 36 | 37 | return 38 | -------------------------------------------------------------------------------- /RadialDistortions/undoheikk.m: -------------------------------------------------------------------------------- 1 | function p=imcorr(sys,par,dp) 2 | %IMCORR corrects image coordinates, which are contaminated by radial 3 | %and tangential distortion. 4 | % 5 | %Usage: 6 | % p=imcorr(name,par2,dp) 7 | % 8 | %where 9 | % name = string that is specific to the camera and the framegrabber. 10 | % This string must be defined in configc.m 11 | % par2 = camera intrinsic parameters for correcting the coordinates. 12 | % these parameters are computed by using invmodel.m. 13 | % dp = distorted image coordinates in pixels (n x 2 matrix) 14 | % p = corrected image coordinates 15 | 16 | % Version 3.0 10-17-00 17 | % Janne Heikkila, University of Oulu, Finland 18 | 19 | NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4); 20 | Asp=par(1); Foc=par(2); 21 | Cpx=par(3); Cpy=par(4); 22 | Rad1=par(5); Rad2=par(6); 23 | Tan1=par(7); Tan2=par(8); 24 | 25 | 26 | dx=(dp(:,1)-Cpx)*Sx/NDX/Asp; 27 | dy=(dp(:,2)-Cpy)*Sy/NDY; 28 | 29 | r2=dx.*dx+dy.*dy; 30 | delta=Rad1*r2+Rad2*r2.*r2; 31 | 32 | cx=dx.*(1+delta)+2*Tan1*dx.*dy+Tan2*(r2+2*dx.*dx); 33 | cy=dy.*(1+delta)+Tan1*(r2+2*dy.*dy)+2*Tan2*dx.*dy; 34 | 35 | p=NDX*Asp*cx/Sx+Cpx; 36 | p(:,2)=NDY*cy/Sy+Cpy; 37 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fFDs.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mex.h" 3 | #include "matrix.h" 4 | 5 | #define f1 (*F) 6 | #define f2 (*(F+1)) 7 | #define f3 (*(F+2)) 8 | #define f4 (*(F+3)) 9 | #define f5 (*(F+4)) 10 | #define f6 (*(F+5)) 11 | #define f7 (*(F+6)) 12 | #define f8 (*(F+7)) 13 | #define f9 (*(F+8)) 14 | 15 | #define u1 (*(u)) 16 | #define u2 (*(u+1)) 17 | #define u4 (*(u+3)) 18 | #define u5 (*(u+4)) 19 | 20 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 21 | { 22 | double *F = mxGetPr(aSrcP[0]); 23 | double *u = mxGetPr(aSrcP[1]); 24 | double *p; 25 | double rx, ry, rwc, ryc, rxc, r; 26 | int len = mxGetN(aSrcP[1]); 27 | int i; 28 | 29 | aDstP[0] = mxCreateDoubleMatrix(1, len, mxREAL); 30 | p = (double *)mxGetData(aDstP[0]); 31 | 32 | for (i=1; i<=len; i++) 33 | { 34 | rxc = f1 * u4 + f4 * u5 + f7; 35 | ryc = f2 * u4 + f5 * u5 + f8; 36 | rwc = f3 * u4 + f6 * u5 + f9; 37 | r =(u1 * rxc + u2 * ryc + rwc); 38 | rx = f1 * u1 + f2 * u2 + f3; 39 | ry = f4 * u1 + f5 * u2 + f6; 40 | 41 | *p = r*r / (rxc*rxc + ryc*ryc + rx*rx + ry*ry); 42 | p ++; 43 | u += 6; 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/fFDs.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mex.h" 3 | #include "matrix.h" 4 | 5 | #define f1 (*F) 6 | #define f2 (*(F+1)) 7 | #define f3 (*(F+2)) 8 | #define f4 (*(F+3)) 9 | #define f5 (*(F+4)) 10 | #define f6 (*(F+5)) 11 | #define f7 (*(F+6)) 12 | #define f8 (*(F+7)) 13 | #define f9 (*(F+8)) 14 | 15 | #define u1 (*(u)) 16 | #define u2 (*(u+1)) 17 | #define u4 (*(u+3)) 18 | #define u5 (*(u+4)) 19 | 20 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 21 | { 22 | double *F = mxGetPr(aSrcP[0]); 23 | double *u = mxGetPr(aSrcP[1]); 24 | double *p; 25 | double rx, ry, rwc, ryc, rxc, r; 26 | int len = mxGetN(aSrcP[1]); 27 | int i; 28 | 29 | aDstP[0] = mxCreateDoubleMatrix(1, len, mxREAL); 30 | p = (double *)mxGetData(aDstP[0]); 31 | 32 | for (i=1; i<=len; i++) 33 | { 34 | rxc = f1 * u4 + f4 * u5 + f7; 35 | ryc = f2 * u4 + f5 * u5 + f8; 36 | rwc = f3 * u4 + f6 * u5 + f9; 37 | r =(u1 * rxc + u2 * ryc + rwc); 38 | rx = f1 * u1 + f2 * u2 + f3; 39 | ry = f4 * u1 + f5 * u2 + f6; 40 | 41 | *p = r*r / (rxc*rxc + ryc*ryc + rx*rx + ry*ry); 42 | p ++; 43 | u += 6; 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/utils/eucl_dist_only.m: -------------------------------------------------------------------------------- 1 | %eucl_dist_only Return Eucledian norm of the difference between two scenes. 2 | % 3 | % [e_norm, distances] = eucl_dist_only(M0, M [,I [,step]]) 4 | % 5 | % Parameters: 6 | % step .. same as in k2i (see help k2i) 7 | % default: step = 2 i.e. inhomogenous coordinates are assumed 8 | % 9 | % distances .. Eucledian distances between each known elements of M0 and M 10 | 11 | function [e_norm, distances] = eucl_dist_only(M0, M, I, step) 12 | 13 | if nargin < 3, I = ~isnan(M0(1:2:end,:)) && ~isnan(M(1:2:end,:)); end 14 | if nargin < 4, step = 2; end 15 | 16 | if nargin >= 3 17 | m = size(M,1)/step; 18 | if size(I,1) ~= m && ~isempty(M) 19 | disp(sprintf(['!!! Warning: eucl_dist_only:' ' the height of I is' ... 20 | ' bad, it should be equal to %d'], m)); keyboard;end 21 | end 22 | 23 | known = find(I); 24 | diff = (M0(k2i(known,step)) - M(k2i(known,step))) .^ 2; 25 | 26 | % sqrt must be performed on each point separately 27 | for s = 1:step 28 | B(s,:) = diff(s:step:end)'; % x-coordinates 29 | B(s,:) = diff(s:step:end)'; % y-coordinates 30 | end 31 | distances = sqrt(sum(B)); 32 | e_norm = sum(distances); 33 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/reprerror.m: -------------------------------------------------------------------------------- 1 | % reprerror Estimate reprojection error 2 | % 3 | % [cam] = reprerror(cam,Pe,Xe,FRAMES,inliers); 4 | % 5 | % $Id: reprerror.m,v 2.0 2003/06/19 12:06:50 svoboda Exp $ 6 | 7 | function [cam] = reprerror(cam,Pe,Xe,FRAMES,inliers); 8 | 9 | CAMS = size(Pe,1)/3; 10 | 11 | for i=1:CAMS, 12 | xe = Pe(((3*i)-2):(3*i),:)*Xe; 13 | cam(i).xe = xe./repmat(xe(3,:),3,1); 14 | % these points were the input into Martinec and Pajdla filling 15 | mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far 16 | mask.vis = zeros(1,FRAMES); % maks of visible points 17 | mask.rec(inliers.idx) = 1; 18 | mask.vis(cam(i).idlin) = 1; 19 | mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera 20 | unmask.rec = cumsum(mask.rec); 21 | unmask.vis = cumsum(mask.vis); 22 | cam(i).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); 23 | cam(i).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); 24 | cam(i).err2d = sqrt(sum([cam(i).xe(1:2,cam(i).recandvis) - cam(i).xgt(1:2,cam(i).visandrec)].^2)); 25 | cam(i).mean2Derr = mean(cam(i).err2d); 26 | cam(i).std2Derr = std(cam(i).err2d); 27 | end 28 | 29 | return; 30 | -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/nfi2r.m: -------------------------------------------------------------------------------- 1 | % nfi2r Computes rotation matrix, axis of rotation and the angle are given 2 | % 3 | % R = nfi2r(n,fi) 4 | % Inputs: 5 | % n[3x1] ... axis of rotation (vector of direction) 6 | % fi[rad] ... angle of rotation (counter clockwise) 7 | % Return: 8 | % R ... rotation matrix 9 | % 10 | % T. Svoboda, 3/1998, CMP Prague 11 | % 12 | % $Id: nfi2r.m,v 1.1 2003/07/03 15:38:40 svoboda Exp $ 13 | 14 | % page 203 of the book: 15 | % @BOOK{Kanatani90, 16 | % AUTHOR = {Kanatani, Kenichi}, 17 | % PUBLISHER = {Springer-{V}erlag}, 18 | % TITLE = {Group-{T}heoretical Methods in Image Understanding}, 19 | % YEAR = {1990}, 20 | % HARDCOPY = { CMPlib.book.BC14 }, 21 | % SIGNATURE = {X-copy}, 22 | % ISSN_ISBN = {3-540-51263-5}, 23 | % 24 | 25 | function R = nfi2r(n,fi) 26 | 27 | n = n./norm(n,2); 28 | cfi = cos(fi); 29 | sfi = sin(fi); 30 | 31 | R(1,1:3) = [ cfi+n(1)^2*(1-cfi), n(1)*n(2)*(1-cfi)-n(3)*sfi, n(1)*n(3)*(1-cfi)+n(2)*sfi ]; 32 | R(2,1:3) = [ n(1)*n(2)*(1-cfi)+n(3)*sfi, cfi+n(2)^2*(1-cfi), n(2)*n(3)*(1-cfi)-n(1)*sfi ]; 33 | R(3,1:3) = [ n(3)*n(1)*(1-cfi)-n(2)*sfi, n(3)*n(2)*(1-cfi)+n(1)*sfi, cfi+n(3)^2*(1-cfi) ]; 34 | 35 | R = R'; % due to reverse notation of Kanatani 36 | 37 | return 38 | -------------------------------------------------------------------------------- /RansacM/u2Fdlt.m: -------------------------------------------------------------------------------- 1 | function F = u2Fdlt(u,do_norm) 2 | % u2Fdlt linear estimation of the Fundamental matrix 3 | % from point correspondences 4 | % 5 | % H = u2Fdlt(u,{do_norm=1}) 6 | % u ... {4|6}xN corresponding coordinates 7 | % do_norm .. do isotropic normalization of points? 8 | % 9 | % F ... 3x3 fundamental matrix 10 | % 11 | % $Id: u2Fdlt.m,v 1.1 2005/05/23 16:16:01 svoboda Exp $ 12 | 13 | NoPoints = size(u,2); 14 | 15 | if nargin < 2 16 | do_norm=1; 17 | end 18 | 19 | u = u'; 20 | 21 | % parse the input parameters 22 | if NoPoints<8 23 | error('Too few correspondences') 24 | end 25 | 26 | if size(u,2) == 4, 27 | % make the homogenous coordinates 28 | u = [u(:,1:2), ones(size(u(:,1))), u(:,3:4), ones(size(u(:,1)))]; 29 | end 30 | 31 | u1 = u(:,1:3); 32 | u2 = u(:,4:6); 33 | 34 | if do_norm 35 | [u1,T1] = pointnormiso(u1'); 36 | u1 = u1'; 37 | [u2,T2] = pointnormiso(u2'); 38 | u2 = u2'; 39 | end 40 | 41 | % create the data matrix 42 | A = zeros(NoPoints,9); 43 | for i=1:NoPoints % create equations 44 | for j=1:3 45 | for k=1:3 46 | A(i,(j-1)*3+k)=u2(i,j)*u1(i,k); 47 | end 48 | end 49 | end 50 | 51 | [U,S,V] = svd(A); 52 | f = V(:,size(V,2)); 53 | F = reshape(f,3,3)'; 54 | 55 | if do_norm 56 | F = inv(T2)*F*T1; 57 | end 58 | 59 | return; 60 | -------------------------------------------------------------------------------- /CalTechCal/comp_error_calib.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% 2 | 3 | check_active_images; 4 | 5 | % Reproject the patterns on the images, and compute the pixel errors: 6 | 7 | ex = []; % Global error vector 8 | x = []; % Detected corners on the image plane 9 | y = []; % Reprojected points 10 | 11 | if ~exist('alpha_c'), 12 | alpha_c = 0; 13 | end; 14 | 15 | for kk = 1:n_ima, 16 | 17 | eval(['omckk = omc_' num2str(kk) ';']); 18 | eval(['Tckk = Tc_' num2str(kk) ';']); 19 | 20 | if active_images(kk) & (~isnan(omckk(1,1))), 21 | 22 | %Rkk = rodrigues(omckk); 23 | 24 | eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); 25 | 26 | eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); 27 | 28 | eval(['x_kk = x_' num2str(kk) ';']); 29 | 30 | eval(['ex = [ex ex_' num2str(kk) '];']); 31 | eval(['x = [x x_' num2str(kk) '];']); 32 | eval(['y = [y y_' num2str(kk) '];']); 33 | 34 | else 35 | 36 | % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); 37 | 38 | 39 | % If inactivated image, the error does not make sense: 40 | eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); 41 | 42 | end; 43 | 44 | end; 45 | 46 | err_std = std(ex')'; 47 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: test 2 | 3 | on: 4 | push: 5 | branches: ["**"] 6 | pull_request: 7 | branches: ["**"] 8 | schedule: 9 | # At 23:25 on Thursday. 10 | - cron: "25 23 * * 4" 11 | 12 | jobs: 13 | test-octave-only: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-20.04, ubuntu-latest] 18 | steps: 19 | - uses: actions/checkout@v3 20 | - name: Install octave 21 | run: sudo apt update && sudo apt install -y octave 22 | - name: Run test 23 | run: cd MultiCamSelfCal && octave gocal.m --config=../strawlab/test-data/DATA20100906_134124/no-global-iterations.cfg 24 | 25 | test-python: 26 | runs-on: ${{ matrix.os }} 27 | strategy: 28 | matrix: 29 | os: [ubuntu-20.04, ubuntu-latest] 30 | python-version: [ '3.8', '3.x' ] 31 | steps: 32 | - uses: actions/checkout@v3 33 | - name: Install octave 34 | run: sudo apt update && sudo apt install -y octave 35 | - name: Setup python 36 | uses: actions/setup-python@v4 37 | with: 38 | python-version: ${{ matrix.python-version }} 39 | - name: Install Python MCSC wrapper 40 | run: pip install . -v 41 | - name: Install pytest testing framework 42 | run: pip install pytest 43 | - name: Test Python wrapper 44 | run: pytest 45 | -------------------------------------------------------------------------------- /MultiCamSelfCal/BlueCLocal/align_existing_camera_centers.m: -------------------------------------------------------------------------------- 1 | function [align] = align_existing_camera_centers(in,config) 2 | 3 | Cst = in.Cst; 4 | Rot = in.Rot; 5 | 6 | Cam = load([config.paths.data,'original_cam_centers.dat'],'-ASCII'); 7 | 8 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 9 | if ~Octave, 10 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom (no sRt)',config.cal.cams2use); 11 | end 12 | 13 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst'],[Cam']); 14 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 15 | % save aligned data 16 | [align.Cst,align.Rot] = savecalpar(align.P,config); 17 | 18 | if ~Octave, 19 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 20 | 21 | set(gca,'CameraTarget',[0,0,0]); 22 | set(gca,'CameraPosition',[0,0,1]); 23 | 24 | figure(61), 25 | % print -depsc graphevalaligned.eps 26 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 27 | 28 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 29 | 30 | set(gca,'CameraTarget',[0,0,0.9]); 31 | set(gca,'CameraPosition',[2,0,0.9]); 32 | 33 | figure(62), 34 | % print -depsc graphevalaligned.eps 35 | eval(['print -depsc ', config.paths.data, 'sideview.eps']) 36 | end 37 | 38 | return 39 | -------------------------------------------------------------------------------- /CalTechCal/begin-attempt-initialize-rad.patch: -------------------------------------------------------------------------------- 1 | Index: CalTechCal/goradf.m 2 | =================================================================== 3 | --- CalTechCal/goradf.m (revision 2506) 4 | +++ CalTechCal/goradf.m (working copy) 5 | @@ -32,8 +32,27 @@ 6 | % handle image resolutions correctly 7 | nx = config.cal.Res(count,1); 8 | ny = config.cal.Res(count,2); 9 | + % ADS start ----- 10 | + % Begin from prior conditions rather than blank initialization 11 | + fprintf(1,'ADS: nx = %d, ny = %d\n',nx,ny); 12 | + rad_fname = sprintf(config.files.rad,config.cal.cams2use(i)) 13 | + [K,kc] = readradfile(rad_fname); 14 | + fprintf(1,'ADS: loaded rad file %s',rad_fname); 15 | +% recompute_extrinsic=0; 16 | +% est_aspect_ratio=1; % estimate aspect ratio for distortions 17 | +% center_optim=1; % estimate center point for distortions 18 | +% est_alpha=0; % don't estimate skew 19 | + K 20 | + kc 21 | + kc = kc'; 22 | + cc = [K(1,3);K(2,3);] 23 | + est_fc = [1;1]; % estimate focal length 24 | + fc = [K(1,1);K(2,2);] 25 | +% check_cond=0; % dont check for ill-conditioned image 26 | + % ADS end ------ 27 | go_calib_optim_iter 28 | if any(isnan(param)) | any(err_std > 2*INL_TOL) 29 | + fprintf(1,'ADS: failed caltech cal!!!!!!!!!!!!!!!!!!!!\n'); 30 | % when the iteration fails insert null distortion 31 | % it is better than nonsense 32 | KK = [700 0 320; 0 700 240; 0 0 1]; % void calibration matrix 33 | -------------------------------------------------------------------------------- /CalTechCal/normalize.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize(x_kk,fc,cc,kc,alpha_c), 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | % 20 | %comp_distortion_oulu: undistort pixel coordinates. 21 | 22 | if nargin < 5, 23 | alpha_c = 0; 24 | if nargin < 4; 25 | kc = [0;0;0;0;0]; 26 | if nargin < 3; 27 | cc = [0;0]; 28 | if nargin < 2, 29 | fc = [1;1]; 30 | end; 31 | end; 32 | end; 33 | end; 34 | 35 | 36 | % First: Subtract principal point, and divide by the focal length: 37 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 38 | 39 | % Second: undo skew 40 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 41 | 42 | if norm(kc) ~= 0, 43 | % Third: Compensate for lens distortion: 44 | xn = comp_distortion_oulu(x_distort,kc); 45 | else 46 | xn = x_distort; 47 | end; 48 | -------------------------------------------------------------------------------- /CalTechCal/comp_distortion_oulu.m: -------------------------------------------------------------------------------- 1 | function [x] = comp_distortion_oulu(xd,k); 2 | 3 | %comp_distortion_oulu.m 4 | % 5 | %[x] = comp_distortion_oulu(xd,k) 6 | % 7 | %Compensates for radial and tangential distortion. Model From Oulu university. 8 | %For more informatino about the distortion model, check the forward projection mapping function: 9 | %project_points.m 10 | % 11 | %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 12 | % k: Distortion coefficients (radial and tangential) (4x1 vector) 13 | % 14 | %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 15 | % 16 | %Method: Iterative method for compensation. 17 | % 18 | %NOTE: This compensation has to be done after the subtraction 19 | % of the principal point, and division by the focal length. 20 | 21 | 22 | if length(k) == 1, 23 | 24 | [x] = comp_distortion(xd,k); 25 | 26 | else 27 | 28 | k1 = k(1); 29 | k2 = k(2); 30 | k3 = k(5); 31 | p1 = k(3); 32 | p2 = k(4); 33 | 34 | x = xd; % initial guess 35 | 36 | for kk=1:20, 37 | 38 | r_2 = sum(x.^2); 39 | k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; 40 | delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); 41 | p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; 42 | x = (xd - delta_x)./(ones(2,1)*k_radial); 43 | 44 | end; 45 | 46 | end; 47 | 48 | -------------------------------------------------------------------------------- /RadialDistortions/comp_distortion_oulu.m: -------------------------------------------------------------------------------- 1 | function [x] = comp_distortion_oulu(xd,k); 2 | 3 | %comp_distortion_oulu.m 4 | % 5 | %[x] = comp_distortion_oulu(xd,k) 6 | % 7 | %Compensates for radial and tangential distortion. Model From Oulu university. 8 | %For more informatino about the distortion model, check the forward projection mapping function: 9 | %project_points.m 10 | % 11 | %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 12 | % k: Distortion coefficients (radial and tangential) (4x1 vector) 13 | % 14 | %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 15 | % 16 | %Method: Iterative method for compensation. 17 | % 18 | %NOTE: This compensation has to be done after the subtraction 19 | % of the principal point, and division by the focal length. 20 | 21 | 22 | if length(k) == 1, 23 | 24 | [x] = comp_distortion(xd,k); 25 | 26 | else 27 | 28 | k1 = k(1); 29 | k2 = k(2); 30 | k3 = k(5); 31 | p1 = k(3); 32 | p2 = k(4); 33 | 34 | x = xd; % initial guess 35 | 36 | for kk=1:20, 37 | 38 | r_2 = sum(x.^2); 39 | k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; 40 | delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); 41 | p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; 42 | x = (xd - delta_x)./(ones(2,1)*k_radial); 43 | 44 | end; 45 | 46 | end; 47 | 48 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/M2Fe.m: -------------------------------------------------------------------------------- 1 | %M2Fe Estimate epipolar geometry of MM in sequence or using the central image. 2 | % 3 | % [F,e,rows,nonrows] = M2Fe(M, central) 4 | % 5 | % Parametres: 6 | % central ... If zero, the concept of sequence is used otherwise the 7 | % concept of the central image is used with central 8 | % image number ``central''. 9 | 10 | function [F,ep,rows,nonrows] = M2Fe(M, central) 11 | 12 | m = size(M,1)/3; nonrows = []; F = []; ep = []; 13 | 14 | if central, rows = [1:central-1, central+1:m]; 15 | else, rows = [2:m]; end 16 | 17 | %estimate the fundamental matrices and epipoles with the method of [Har95] 18 | for k = rows 19 | if central, j = central; 20 | else, j = k-1; end 21 | G = u2FI([M(3*k-2:3*k,:);M(3*j-2:3*j,:)], 'donorm'); 22 | if G==0 23 | rows = setdiff(rows,k); 24 | nonrows = [nonrows k]; 25 | else 26 | %ep=null(G'); %it must be transposed otherwise it's the second epipole 27 | % sometimes returns empty matrix => compute it "by hand" by svd 28 | [u,s,v] = svd(G); 29 | epip = u(:,3); 30 | 31 | F(k,j,1:3,1:3) = G; 32 | ep(k,j,1:3) = epip; 33 | end 34 | end 35 | 36 | if central, rows = union(rows, central); 37 | else rows = [1 rows]; end 38 | 39 | if ~isempty(nonrows) && ~central % find the longest continuous subsequence 40 | I_(rows,1) = 1; 41 | [b, len] = subseq_longest(I_); 42 | rows = b:b+len-1; 43 | nonrows = setdiff(1:m, rows); 44 | end 45 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "multicamselfcal" 3 | version = "0.3.2" 4 | description = "wrapper for the Octave/Matlab multicamselfcal library" 5 | authors = [{ name = "John Stowers"}] 6 | maintainers = [{ name = "Andrew Straw", email = "strawman@astraw.com" }] 7 | readme = "README.md" 8 | requires-python = ">=3.8" 9 | urls.homepage = "https://github.com/strawlab/multicamselfcal" 10 | 11 | dependencies = [ 12 | "numpy", 13 | "PyYAML", 14 | "importlib-resources >= 6", 15 | ] 16 | 17 | [build-system] 18 | requires = ["setuptools"] 19 | build-backend = "setuptools.build_meta" 20 | 21 | [tool.setuptools] 22 | packages = [ 23 | "pymulticamselfcal", # Python wrapper 24 | "MultiCamSelfCal", # Octave/Matlab MCSC code 25 | "MultiCamSelfCal.BlueCLocal", 26 | "MultiCamSelfCal.CoreFunctions", 27 | "MultiCamSelfCal.FindingPoints", 28 | "MultiCamSelfCal.LocalAlignments", 29 | "MultiCamSelfCal.MartinecPajdla", 30 | "MultiCamSelfCal.MartinecPajdla.fill_mm", 31 | "MultiCamSelfCal.MartinecPajdla.fill_mm_test", 32 | "MultiCamSelfCal.MartinecPajdla.utils", 33 | "MultiCamSelfCal.OutputFunctions", 34 | "MultiCamSelfCal.Ransac", 35 | "CommonCfgAndIO", # Octave/Matlab MCSC code 36 | "RadialDistortions", # Octave/Matlab MCSC code 37 | "CalTechCal", # Octave/Matlab MCSC code 38 | "RansacM", # Octave/Matlab MCSC code 39 | ] 40 | 41 | [tool.setuptools.package-data] 42 | # include all .m files as package data for python 43 | "*" = ["*.m"] 44 | -------------------------------------------------------------------------------- /MultiCamSelfCal/BlueCLocal/humdra.m: -------------------------------------------------------------------------------- 1 | function [align] = humdra(in,config) 2 | 3 | Cst = in.Cst; 4 | Rot = in.Rot; 5 | 6 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 7 | if ~Octave, 8 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom (no sRt)',config.cal.cams2use); 9 | end 10 | 11 | % definition of the absolute world frame 12 | 13 | %% for hummingbird cage 14 | % 15 | cam(1).C = [-4600 -100 2200]'; 16 | cam(2).C = [-4600 3100 2200]'; 17 | cam(3).C = [340 -100 2350]'; 18 | cam(4).C = [380 3100 2450]'; 19 | 20 | % of the similarity computation 21 | 22 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst'],[cam(:).C]); 23 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 24 | % save aligned data 25 | if 1 % SAVE_STEPHI | SAVE_PGUHA 26 | [align.Cst,align.Rot] = savecalpar(align.P,config); 27 | end 28 | 29 | if ~Octave, 30 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 31 | 32 | set(gca,'CameraTarget',[0,0,0]); 33 | set(gca,'CameraPosition',[0,0,1]); 34 | 35 | figure(61), 36 | % print -depsc graphevalaligned.eps 37 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 38 | 39 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 40 | 41 | set(gca,'CameraTarget',[0,0,0.9]); 42 | set(gca,'CameraPosition',[2,0,0.9]); 43 | 44 | %figure(62), 45 | % print -depsc graphevalaligned.eps 46 | %eval(['print -depsc ', config.paths.data, 'sideview.eps']) 47 | end 48 | 49 | return 50 | -------------------------------------------------------------------------------- /BlueCFindingPoints/findpointsBlueC: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # convert the *PVI files from on a local discs and copy only the *.jpg files 4 | 5 | echo "Tool to process PVI sequences, Tomas Svoboda, ETHZ, 07/2002" 6 | 7 | # basepath for the selfcalib codes 8 | selfcalib_basepath=/home/svoboda/Work/BlueCCal/BlueCFindingPoints 9 | 10 | ####################################################### 11 | ### config definitions 12 | ### the variables should be specified in .cshrc 13 | ### or something like that 14 | ####################################################### 15 | # basepath for binaries and auxiliary scripts 16 | basepath=$BlueC_BASEPATH 17 | # image basename 18 | imname=$BlueC_IMNAME 19 | # machine basename 20 | mname=$BlueC_MNAME 21 | workmachine=$BlueC_WORKMACHINE 22 | # working directory on local machines 23 | localdir=$BlueC_LOCALDIR 24 | # for all available indexes 25 | indexes=$BlueC_INDEXES 26 | 27 | # first clean the old local data 28 | for i in $indexes; do 29 | command="ssh ${mname}${i} rm -f ${localdir}/*.dat*" 30 | echo $command 31 | eval $command 32 | done 33 | 34 | for i in $indexes; do 35 | command="ssh ${mname}${i} rm -f ${localdir}/*.tiff" 36 | echo $command 37 | eval $command 38 | done 39 | 40 | wait 41 | 42 | # call the local finding procedure 43 | for i in $indexes; do 44 | command="ssh ${mname}${i} /usr/sepp/bin/matlab -nosplash -nojvm < ${selfcalib_basepath}/im2pointsLocally.m > ${selfcalib_basepath}/im2pointsLocally.out.${i} &" 45 | 46 | echo $command 47 | eval $command 48 | sleep 1 49 | done 50 | 51 | wait 52 | 53 | # clean output files 54 | rm im2pointsLocally.out.* 55 | 56 | -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/workvolume.m: -------------------------------------------------------------------------------- 1 | function [Xmat,idxisa] = workvolume(cam,room,imres,idxcams) 2 | 3 | STEP = 0.1; 4 | 5 | if nargin < 4 6 | idxcams = [1:size(cam,2)]; 7 | end 8 | 9 | if nargin < 3 10 | imres = repmat([640 480],size(idxcams,2),1); 11 | end 12 | imres = imres(idxcams,:); 13 | 14 | if nargin < 2 15 | % room [x_min, x_max, y_min, y_max, z_min, z_max] 16 | room = [-3 3 -3 3 0 3]; 17 | end 18 | 19 | % compose Pmat containing all P matrices 20 | Pmat = []; 21 | for i=idxcams, 22 | Pmat = [Pmat; cam(i).P]; 23 | end 24 | 25 | % create points 26 | 27 | zcoor = room(5):STEP:room(6); 28 | znum = size(zcoor,2); 29 | ycoor = room(3):STEP:room(4); 30 | ynum = size(ycoor,2); 31 | xcoor = room(1):STEP:room(2); 32 | xnum = size(xcoor,2); 33 | 34 | 35 | buff = repmat(ycoor,znum,1); 36 | yvec = reshape(buff,prod(size(buff)),1); 37 | zvec = repmat(zcoor',ynum,1); 38 | 39 | buff = repmat(xcoor,znum*ynum,1); 40 | xvec = reshape(buff,prod(size(buff)),1); 41 | 42 | Xmat = [xvec,repmat([yvec,zvec],xnum,1)]; 43 | Xmat = [Xmat,ones(size(Xmat(:,1)))]; 44 | 45 | clear buff 46 | 47 | size(Xmat) 48 | umat = Pmat*Xmat'; 49 | 50 | % normalize projected points in umat 51 | scalemat = []; 52 | for i=1:size(idxcams,2), 53 | scalemat = [scalemat; repmat(umat(3*i,:),3,1)]; 54 | end 55 | umat = umat./scalemat; 56 | clear scalemat; 57 | mask = zeros(size(umat)); 58 | mask(1:3:end,:) = umat(1:3:end,:)0); 62 | idxisa = find(sum(mask)==size(mask,1)); 63 | 64 | return; 65 | 66 | -------------------------------------------------------------------------------- /MultiCamSelfCal/BlueCLocal/strawlab_test.m: -------------------------------------------------------------------------------- 1 | function [align] = strawlab_test(in,config) 2 | 3 | Cst = in.Cst; 4 | Rot = in.Rot; 5 | 6 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 7 | if ~Octave, 8 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom (no sRt)',config.cal.cams2use); 9 | end 10 | 11 | % These locations are meaningless, and this file is just here for the 12 | % cases when you actually want to do this alignment in MATLAB/Octave. 13 | 14 | cam(1).C = [-4600 -100 2200]'; 15 | cam(2).C = [-4600 3100 2200]'; 16 | cam(3).C = [340 -100 2350]'; 17 | 18 | % of the similarity computation 19 | 20 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst'],[cam(:).C]); 21 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 22 | % save aligned data 23 | if 1 % SAVE_STEPHI | SAVE_PGUHA 24 | [align.Cst,align.Rot] = savecalpar(align.P,config); 25 | end 26 | 27 | if ~Octave, 28 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 29 | 30 | set(gca,'CameraTarget',[0,0,0]); 31 | set(gca,'CameraPosition',[0,0,1]); 32 | 33 | figure(61), 34 | % print -depsc graphevalaligned.eps 35 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 36 | 37 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 38 | 39 | set(gca,'CameraTarget',[0,0,0.9]); 40 | set(gca,'CameraPosition',[2,0,0.9]); 41 | 42 | %figure(62), 43 | % print -depsc graphevalaligned.eps 44 | %eval(['print -depsc ', config.paths.data, 'sideview.eps']) 45 | end 46 | 47 | return 48 | -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/g9.m: -------------------------------------------------------------------------------- 1 | function [align] = g9(in,config) 2 | % g9 ... local alignment for room G9 3 | % 4 | % [align] = G9(in,config) 5 | % in, cam, config ... see the main GOCAL script 6 | % 7 | % align ... structures aligned wit the specified world frame 8 | % 9 | % $Id: g9.m,v 1.1 2005/05/20 15:31:31 svoboda Exp $ 10 | 11 | REALVIZ = 0; 12 | 13 | Cst = in.Cst; 14 | Rot = in.Rot; 15 | 16 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); 17 | 18 | % definition of the absolute world frame 19 | % cam(1).C = [3, 1.25, 0.57]'; 20 | cam(2).C = [3, 1.8, 0.2]'; 21 | cam(3).C = [-2.2, 2.05, 0.1]'; 22 | cam(4).C = [-2.2, 2.05, 3.2]'; 23 | 24 | idx = [2:4]; 25 | 26 | % of the similarity computation 27 | 28 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst(idx,:)'],[cam(idx).C]); 29 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 30 | % save aligned data 31 | if 1 % SAVE_STEPHI | SAVE_PGUHA 32 | [align.Cst,align.Rot] = savecalpar(align.P,config); 33 | end 34 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 35 | 36 | set(gca,'CameraTarget',[0,0,0]); 37 | set(gca,'CameraPosition',[0,1,0]); 38 | 39 | figure(61), 40 | % print -depsc graphevalaligned.eps 41 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 42 | 43 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 44 | 45 | set(gca,'CameraTarget',[0,2.05,0]); 46 | set(gca,'CameraPosition',[0,2.05,3]); 47 | 48 | figure(62), 49 | % print -depsc graphevalaligned.eps 50 | eval(['print -depsc ', config.paths.data, 'sideview.eps']) 51 | 52 | return 53 | -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/planarmove.m: -------------------------------------------------------------------------------- 1 | function [align] = planarmove(in,config) 2 | % planarmove ... alignment under assumption of planar motion 3 | % 4 | % [align] = planarmove(in,config) 5 | % in, cam, config ... see the main GOCAL script 6 | % 7 | % align ... structures aligned wit the specified world frame 8 | % 9 | % $Id: planarmove.m,v 1.2 2005/05/20 15:31:31 svoboda Exp $ 10 | 11 | % fit a plane to the reconstructed points and estimate normal 12 | 13 | plane.n = planefit(in.Xe(1:3,:)'); 14 | 15 | new.n = [0,0,1]'; % align the xy plane horizontally 16 | 17 | rotaxis = cross(plane.n,new.n); 18 | rotangle = acos( (plane.n'*new.n)/norm(plane.n)*norm(new.n) ); 19 | 20 | R = nfi2r(rotaxis,rotangle); 21 | s = 3; 22 | t = [0,0,1]' - s*R*mean(in.Xe(1:3,:)')'; 23 | 24 | align.simT.s = s; 25 | align.simT.R = R; 26 | align.simT.t = t; 27 | 28 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 29 | % save aligned data 30 | if 1 % SAVE_STEPHI | SAVE_PGUHA 31 | [align.Cst,align.Rot] = savecalpar(align.P,config); 32 | end 33 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data, TopView',config.cal.cams2use); 34 | 35 | set(gca,'CameraTarget',[0,0,1]); 36 | set(gca,'CameraPosition',[0,0,2]); 37 | 38 | figure(61), 39 | % print -depsc graphevalaligned.eps 40 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 41 | 42 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data, SideView',config.cal.cams2use); 43 | 44 | set(gca,'CameraTarget',[0,0,0.9]); 45 | set(gca,'CameraPosition',[2,0,0.9]); 46 | 47 | figure(62), 48 | % print -depsc graphevalaligned.eps 49 | eval(['print -depsc ', config.paths.data, 'sideview.eps']) 50 | 51 | return 52 | 53 | 54 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/estsimt.m: -------------------------------------------------------------------------------- 1 | function [s,R,T] = estsimt(X1,X2) 2 | % ESTimate SIMilarity Transformation 3 | % 4 | % [s,R,T] = estsimt(X1,X2) 5 | % 6 | % X1,X2 ... 3xN matrices with corresponding 3D points 7 | % 8 | % X2 = s*R*X1 + T 9 | % s ... scalar scale 10 | % R ... 3x3 rotation matrix 11 | % T ... 3x1 translation vector 12 | % 13 | % This is done according to the paper: 14 | % "Least-Squares Fitting of Two 3-D Point Sets" 15 | % by K.S. Arun, T. S. Huang and S. D. Blostein 16 | % 17 | % $Id: estsimt.m,v 2.1 2005/05/23 16:24:59 svoboda Exp $ 18 | 19 | % number of points 20 | N = size(X1,2); 21 | 22 | if N ~= size(X2,2) 23 | error('estsimt: both sets must contain the same number of points') 24 | end 25 | 26 | X1cent = mean(X1,2); 27 | X2cent = mean(X2,2); 28 | % normalize coordinate systems for both set of points 29 | x1 = X1 - repmat(X1cent,1,N); 30 | x2 = X2 - repmat(X2cent,1,N); 31 | 32 | % first estimate the scale s 33 | % dists1 = sum(sqrt(x1.^2)); 34 | % dists2 = sum(sqrt(x2.^2)); 35 | 36 | % mutual distances; 37 | d1 = x1(:,2:end)-x1(:,1:(end-1)); 38 | d2 = x2(:,2:end)-x2(:,1:(end-1)); 39 | ds1 = sum(d1.^2).^(1/2); 40 | ds2 = sum(d2.^2).^(1/2); 41 | 42 | scales = ds2./ds1; 43 | 44 | % the scales should be the same for all points 45 | % because of noise they are not 46 | % scales = dists2./dists1 47 | s = median(sort(scales)); 48 | 49 | % undo scale 50 | x1s = s*x1; 51 | 52 | % finding rotation 53 | H = zeros(3,3); 54 | for i=1:N 55 | H = H+ x1s(:,i)*x2(:,i)'; 56 | end 57 | 58 | [U,S,V] = svd(H,0); 59 | X = V*U'; 60 | if det(X) > 0 61 | R = X; % estimated rotation matrix 62 | else 63 | % V = [V(:,1:2),-V(:,3)]; 64 | % R = V*U'; 65 | % s = -s; 66 | R = X; 67 | end 68 | 69 | T = X2cent - s*R*X1cent; 70 | 71 | 72 | -------------------------------------------------------------------------------- /CalTechCal/extract_parameters.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | check_active_images; 5 | 6 | if ~exist('solution_error') 7 | solution_error = zeros(6*n_ima + 15,1); 8 | end; 9 | 10 | fc = solution(1:2);%*** 11 | cc = solution(3:4);%*** 12 | alpha_c = solution(5);%*** 13 | kc = solution(6:10);%*** 14 | 15 | fc_error = solution_error(1:2); 16 | cc_error = solution_error(3:4); 17 | alpha_c_error = solution_error(5); 18 | kc_error = solution_error(6:10); 19 | 20 | % Calibration matrix: 21 | 22 | KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; 23 | inv_KK = inv(KK); 24 | 25 | % Extract the extrinsic paramters, and recomputer the collineations 26 | 27 | for kk = 1:n_ima, 28 | 29 | if active_images(kk), 30 | 31 | omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** 32 | Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** 33 | 34 | omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 35 | Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 36 | 37 | Rckk = rodrigues(omckk); 38 | 39 | Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 40 | 41 | Hkk = Hkk / Hkk(3,3); 42 | 43 | else 44 | 45 | omckk = NaN*ones(3,1); 46 | Tckk = NaN*ones(3,1); 47 | Rckk = NaN*ones(3,3); 48 | Hkk = NaN*ones(3,3); 49 | omckk_error = NaN*ones(3,1); 50 | Tckk_error = NaN*ones(3,1); 51 | 52 | end; 53 | 54 | eval(['omc_' num2str(kk) ' = omckk;']); 55 | eval(['Rc_' num2str(kk) ' = Rckk;']); 56 | eval(['Tc_' num2str(kk) ' = Tckk;']); 57 | eval(['H_' num2str(kk) '= Hkk;']); 58 | eval(['omc_error_' num2str(kk) ' = omckk_error;']); 59 | eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /RansacM/rEG.m: -------------------------------------------------------------------------------- 1 | function [F, inls] = rEG(u,th,th4,conf,ss) 2 | % REG robust estimation of the epipolar geometry via RANSAC 3 | % 4 | % [H, inls] = rRG(u,th,{th4=th,conf=0.99,ss=8}) 5 | % u ... 6xN pairs homogenous coordinates 6 | % th ... inlier tolerance in pixels 7 | % th4 ... currently not used 8 | % conf ... confidence, the higher -> more samples 9 | % ss ... sample size 10 | % 11 | % F ... 3x3 fundamental matrix 12 | % inls ... 1xN logical 1->inlier. 0->outlier 13 | % 14 | % $Id: rEG.m,v 1.1 2005/05/23 16:16:00 svoboda Exp $ 15 | 16 | 17 | MAX_SAM = 100000; % maimal number of random samples 18 | 19 | len = size(u,2); 20 | 21 | % parsing the inputs 22 | if nargin < 3 23 | th4 = th; 24 | end 25 | 26 | if nargin < 4 27 | conf = 0.99; 28 | end 29 | 30 | if nargin < 5 31 | ss = 8; % sample size 32 | end 33 | 34 | len = size(u,2); 35 | ptr = 1:len; 36 | max_i = 5; 37 | max_sam = MAX_SAM; 38 | 39 | no_sam = 0; 40 | no_mod = 0; 41 | 42 | th = 2*th^2; 43 | 44 | while no_sam < max_sam 45 | for pos = 1:ss 46 | idx = pos + ceil(rand * (len-pos)); 47 | ptr([pos, idx]) = ptr([idx, pos]); 48 | end; 49 | 50 | no_sam = no_sam +1; 51 | 52 | sF = u2Fdlt(u(:,ptr(1:ss)),0); 53 | errs = Fsampson(sF,u); 54 | v = errs < th; 55 | no_i = sum(v); 56 | 57 | if max_i < no_i 58 | inls = v; 59 | F = sF; 60 | max_i = no_i; 61 | max_sam = min([max_sam,nsamples(max_i, len, ss, conf)]); 62 | end 63 | end 64 | 65 | %%% 66 | % refine the F by using all detected outliers and with point normalization 67 | F = u2Fdlt(u(:,inls),1); 68 | 69 | if no_sam == MAX_SAM 70 | warning(sprintf('RANSAC - termination forced after %d samples expected number of samples is %d', no_sam, exp_sam)); 71 | else 72 | disp(sprintf('RANSAC: %d samples, %d inliers out of %d points',no_sam,sum(inls),len)) 73 | end; 74 | 75 | return; -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/planarcams.m: -------------------------------------------------------------------------------- 1 | % planarcams ... alignment under assumption of planar camera arrangements 2 | % 3 | % [align,cam] = planarmove(in,cam,config,idx) 4 | % in, cam, config ... see the main GOCAL script 5 | % idx ... indexes of cameras which are suppose to be in one plane 6 | % 7 | % align ... structures aligned wit the specified world frame 8 | % 9 | % $Id: planarcams.m,v 1.1 2003/07/03 15:36:55 svoboda Exp $ 10 | 11 | function [align,cam] = planarcams(in,cam,config,idx) 12 | 13 | % fit a plane to the reconstructed points and estimate normal 14 | 15 | plane.n = planefit(in.Ce(:,idx)'); 16 | 17 | new.n = [0,0,1]'; % align the xy plane horizontally 18 | 19 | rotaxis = cross(new.n,plane.n); 20 | rotangle = acos( (plane.n'*new.n)/norm(plane.n)*norm(new.n) ); 21 | 22 | R = nfi2r(rotaxis,rotangle); 23 | s = 3; 24 | t = [0,0,1]' - s*R*mean(in.Xe(1:3,:)')'; 25 | 26 | align.simT.s = s; 27 | align.simT.R = R; 28 | align.simT.t = t; 29 | 30 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 31 | % save aligned data 32 | if 1 % SAVE_STEPHI | SAVE_PGUHA 33 | [align.Cst,align.Rot] = savecalpar(align.P,config); 34 | end 35 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data, TopView',config.cal.cams2use); 36 | 37 | set(gca,'CameraTarget',[0,0,0]); 38 | set(gca,'CameraPosition',[0,0,1]); 39 | 40 | figure(61), 41 | % print -depsc graphevalaligned.eps 42 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 43 | 44 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data, SideView',config.cal.cams2use); 45 | 46 | set(gca,'CameraTarget',[0,0,0.9]); 47 | set(gca,'CameraPosition',[2,0,0.9]); 48 | 49 | figure(62), 50 | % print -depsc graphevalaligned.eps 51 | eval(['print -depsc ', config.paths.data, 'sideview.eps']) 52 | 53 | return 54 | 55 | 56 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/drawscene.m: -------------------------------------------------------------------------------- 1 | function drawscene(X,C,R,fig,ctypehint,scenetitle,camsId) 2 | 3 | % drawscene ... plots calibration points, cameras and their viewing axes 4 | % 5 | % drawscene(X,C,R,fig,ctypehint,scenetitle) 6 | % 7 | % X ............ 4xPOINTS matrix containg POINTS object points 8 | % C ............ 3xCAMS matrix containing the camera centers (in world coord.) 9 | % R ............ 3*CAMSx3 matrix containing camera rotation matrices 10 | % (needed for drawing the viewing axes) 11 | % fig .......... figure handle (defaults to 1) 12 | % ctypehint .... calibration object type of X (defaults to 'cloud') 13 | % scenetitle ... title of the plot (defaults to '') 14 | % camsIs ....... 1xCAMS vector with cameas Id (default is 1:CAMS 15 | 16 | % $Author: svoboda $ 17 | % $Revision: 2.0 $ 18 | % $Id: drawscene.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $ 19 | % $State: Exp $ 20 | 21 | POINTS = size(X,2); 22 | CAMS = size(C,2); 23 | 24 | if nargin < 7 25 | camsId = [1:CAMS]; 26 | end 27 | 28 | if (nargin < 3) 29 | error('not enough input arguments'); 30 | end 31 | if (nargin < 5) 32 | scenetitle = ''; 33 | end 34 | if (nargin < 4) 35 | ctypehint = 'cloud'; 36 | end 37 | 38 | figure(fig); clf 39 | title(scenetitle) 40 | grid on 41 | axis equal 42 | 43 | % plot camera positions (blue) 44 | drawcloud(C,fig,'b'); 45 | 46 | % plot calibration object (red) 47 | drawobject(X,ctypehint,fig,'r'); 48 | 49 | % Mean of all points 50 | centroid = mean(X(1:3,:)'); 51 | 52 | % plot viewing axes 53 | for i=1:CAMS 54 | axis_dir = -R(3*i,:); % 3rd row of i-th rotation matrix 55 | axis_len = 0.6*norm(C(1:3,i)-centroid'); 56 | endpoint = C(1:3,i)+axis_len*axis_dir'; 57 | line([C(1,i),endpoint(1)],[C(2,i),endpoint(2)],[C(3,i),endpoint(3)]); 58 | text(C(1,i),C(2,i),C(3,i),sprintf('%4d',camsId(i)),'Color','k'); 59 | end 60 | 61 | 62 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/rroots.c: -------------------------------------------------------------------------------- 1 | #include 2 | #define rr_a (*po) 3 | #define rr_d (*(po + 3)) 4 | #define eps (2.2204e-016) 5 | 6 | #include "mex.h" 7 | #include "matrix.h" 8 | 9 | 10 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 11 | { 12 | 13 | double *po = mxGetPr(aSrcP[0]); 14 | 15 | double b,c, b2, bt, v, pit, e, *r; 16 | double p, q, D, A, cosphi, phit, R, _2R; 17 | 18 | b = *(po + 1) / rr_a; 19 | c = *(po + 2) / rr_a; 20 | b2 = b*b; 21 | bt = b/3; 22 | 23 | p = (3*c - b2)/ 9; 24 | q = ((2 * b2 * b)/27 - b*c/3 + rr_d/rr_a) / 2; 25 | 26 | D = q*q + p*p*p; 27 | 28 | if (D > 0) 29 | { 30 | aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); 31 | r = mxGetPr(aDstP[0]); 32 | 33 | A = sqrt(D) - q; 34 | if (A > 0) 35 | { 36 | v = pow(A,1.0/3); 37 | *r = v - p/v - bt; 38 | } else 39 | { 40 | v = pow(-A,1.0/3); 41 | *r = p/v - v - bt; 42 | } 43 | } else 44 | { 45 | /* if (p > -eps) 46 | { 47 | printf("%.17f\n", p); 48 | aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); 49 | r = mxGetPr(aDstP[0]); 50 | *r = pow(q,1.0/3) - bt; 51 | } 52 | else */ 53 | { 54 | 55 | aDstP[0] = mxCreateDoubleMatrix( 3, 1, mxREAL ); 56 | r = mxGetPr(aDstP[0]); 57 | if (q > 0) e = 1; else e = -1; 58 | R = e * sqrt(-p); 59 | _2R = R *2; 60 | cosphi = q / (R*R*R); 61 | if (cosphi > 1) cosphi = 1; else 62 | if (cosphi < -1) cosphi = -1; 63 | phit = acos(cosphi) /3; 64 | pit = 3.14159265358979/3; 65 | 66 | r[0] = -_2R * cos(phit) -bt; 67 | r[1] = _2R * cos(pit - phit) -bt; 68 | r[2] = _2R * cos(pit + phit) -bt; 69 | } 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/rroots.c: -------------------------------------------------------------------------------- 1 | #include 2 | #define rr_a (*po) 3 | #define rr_d (*(po + 3)) 4 | #define eps (2.2204e-016) 5 | 6 | #include "mex.h" 7 | #include "matrix.h" 8 | 9 | 10 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 11 | { 12 | 13 | double *po = mxGetPr(aSrcP[0]); 14 | 15 | double b,c, b2, bt, v, pit, e, *r; 16 | double p, q, D, A, cosphi, phit, R, _2R; 17 | 18 | b = *(po + 1) / rr_a; 19 | c = *(po + 2) / rr_a; 20 | b2 = b*b; 21 | bt = b/3; 22 | 23 | p = (3*c - b2)/ 9; 24 | q = ((2 * b2 * b)/27 - b*c/3 + rr_d/rr_a) / 2; 25 | 26 | D = q*q + p*p*p; 27 | 28 | if (D > 0) 29 | { 30 | aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); 31 | r = mxGetPr(aDstP[0]); 32 | 33 | A = sqrt(D) - q; 34 | if (A > 0) 35 | { 36 | v = pow(A,1.0/3); 37 | *r = v - p/v - bt; 38 | } else 39 | { 40 | v = pow(-A,1.0/3); 41 | *r = p/v - v - bt; 42 | } 43 | } else 44 | { 45 | /* if (p > -eps) 46 | { 47 | printf("%.17f\n", p); 48 | aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); 49 | r = mxGetPr(aDstP[0]); 50 | *r = pow(q,1.0/3) - bt; 51 | } 52 | else */ 53 | { 54 | 55 | aDstP[0] = mxCreateDoubleMatrix( 3, 1, mxREAL ); 56 | r = mxGetPr(aDstP[0]); 57 | if (q > 0) e = 1; else e = -1; 58 | R = e * sqrt(-p); 59 | _2R = R *2; 60 | cosphi = q / (R*R*R); 61 | if (cosphi > 1) cosphi = 1; else 62 | if (cosphi < -1) cosphi = -1; 63 | phit = acos(cosphi) /3; 64 | pit = 3.14159265358979/3; 65 | 66 | r[0] = -_2R * cos(phit) -bt; 67 | r[1] = _2R * cos(pit - phit) -bt; 68 | r[2] = _2R * cos(pit + phit) -bt; 69 | } 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/u2FI.m: -------------------------------------------------------------------------------- 1 | %u2FI Estimate fundamental matrix using ortogonal LS regression 2 | % 3 | % F = u2F(u) estimates F from u using NORMU 4 | % F = u2F(u,'nonorm') disables normalization 5 | % 6 | % see also NORMU, U2FA 7 | % 8 | % Returns 0 if too few points are available 9 | 10 | function F = u2FI (u, str, A1, A2) 11 | 12 | sampcols = find(sum(~isnan(u(1:3:end,:))) == 2); 13 | if length(sampcols) < 8 14 | F = 0; return 15 | end 16 | 17 | if (nargin > 2) && ~strcmp(str, 'nonorm') && ~strcmp(str, 'usenorm') 18 | donorm = 1; 19 | else 20 | donorm = 0; 21 | end 22 | 23 | ptNum = size(sampcols,2); 24 | 25 | if donorm 26 | A1 = normu(u(1:3,sampcols)); 27 | A2 = normu(u(4:6,sampcols)); 28 | if isempty(A1) | isempty(A2), F = 0; return; end 29 | 30 | u1 = A1*u(1:3,sampcols); %in u1, u2 there are only columns of sampcols 31 | u2 = A2*u(4:6,sampcols); 32 | else 33 | u1 = u(1:3,sampcols); %" " 34 | u2 = u(4:6,sampcols); 35 | end 36 | 37 | for i = 1:ptNum 38 | Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); 39 | end 40 | 41 | M = Z'*Z; 42 | V = seig(M); 43 | F = reshape(V(:,1),3,3); 44 | 45 | %odrizneme nejmensi vlastni slozku, aby F melo hodnost 2 46 | [uu,us,uv] = svd(F); 47 | %[y,i] = min (abs(diag(us))); 48 | i = 3; 49 | %if us(i,i) > 1e-12, disp('rank(F)>2'); end 50 | us(i,i) = 0; 51 | F = uu*us*uv'; 52 | 53 | if donorm | strcmp(str, 'usenorm') 54 | F = A1'*F*A2; 55 | end 56 | 57 | F1=F; 58 | 59 | F = F /norm(F,2); 60 | 61 | if rank(F) > 2 62 | %disp('!!! Error: u2FI: rank(F) > 2'); 63 | % snizime hodnost, us(3,3) je stejne 1e-16 64 | [uu,us,uv] = svd(F); 65 | us(3,3) = 0; 66 | F = uu*us*uv'; 67 | end 68 | 69 | %seig sorted eigenvalues 70 | function [V,d] = seig(M) 71 | [V,D] = eig(M); 72 | [d,s] = sort(diag(D)); 73 | V = V(:,s); 74 | -------------------------------------------------------------------------------- /MultiCamValidation/InputOutputFunctions/drawscene.m: -------------------------------------------------------------------------------- 1 | function drawscene(X,C,R,fig,ctypehint,scenetitle,camsId) 2 | 3 | % drawscene ... plots calibration points, cameras and their viewing axes 4 | % 5 | % drawscene(X,C,R,fig,ctypehint,scenetitle) 6 | % 7 | % X ............ 4xPOINTS matrix containg POINTS object points 8 | % C ............ 3xCAMS matrix containing the camera centers (in world coord.) 9 | % R ............ 3*CAMSx3 matrix containing camera rotation matrices 10 | % (needed for drawing the viewing axes) 11 | % fig .......... figure handle (defaults to 1) 12 | % ctypehint .... calibration object type of X (defaults to 'cloud') 13 | % scenetitle ... title of the plot (defaults to '') 14 | % camsIs ....... 1xCAMS vector with cameas Id (default is 1:CAMS 15 | 16 | % $Author: svoboda $ 17 | % $Revision: 2.0 $ 18 | % $Id: drawscene.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $ 19 | % $State: Exp $ 20 | 21 | POINTS = size(X,2); 22 | CAMS = size(C,2); 23 | 24 | if nargin < 7 25 | camsId = [1:CAMS]; 26 | end 27 | 28 | if (nargin < 3) 29 | error('not enough input arguments'); 30 | end 31 | if (nargin < 5) 32 | scenetitle = ''; 33 | end 34 | if (nargin < 4) 35 | ctypehint = 'cloud'; 36 | end 37 | 38 | figure(fig); clf 39 | title(scenetitle) 40 | grid on 41 | axis equal 42 | 43 | % plot camera positions (blue) 44 | drawcloud(C,fig,'b'); 45 | 46 | % plot calibration object (red) 47 | drawobject(X,ctypehint,fig,'r'); 48 | 49 | % Mean of all points 50 | centroid = mean(X(1:3,:)'); 51 | 52 | % plot viewing axes 53 | for i=1:CAMS 54 | axis_dir = -R(3*i,:); % 3rd row of i-th rotation matrix 55 | axis_len = 0.6*norm(C(1:3,i)-centroid'); 56 | endpoint = C(1:3,i)+axis_len*axis_dir'; 57 | line([C(1,i),endpoint(1)],[C(2,i),endpoint(2)],[C(3,i),endpoint(3)]); 58 | text(C(1,i),C(2,i),C(3,i),sprintf('%4d',camsId(i)),'Color','k'); 59 | end 60 | 61 | 62 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/L2depths.m: -------------------------------------------------------------------------------- 1 | %L2depths Compute depths of PRMM from basis L. 2 | % 3 | % No known depths are exploited. These can be different because of noise 4 | % etc. 5 | % 6 | % Parameters: 7 | % opt.verbose(1) .. whether display info 8 | 9 | function [Mdepths, lambda] = L2depths(L, M, Idepths, opt) 10 | 11 | if nargin < 4, opt = []; end 12 | if ~isfield(opt,'verbose') 13 | opt.verbose = 1; end 14 | 15 | if opt.verbose, fprintf('Computing depths...'); tic; end 16 | 17 | Mdepths = M; 18 | 19 | [m n] = size(M); m = m/3; 20 | lambda(m, n) = 0; % memory allocation 21 | 22 | for j = 1:n 23 | full = find(~isnan(M(1:3:end,j))); 24 | mis_rows = intersect(find(Idepths(:,j)==0),full); 25 | if length(mis_rows) > 0 26 | submatrix = spread_depths_col(M(k2i(full),j),Idepths(full,j)); 27 | 28 | % We want submatrix to be in the space L -> 29 | % we search for combination of columns of the base of L i.e. 30 | % L(k2i(full),:)*res(1:4)-submatrix*[1 res(5:length(res))] = 0 31 | right = submatrix(:,1); 32 | A = [ L(k2i(full),:) -(submatrix(:,2:size(submatrix,2))) ]; 33 | if rank(A) < size(A, 2) % depths cannot be computed => kill the data 34 | kill = full(~Idepths(full,j)); 35 | Mdepths(k2i(kill),j) = NaN; lambda(kill,j) = NaN; 36 | else 37 | res = A \ right; 38 | 39 | %test: er should be near to zero 40 | %er=L(k2i(full),:)*res(1:4)-submatrix*[1 res(5:length(res))']' 41 | 42 | % depth corresponding to right is/are set to 1 43 | i = full(find(right(1:3:end))); lambda(i,j) = 1; 44 | Mdepths(k2i(i),j) = M(k2i(i),j); 45 | 46 | for ii = 1:size(submatrix,2)-1 47 | i = full(find(submatrix(1:3:end,1+ii))); lambda(i,j) = res(4+ii); 48 | Mdepths(k2i(i),j) = M(k2i(i),j)*lambda(i,j); 49 | end 50 | end 51 | end 52 | end 53 | 54 | if opt.verbose, disp(['(' num2str(toc) ' sec)']); end 55 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/evalreprerror.m: -------------------------------------------------------------------------------- 1 | % evalreprerror ... computes and plots the final error statistics 2 | % 3 | % cam = evalreprerror(cam,config) 4 | % cam, config ... see the main GOCAL script 5 | % 6 | % $Id: evalreprerror.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $ 7 | 8 | function cam = evalreprerror(cam,config) 9 | 10 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 11 | 12 | disp('2D reprojection error') 13 | disp(sprintf('All points: mean %2.2f pixels, std is %2.2f',mean([cam.err2d]), std([cam.err2d])')); 14 | % disp(sprintf('Inliers: mean %2.2f pixels, std is %2.2f',mean([cam.inerr2d]), std([cam.inerr2d])')); 15 | if mean([cam.err2d])>1.5 || std([cam.err2d])>1.5 16 | disp('***************************************************') 17 | disp('W A R N I N G: the reprojection error is relatively high !') 18 | end 19 | 20 | %%% 21 | % evaluate the reprojection error for each camera separately to detect possible problems 22 | %%% 23 | for i=1:size(config.cal.cams2use,2), 24 | cam(i).mean2Derr = mean(cam(i).err2d); 25 | cam(i).std2Derr = std(cam(i).err2d); 26 | end 27 | % sort the values and print them to the 2D graphs 28 | 29 | if ~Octave, 30 | figure(30), 31 | clf 32 | plot(config.cal.cams2use,[cam.mean2Derr],'bd'), 33 | hold on, grid on, 34 | plot(config.cal.cams2use,[cam.mean2Derr],'b-'), 35 | plot(config.cal.cams2use,[cam.std2Derr],'rd') 36 | plot(config.cal.cams2use,[cam.std2Derr],'r-') 37 | xlabel('Id of the camera') 38 | title('2D error: mean (blue), std (red)') 39 | ylabel('pixels') 40 | 41 | figure(31) 42 | clf 43 | bar(config.cal.cams2use,[cam.mean2Derr;cam.std2Derr]',1.5) 44 | grid on 45 | xlabel('Id of the camera') 46 | title('2D error: mean (blue), std (red)') 47 | ylabel('pixels') 48 | 49 | figure(31), 50 | eval(['print -depsc ', config.paths.data, 'reprerrors.eps']) 51 | 52 | figure(4), 53 | eval(['print -depsc ', config.paths.data, 'reconstructedsetup.eps']) 54 | end 55 | 56 | Ret = 1; 57 | return 58 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/estimateLambda.m: -------------------------------------------------------------------------------- 1 | % estimateLambda ... estimate some initial projective depth given 2 | % the measurement matrix 3 | % 4 | % This algorithm is based on a paper by P. Sturm and B. Triggs called 5 | % "A Factorization Based Algorithm for Multi-Image Projective Structure 6 | % and motion" (1996) 7 | % Projective depths are estimated using fundamental matrices. 8 | % 9 | % [Lambda] = estimateLambda(Ws) 10 | % 11 | % Ws ...... the 3*nxm measurement matrix (the data should be normalized 12 | % before calling this functions) 13 | % pair .... array of camera pairs containing Fundamental matrices and 14 | % indexes of points used for their computations 15 | % it is the output from RANSAC validation step 16 | % 17 | % Lambda .. nxm matrix containing the estimated projective depths 18 | % 19 | % 09/2001, Dejan Radovic 20 | % 05/2002, Tomas Svoboda 21 | 22 | % $Author: svoboda $ 23 | % $Revision: 2.0 $ 24 | % $Id: estimateLambda.m,v 2.0 2003/06/19 12:06:49 svoboda Exp $ 25 | % $State: Exp $ 26 | 27 | 28 | function [Lambda] = estimateLambda(Ws,pair) 29 | n = size(Ws,1)/3; % cameras 30 | m = size(Ws,2); % frames 31 | 32 | % estimate (n-1) fundamental matrices F_12, F_23, ..., F_(n-1)n 33 | F = []; % the fundamental matrices 34 | e = []; % the epipoles (as columns) 35 | Lambda = ones(n,m); % the estimated projective depths 36 | for i=1:n-1 37 | j=i+1; 38 | F_ij = pair(i).F; 39 | % compute epipole from F_ij*e_ij == 0 40 | [U,S,V] = svd(F_ij,0); 41 | % diag(S)' 42 | e_ij = V(:,size(V,2)); 43 | for p=pair(i).idxin, 44 | q_ip = Ws(i*3-2:i*3,p); 45 | q_jp = Ws(j*3-2:j*3,p); 46 | Lambda(j,p) = Lambda(i,p)*((norm(cross(e_ij,q_ip)))^2/sum(cross(e_ij,q_ip).*(F_ij'*q_jp))); 47 | % Lambda(j,p) = Lambda(i,p)*(sum((F_ij'*q_jp).*cross(e_ij,q_ip))/(norm(F_ij'*q_jp))^2); 48 | end 49 | end 50 | 51 | return 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /python/test/test_align.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | from pymulticamselfcal.align import estsimt, align_points, align_pmat 3 | import numpy as np 4 | 5 | def test_align(): 6 | orig_points = np.array([ 7 | [3.36748406, 1.61036404, 3.55147255], 8 | [3.58702265, 0.06676394, 3.64695356], 9 | [0.28452026, -0.11188296, 3.78947735], 10 | [0.25482713, 1.57828256, 3.6900808], 11 | 12 | [3.54938525, 1.74057692, 5.13329681], 13 | [3.6855626 , 0.10335229, 5.26344841], 14 | [0.25025385, -0.06146044, 5.57085135], 15 | [0.20742481, 1.71073272, 5.41823085], 16 | ]).T 17 | 18 | ft2inch = 12.0 19 | inch2cm = 2.54 20 | cm2m = 0.01 21 | ft2m = ft2inch * inch2cm * cm2m 22 | 23 | x1,y1,z1=0,0,0 24 | x2,y2,z2=np.array([10,5,5])*ft2m 25 | 26 | new_points = np.array([ 27 | [x2, y2, z2], 28 | [x2, y1, z2], 29 | [x1, y1, z2], 30 | [x1, y2, z2], 31 | 32 | [x2, y2, z1], 33 | [x2, y1, z1], 34 | [x1, y1, z1], 35 | [x1, y2, z1], 36 | ]).T 37 | 38 | print(orig_points.T) 39 | print(new_points.T) 40 | 41 | s,R,t = estsimt(orig_points,new_points) 42 | print('s=%s'%repr(s)) 43 | print('R=%s'%repr(R.tolist())) 44 | print('t=%s'%repr(t.tolist())) 45 | Xnew = align_points( s,R,t, orig_points ) 46 | 47 | # measure distance between elements 48 | mean_absdiff = np.mean( abs(Xnew[:3]-new_points).flatten() ) 49 | assert mean_absdiff < 0.05 50 | 51 | pmat_orig = np.array([[1,2,3,4], 52 | [5,6,7,8], 53 | [9,10,11,12]],dtype=float) 54 | print('Xnew.T') 55 | print(Xnew.T) 56 | 57 | pmat_new = align_pmat( s,R,t, pmat_orig ) 58 | print('pmat_new') 59 | print(pmat_new) 60 | 61 | ## print('s',s) 62 | ## print('R') 63 | ## print(R) 64 | ## print('T') 65 | ## print(T) 66 | 67 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/rEG.m: -------------------------------------------------------------------------------- 1 | function [F, inls] = rEG(u, th, th7, conf) 2 | 3 | %rEG Robust computation of epipolar geometry based on RANSAC 4 | % 5 | % [F, inls] = rEG(u, th, th7, conf) 6 | % u point correspondences (6xn), where n is the number of corrs. 7 | % th threshold value for the Sampson's distance (see FDs) 8 | % th7 threshold for inliers to iterate on F (default = th) 9 | % conf confidence level of self-termination (default = .95) 10 | 11 | MAX_SAM = 100000; 12 | iter_amount = .5; 13 | 14 | if nargin < 3 15 | th7 = th; 16 | end; 17 | 18 | if nargin < 4 19 | conf = .95; 20 | end; 21 | 22 | len = size(u,2); 23 | ptr = 1:len; 24 | max_i = 8; 25 | max_sam = MAX_SAM; 26 | 27 | no_sam = 0; 28 | no_mod = 0; 29 | 30 | while no_sam < max_sam 31 | for pos = 1:7 32 | idx = pos + ceil(rand * (len-pos)); 33 | ptr([pos, idx]) = ptr([idx, pos]); 34 | end; 35 | 36 | no_sam = no_sam +1; 37 | 38 | aFs = fu2F7(u(:,ptr(1:7))); 39 | 40 | for i = 1:size(aFs,3) 41 | no_mod = no_mod +1; 42 | aF = aFs(:,:,i); 43 | Ds = fFDs(aF,u); 44 | v = Ds < th; 45 | v7 = Ds < th7; 46 | no_i = sum(v); 47 | 48 | if max_i < no_i 49 | inls = v; 50 | F = aF; 51 | max_i = no_i; 52 | max_sam = min([max_sam,nsamples(max_i, len, 7, conf)]); 53 | end 54 | 55 | if sum(v7) >= 8 + iter_amount*(max_i - 8) 56 | aF = u2F(u(:,v7)); 57 | Ds = fFDs(aF,u); 58 | v = Ds < th; 59 | no_i = sum(v); 60 | if max_i < no_i 61 | inls = v; 62 | F = aF; 63 | max_i = no_i; 64 | exp_sam = nsamples(max_i, len, 7, .95); 65 | max_sam = min([max_sam,exp_sam]); 66 | end 67 | end 68 | end 69 | end 70 | 71 | if no_sam == MAX_SAM 72 | warning(sprintf('RANSAC - termination forced after %d samples expected number of samples is %d', no_sam, exp_sam)); 73 | end; 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm_test/fill_mm_bundle.m: -------------------------------------------------------------------------------- 1 | %fill_mm_bundle Proj. reconstruction from MM [with bundle adjustment]. 2 | % 3 | % Call fill_mm [and bundle adjustment]. 4 | % 5 | % [ P,X, u1,u2, info ] = fill_mm_bundle(M, imsize, nl_params_all_cams [,opt]) 6 | % 7 | % Parameters: 8 | % M .. measurement matrix (MM) with homogeneous image points with NaNs 9 | % standing for the unknown elements in all three coordinates 10 | % imsize .. double(2,m), image sizes: imsize(:,i) is size of image i 11 | % m .. No. of cameras 12 | % opt .. options with default values in (): 13 | % .no_BA(0) .. whether refine using bundle adjustment 14 | % .verbose(1) .. whether display info 15 | % .verbose_short .. see opt in bundle_PX_proj 16 | % ... other options see in fill_mm 17 | % 18 | % Return parameters: 19 | % info.R_lin .. linear estimation of filled M 20 | % ... other parameters see in fill_mm 21 | 22 | function [ P,X, u1,u2, info ] = fill_mm_bundle(M, imsize, nl_params_all_cams, opt) 23 | 24 | if nargin < 4, opt = []; end 25 | if ~isfield(opt, 'no_BA') 26 | opt.no_BA = 0; end 27 | if ~isfield(opt, 'verbose'), 28 | opt.verbose = 1; end 29 | 30 | [P,X, u1,u2, info] = fill_mm(M, opt); 31 | 32 | info.R_lin = P*X; 33 | 34 | if ~opt.no_BA && length(u1) < size(M,1)/3 && length(u2) < size(M,2) 35 | if opt.verbose, fprintf(1, 'Bundle adjustment...\n'); tic; end 36 | 37 | [m,n] = size(M); m = m/3; r1 = setdiff(1:m,u1); r2 = setdiff(1:n,u2); 38 | [P,X] = bundle_PX_proj(P,X, normalize_cut(M(k2i(r1),r2)), imsize, nl_params_all_cams, opt); 39 | % old bundler: 40 | %[P,X] = qPXbundle_cmp(P,X, normalize_cut(M(k2i(r1),r2))); 41 | 42 | if opt.verbose, disp(['(' num2str(toc) ' sec)']); end 43 | info.err.BA = dist(M(k2i(r1),r2), P*X, info.opt.metric); 44 | if opt.verbose, fprintf('Error (after BA): %f\n', info.err.BA); 45 | else, fprintf(' %f\n', info.err.BA); end 46 | else, if ~opt.verbose, fprintf('\n'); end; end 47 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/rEG.m: -------------------------------------------------------------------------------- 1 | function [F, inls] = rEG(u, th, th7, conf) 2 | 3 | %rEG Robust computation of epipolar geometry based on RANSAC 4 | % 5 | % [F, inls] = rEG(u, th, th7, conf) 6 | % u point correspondences (6xn), where n is the number of corrs. 7 | % th threshold value for the Sampson's distance (see FDs) 8 | % th7 threshold for inliers to iterate on F (default = th) 9 | % conf confidence level of self-termination (default = .95) 10 | 11 | MAX_SAM = 100000; 12 | iter_amount = .5; 13 | 14 | if nargin < 3 15 | th7 = th; 16 | end; 17 | 18 | if nargin < 4 19 | conf = .95; 20 | end; 21 | 22 | len = size(u,2); 23 | ptr = 1:len; 24 | max_i = 8; 25 | max_sam = MAX_SAM; 26 | 27 | no_sam = 0; 28 | no_mod = 0; 29 | 30 | while no_sam < max_sam 31 | for pos = 1:7 32 | idx = pos + ceil(rand * (len-pos)); 33 | ptr([pos, idx]) = ptr([idx, pos]); 34 | end; 35 | 36 | no_sam = no_sam +1; 37 | 38 | aFs = fu2F7(u(:,ptr(1:7))); 39 | 40 | for i = 1:size(aFs,3) 41 | no_mod = no_mod +1; 42 | aF = aFs(:,:,i); 43 | Ds = mfFDs(aF,u); 44 | % Ds = fFDs(aF,u); 45 | v = Ds < th; 46 | v7 = Ds < th7; 47 | no_i = sum(v); 48 | 49 | if max_i < no_i 50 | inls = v; 51 | F = aF; 52 | max_i = no_i; 53 | max_sam = min([max_sam,nsamples(max_i, len, 7, conf)]); 54 | end 55 | 56 | if sum(v7) >= 8 + iter_amount*(max_i - 8) 57 | aF = u2F(u(:,v7)); 58 | Ds = mfFDs(aF,u); 59 | % Ds = fFDs(aF,u); 60 | v = Ds < th; 61 | no_i = sum(v); 62 | if max_i < no_i 63 | inls = v; 64 | F = aF; 65 | max_i = no_i; 66 | exp_sam = nsamples(max_i, len, 7, .95); 67 | max_sam = min([max_sam,exp_sam]); 68 | end 69 | end 70 | end 71 | end 72 | 73 | if no_sam == MAX_SAM 74 | warning(sprintf('RANSAC - termination forced after %d samples expected number of samples is %d', no_sam, exp_sam)); 75 | end; 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /CommonCfgAndIO/ConvertOldConfigToNew.m: -------------------------------------------------------------------------------- 1 | % CUT-N-PASTE 2 | config = configdata(argv()(1)); 3 | 4 | disp('[Paths]'); 5 | disp(strcat('Data: ', config.paths.data)); 6 | if isfield(config.paths, 'img') 7 | disp(strcat('Camera-Images: ', config.paths.img)); 8 | end 9 | 10 | disp('[Files]'); 11 | if isfield(config.files, 'basename') 12 | disp(strcat('Basename: ', config.files.basename)); 13 | end 14 | if isfield(config.files, 'imnames') 15 | disp(strcat('Image-Name-Prefix: ', config.files.imnames)); 16 | end 17 | if isfield(config.files, 'imgext') 18 | disp(strcat('Image-Extension: ', config.files.imgext)); 19 | end 20 | 21 | disp('[Images]'); 22 | if isfield(config.imgs, 'LEDsize') 23 | disp(strcat('LED-Size: ', num2str(config.imgs.LEDsize))); 24 | end 25 | if isfield(config.imgs, 'LEDcolor') 26 | disp(strcat('LED-Color: \'', config.imgs.LEDcolor, '\'')); 27 | end 28 | if isfield(config.imgs, 'LEDthr') 29 | disp(strcat('LED-Threshold: ', num2str(config.imgs.LEDthr))); 30 | end 31 | disp(strcat('Subpix: ', num2str(config.imgs.subpix))); 32 | 33 | disp('[Calibration]'); 34 | disp(strcat('Nonlinear-Parameters: ', num2str(config.cal.nonlinpar))); 35 | disp(strcat('Nonlinear-Update: ', num2str(config.cal.NL_UPDATE))); 36 | disp(strcat('Initial-Tolerance: ', num2str(config.cal.INL_TOL))); 37 | disp(strcat('Do-Global-Iterations: ', num2str(config.cal.DO_GLOBAL_ITER))); 38 | disp(strcat('Global-Iteration-Threshold: ', num2str(config.cal.GLOBAL_ITER_THR))); 39 | disp(strcat('Global-Iteration-Max: ', num2str(config.cal.GLOBAL_ITER_MAX))); 40 | disp(strcat('Num-Cameras-Fill: ', num2str(config.cal.NUM_CAMS_FILL))); 41 | disp(strcat('Do-Bundle-Adjustment: ', num2str(config.cal.DO_BA))); 42 | disp(strcat('Undo-Radial: ', num2str(config.cal.UNDO_RADIAL))); 43 | disp(strcat('Min-Points-Value: ', num2str(config.cal.MIN_PTS_VAL))); 44 | disp(strcat('N-Tuples: ', num2str(config.cal.NTUPLES))); 45 | disp(strcat('Square-Pixels: ', num2str(config.cal.SQUARE_PIX))); 46 | disp(strcat('Use-Nth-Frame: ',num2str(config.cal.USE_NTH_FRAME))); 47 | -------------------------------------------------------------------------------- /CalTechCal/rigid_motion.m: -------------------------------------------------------------------------------- 1 | function [Y,dYdom,dYdT] = rigid_motion(X,om,T); 2 | 3 | %rigid_motion.m 4 | % 5 | %[Y,dYdom,dYdT] = rigid_motion(X,om,T) 6 | % 7 | %Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). 8 | % 9 | %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) 10 | % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame 11 | % om: rotation vector (3x1 vector); T: translation vector (3x1 vector) 12 | % 13 | %OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) 14 | % dYdom: Derivative of Y with respect to om ((3N)x3 matrix) 15 | % dYdT: Derivative of Y with respect to T ((3N)x3 matrix) 16 | % 17 | %Definitions: 18 | %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) 19 | %The coordinate vector of P in the camera reference frame is: Y = R*X + T 20 | %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); 21 | % 22 | %Important function called within that program: 23 | % 24 | %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector 25 | 26 | 27 | 28 | if nargin < 3, 29 | T = zeros(3,1); 30 | if nargin < 2, 31 | om = zeros(3,1); 32 | if nargin < 1, 33 | error('Need at least a 3D structure as input (in rigid_motion.m)'); 34 | return; 35 | end; 36 | end; 37 | end; 38 | 39 | 40 | [R,dRdom] = rodrigues(om); 41 | 42 | [m,n] = size(X); 43 | 44 | Y = R*X + repmat(T,[1 n]); 45 | 46 | if nargout > 1, 47 | 48 | 49 | dYdR = zeros(3*n,9); 50 | dYdT = zeros(3*n,3); 51 | 52 | dYdR(1:3:end,1:3:end) = X'; 53 | dYdR(2:3:end,2:3:end) = X'; 54 | dYdR(3:3:end,3:3:end) = X'; 55 | 56 | dYdT(1:3:end,1) = ones(n,1); 57 | dYdT(2:3:end,2) = ones(n,1); 58 | dYdT(3:3:end,3) = ones(n,1); 59 | 60 | dYdom = dYdR * dRdom; 61 | 62 | end; 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/rroots.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define rr_a (*po) 5 | #define rr_d (*(po + 3)) 6 | #define eps (2.2204e-016) 7 | 8 | DEFUN_DLD (rroots, args, , 9 | "[...] = rroots (...)\n") 10 | { 11 | Matrix ovpo (args(0).matrix_value()); 12 | 13 | double *po = ovpo.rep->data; // this is a naughty way to get a pointer to the data 14 | 15 | double b,c, b2, bt, v, pit, e, *r; 16 | double p, q, D, A, cosphi, phit, R, _2R; 17 | 18 | printf("rroots\n"); 19 | 20 | Matrix output_matrix; 21 | 22 | b = *(po + 1) / rr_a; 23 | c = *(po + 2) / rr_a; 24 | b2 = b*b; 25 | bt = b/3; 26 | 27 | p = (3*c - b2)/ 9; 28 | q = ((2 * b2 * b)/27 - b*c/3 + rr_d/rr_a) / 2; 29 | 30 | D = q*q + p*p*p; 31 | 32 | if (D > 0) 33 | { 34 | output_matrix = Matrix(1,1); 35 | r = output_matrix.rep->data; // this is a naughty way to get a pointer to the data 36 | 37 | A = sqrt(D) - q; 38 | if (A > 0) 39 | { 40 | v = pow(A,1.0/3); 41 | *r = v - p/v - bt; 42 | } else 43 | { 44 | v = pow(-A,1.0/3); 45 | *r = p/v - v - bt; 46 | } 47 | } else 48 | { 49 | /* if (p > -eps) 50 | { 51 | printf("%.17f\n", p); 52 | aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); 53 | r = mxGetPr(aDstP[0]); 54 | *r = pow(q,1.0/3) - bt; 55 | } 56 | else */ 57 | { 58 | 59 | output_matrix = Matrix(3,1); 60 | r = output_matrix.rep->data; // this is a naughty way to get a pointer to the data 61 | 62 | if (q > 0) e = 1; else e = -1; 63 | R = e * sqrt(-p); 64 | _2R = R *2; 65 | cosphi = q / (R*R*R); 66 | if (cosphi > 1) cosphi = 1; else 67 | if (cosphi < -1) cosphi = -1; 68 | phit = acos(cosphi) /3; 69 | pit = 3.14159265358979/3; 70 | 71 | r[0] = -_2R * cos(phit) -bt; 72 | r[1] = _2R * cos(pit - phit) -bt; 73 | r[2] = _2R * cos(pit + phit) -bt; 74 | } 75 | } 76 | return octave_value(output_matrix); 77 | } 78 | -------------------------------------------------------------------------------- /CalTechCal/apply_distortion.m: -------------------------------------------------------------------------------- 1 | function [xd,dxddk] = apply_distortion(x,k) 2 | 3 | 4 | % Complete the distortion vector if you are using the simple distortion model: 5 | length_k = length(k); 6 | if length_k <5 , 7 | k = [k ; zeros(5-length_k,1)]; 8 | end; 9 | 10 | 11 | [m,n] = size(x); 12 | 13 | % Add distortion: 14 | 15 | r2 = x(1,:).^2 + x(2,:).^2; 16 | 17 | r4 = r2.^2; 18 | 19 | r6 = r2.^3; 20 | 21 | 22 | % Radial distortion: 23 | 24 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; 25 | 26 | if nargout > 1, 27 | dcdistdk = [ r2' r4' zeros(n,2) r6']; 28 | end; 29 | 30 | 31 | xd1 = x .* (ones(2,1)*cdist); 32 | 33 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); 34 | 35 | if nargout > 1, 36 | dxd1dk = zeros(2*n,5); 37 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; 38 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; 39 | end; 40 | 41 | 42 | % tangential distortion: 43 | 44 | a1 = 2.*x(1,:).*x(2,:); 45 | a2 = r2 + 2*x(1,:).^2; 46 | a3 = r2 + 2*x(2,:).^2; 47 | 48 | delta_x = [k(3)*a1 + k(4)*a2 ; 49 | k(3) * a3 + k(4)*a1]; 50 | 51 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); 52 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); 53 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); 54 | 55 | if nargout > 1, 56 | ddelta_xdk = zeros(2*n,5); 57 | ddelta_xdk(1:2:end,3) = a1'; 58 | ddelta_xdk(1:2:end,4) = a2'; 59 | ddelta_xdk(2:2:end,3) = a3'; 60 | ddelta_xdk(2:2:end,4) = a1'; 61 | end; 62 | 63 | xd = xd1 + delta_x; 64 | 65 | if nargout > 1, 66 | dxddk = dxd1dk + ddelta_xdk ; 67 | if length_k < 5, 68 | dxddk = dxddk(:,1:length_k); 69 | end; 70 | end; 71 | 72 | 73 | return; 74 | 75 | % Test of the Jacobians: 76 | 77 | n = 10; 78 | 79 | lk = 1; 80 | 81 | x = 10*randn(2,n); 82 | k = 0.5*randn(lk,1); 83 | 84 | [xd,dxddk] = apply_distortion(x,k); 85 | 86 | 87 | % Test on k: OK!! 88 | 89 | dk = 0.001 * norm(k)*randn(lk,1); 90 | k2 = k + dk; 91 | 92 | [x2] = apply_distortion(x,k2); 93 | 94 | x_pred = xd + reshape(dxddk * dk,2,n); 95 | 96 | 97 | norm(x2-xd)/norm(x2 - x_pred) 98 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm_sub.m: -------------------------------------------------------------------------------- 1 | %fill_mm_sub Proj. reconstruction of a normed sub-scene. 2 | % 3 | % When the central image concept is used, the information which image is 4 | % the central image is passed to this function as input. 5 | % 6 | % Parameters: 7 | % 8 | % Mfull .. complete known parts of the problem, used here for the best 9 | % estimate of the fundamental matrices 10 | 11 | function [P,X, lambda, u1,u2, info] = fill_mm_sub(Mfull, M, central,opt,info) 12 | 13 | I = ~isnan(M(1:3:end,:)); 14 | [m n] = size(I); 15 | if isempty(central), central = 0; end 16 | 17 | P=[]; X=[]; lambda=[]; u1=1:m; u2=1:n; 18 | 19 | %estimate the fundamental matrices and epipoles with the method of [Har95] 20 | [F,ep,rows,nonrows] = M2Fe(Mfull, central); 21 | 22 | if ~isempty(nonrows), 23 | disp(sprintf('Used local images:%s.', sprintf(' %d', rows))); end 24 | if length(rows) < 2, return; end 25 | 26 | %determine scale faktors lambda_i_p 27 | if ~central, rows_central = 0; else rows_central = find(rows == central); end 28 | [lambda, Ilamb] = depth_estimation(M(k2i(rows),:),F,ep,rows, ... 29 | rows_central); 30 | 31 | % prepare info.show_prmm - for show_prmm function 32 | info.show_prmm.I = I; 33 | info.show_prmm.Idepths = zeros(m,n); info.show_prmm.Idepths(rows,:)=Ilamb; 34 | 35 | %build the rescaled measurement matrix B 36 | for i = 1:length(rows), B(k2i(i),:) = M(k2i(i),:).*([1;1;1]*lambda(i,:)); end 37 | 38 | %balance W by column-wise and "triplet-of-rows"-wise scalar multiplications 39 | B = balance_triplets(B, opt); 40 | 41 | %fit holes in JIM by Jacobs' algorithm 42 | [P,X, u1,u2, lambda1, info] = fill_prmm(B, Ilamb, central,opt,info); 43 | 44 | r1 = setdiff(1:length(rows),u1); r2 = setdiff(1:n,u2); 45 | 46 | lambda = lambda(r1,r2); % to fit P*X 47 | if ~isempty(lambda1), 48 | new = find(~Ilamb(r1,r2) & I(r1,r2)); lambda(new) = lambda1(new); end 49 | 50 | error = eucl_dist_only(B(k2i(r1), r2), P*X, ~isnan(B(3*r1,r2)), 3); 51 | if opt.verbose, disp(sprintf('Error balanced: %f', error)); end 52 | 53 | u1 = union(nonrows, rows(u1)); 54 | -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/erlangen.m: -------------------------------------------------------------------------------- 1 | function [align] = erlangen(in,config) 2 | % erlangen ... local routines for the hoengg installation 3 | % 4 | % [align] = erlangen(in,config) 5 | % in, cam, config ... see the main GOCAL script 6 | % 7 | % align ... structures aligned wit the specified world frame 8 | % 9 | % $Id: erlangen.m,v 1.3 2005/05/20 15:31:30 svoboda Exp $ 10 | 11 | REALVIZ = 0; 12 | 13 | Cst = in.Cst; 14 | Rot = in.Rot; 15 | 16 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); 17 | 18 | % definition of the absolute world frame 19 | 20 | if REALVIZ 21 | cam(1).C = [1.20055 -1.85769 4.02702]'; 22 | cam(2).C = [0.382772 -4.49652 5.28274]'; 23 | cam(3).C = [-2.99133 -3.90767 6.1968]'; 24 | cam(4).C = [-2.92944 -2.40276 -7.78579]'; 25 | cam(5).C = [-9.00505 -5.61218 -9.73132]'; 26 | cam(6).C = [-7.76965 -2.94546 -1.63609]'; 27 | else % own measurement 28 | cam(1).C = [1.16, 0.3, 1.8]'; 29 | cam(2).C = [1.1, 3.0, 2.5]'; 30 | cam(3).C = [-0.9, 2.4, 1.56]'; 31 | cam(4).C = [0.15, -2.5, 2.2]'; 32 | cam(5).C = [-1.95, -2.65, 2.45]'; 33 | cam(6).C = [-1.75, -2.2, 1.42]'; 34 | end 35 | % of the similarity computation 36 | 37 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst'],[cam(:).C]); 38 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 39 | % save aligned data 40 | if 1 % SAVE_STEPHI | SAVE_PGUHA 41 | [align.Cst,align.Rot] = savecalpar(align.P,config); 42 | end 43 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 44 | 45 | set(gca,'CameraTarget',[0,0,0]); 46 | set(gca,'CameraPosition',[0,0,1]); 47 | 48 | figure(61), 49 | % print -depsc graphevalaligned.eps 50 | eval(['print -depsc ', config.paths.data, 'topview.eps']) 51 | 52 | drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); 53 | 54 | set(gca,'CameraTarget',[0,0,0.9]); 55 | set(gca,'CameraPosition',[2,0,0.9]); 56 | 57 | figure(62), 58 | % print -depsc graphevalaligned.eps 59 | eval(['print -depsc ', config.paths.data, 'sideview.eps']) 60 | 61 | return 62 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fslcm.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define a11 A(0,0) 5 | #define a12 A(0,1) 6 | #define a13 A(0,3) 7 | #define a21 A(1,0) 8 | #define a22 A(1,1) 9 | #define a23 A(1,2) 10 | #define a31 A(2,0) 11 | #define a32 A(2,1) 12 | #define a33 A(2,2) 13 | 14 | #define b11 B(0,0) 15 | #define b12 B(0,1) 16 | #define b13 B(0,2) 17 | #define b21 B(1,0) 18 | #define b22 B(1,1) 19 | #define b23 B(1,2) 20 | #define b31 B(2,0) 21 | #define b32 B(2,1) 22 | #define b33 B(2,2) 23 | 24 | DEFUN_DLD (fslcm, args, , 25 | "[...] = fslcm (...)\n") 26 | { 27 | Matrix A (args(0).matrix_value()); 28 | Matrix B (args(1).matrix_value()); 29 | 30 | ColumnVector p (4); 31 | 32 | printf("fslcm\n"); 33 | 34 | p(0) = -(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - 35 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; 36 | 37 | p(1) = -(a33*b12*b21) + a32*b13*b21 + a33*b11*b22 - 38 | a31*b13*b22 - a32*b11*b23 + a31*b12*b23 + 39 | a23*b12*b31 - a22*b13*b31 - a13*b22*b31 + 40 | 3*b13*b22*b31 + a12*b23*b31 - 3*b12*b23*b31 - 41 | a23*b11*b32 + a21*b13*b32 + a13*b21*b32 - 42 | 3*b13*b21*b32 - a11*b23*b32 + 3*b11*b23*b32 + 43 | (a22*b11 - a21*b12 - a12*b21 + 3*b12*b21 + a11*b22 - 44 | 3*b11*b22)*b33; 45 | 46 | p(2) = -(a21*a33*b12) + a21*a32*b13 + 47 | a13*a32*b21 - a12*a33*b21 + 2*a33*b12*b21 - 48 | 2*a32*b13*b21 - a13*a31*b22 + a11*a33*b22 - 49 | 2*a33*b11*b22 + 2*a31*b13*b22 + a12*a31*b23 - 50 | a11*a32*b23 + 2*a32*b11*b23 - 2*a31*b12*b23 + 51 | 2*a13*b22*b31 - 3*b13*b22*b31 - 2*a12*b23*b31 + 52 | 3*b12*b23*b31 + a13*a21*b32 - 2*a21*b13*b32 - 53 | 2*a13*b21*b32 + 3*b13*b21*b32 + 2*a11*b23*b32 - 54 | 3*b11*b23*b32 + a23* 55 | (-(a32*b11) + a31*b12 + a12*b31 - 2*b12*b31 - 56 | a11*b32 + 2*b11*b32) + 57 | (-(a12*a21) + 2*a21*b12 + 2*a12*b21 - 3*b12*b21 - 58 | 2*a11*b22 + 3*b11*b22)*b33 + 59 | a22*(a33*b11 - a31*b13 - a13*b31 + 2*b13*b31 + 60 | a11*b33 - 2*b11*b33); 61 | 62 | for (int i=0; i < 3; i++) { 63 | for (int j=0; j < 3; j++) { 64 | B(i,j) = A(i,j) - B(i,j); 65 | } 66 | } 67 | 68 | p(3) =-(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - 69 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; 70 | } 71 | -------------------------------------------------------------------------------- /MultiCamSelfCal/Ransac/fslcm.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mex.h" 3 | #include "matrix.h" 4 | 5 | #define a11 (*A) 6 | #define a12 (*(A+1)) 7 | #define a13 (*(A+2)) 8 | #define a21 (*(A+3)) 9 | #define a22 (*(A+4)) 10 | #define a23 (*(A+5)) 11 | #define a31 (*(A+6)) 12 | #define a32 (*(A+7)) 13 | #define a33 (*(A+8)) 14 | 15 | #define b11 (*B) 16 | #define b12 (*(B+1)) 17 | #define b13 (*(B+2)) 18 | #define b21 (*(B+3)) 19 | #define b22 (*(B+4)) 20 | #define b23 (*(B+5)) 21 | #define b31 (*(B+6)) 22 | #define b32 (*(B+7)) 23 | #define b33 (*(B+8)) 24 | 25 | 26 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 27 | { 28 | double *A = mxGetPr(aSrcP[0]), *B = mxGetPr(aSrcP[1]); 29 | double *p; 30 | int i; 31 | 32 | aDstP[0] = mxCreateDoubleMatrix(4, 1, mxREAL); 33 | p = mxGetPr(aDstP[0]); 34 | 35 | *p = -(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - 36 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; 37 | 38 | *(p+1) = -(a33*b12*b21) + a32*b13*b21 + a33*b11*b22 - 39 | a31*b13*b22 - a32*b11*b23 + a31*b12*b23 + 40 | a23*b12*b31 - a22*b13*b31 - a13*b22*b31 + 41 | 3*b13*b22*b31 + a12*b23*b31 - 3*b12*b23*b31 - 42 | a23*b11*b32 + a21*b13*b32 + a13*b21*b32 - 43 | 3*b13*b21*b32 - a11*b23*b32 + 3*b11*b23*b32 + 44 | (a22*b11 - a21*b12 - a12*b21 + 3*b12*b21 + a11*b22 - 45 | 3*b11*b22)*b33; 46 | 47 | *(p+2) = -(a21*a33*b12) + a21*a32*b13 + 48 | a13*a32*b21 - a12*a33*b21 + 2*a33*b12*b21 - 49 | 2*a32*b13*b21 - a13*a31*b22 + a11*a33*b22 - 50 | 2*a33*b11*b22 + 2*a31*b13*b22 + a12*a31*b23 - 51 | a11*a32*b23 + 2*a32*b11*b23 - 2*a31*b12*b23 + 52 | 2*a13*b22*b31 - 3*b13*b22*b31 - 2*a12*b23*b31 + 53 | 3*b12*b23*b31 + a13*a21*b32 - 2*a21*b13*b32 - 54 | 2*a13*b21*b32 + 3*b13*b21*b32 + 2*a11*b23*b32 - 55 | 3*b11*b23*b32 + a23* 56 | (-(a32*b11) + a31*b12 + a12*b31 - 2*b12*b31 - 57 | a11*b32 + 2*b11*b32) + 58 | (-(a12*a21) + 2*a21*b12 + 2*a12*b21 - 3*b12*b21 - 59 | 2*a11*b22 + 3*b11*b22)*b33 + 60 | a22*(a33*b11 - a31*b13 - a13*b31 + 2*b13*b31 + 61 | a11*b33 - 2*b11*b33); 62 | 63 | for (i=0; i < 9; i++) 64 | B[i] = A[i] - B[i]; 65 | 66 | *(p+3) =-(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - 67 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; 68 | } 69 | -------------------------------------------------------------------------------- /MultiCamValidation/Ransac/fslcm.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "mex.h" 3 | #include "matrix.h" 4 | 5 | #define a11 (*A) 6 | #define a12 (*(A+1)) 7 | #define a13 (*(A+2)) 8 | #define a21 (*(A+3)) 9 | #define a22 (*(A+4)) 10 | #define a23 (*(A+5)) 11 | #define a31 (*(A+6)) 12 | #define a32 (*(A+7)) 13 | #define a33 (*(A+8)) 14 | 15 | #define b11 (*B) 16 | #define b12 (*(B+1)) 17 | #define b13 (*(B+2)) 18 | #define b21 (*(B+3)) 19 | #define b22 (*(B+4)) 20 | #define b23 (*(B+5)) 21 | #define b31 (*(B+6)) 22 | #define b32 (*(B+7)) 23 | #define b33 (*(B+8)) 24 | 25 | 26 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) 27 | { 28 | double *A = mxGetPr(aSrcP[0]), *B = mxGetPr(aSrcP[1]); 29 | double *p; 30 | int i; 31 | 32 | aDstP[0] = mxCreateDoubleMatrix(4, 1, mxREAL); 33 | p = mxGetPr(aDstP[0]); 34 | 35 | *p = -(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - 36 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; 37 | 38 | *(p+1) = -(a33*b12*b21) + a32*b13*b21 + a33*b11*b22 - 39 | a31*b13*b22 - a32*b11*b23 + a31*b12*b23 + 40 | a23*b12*b31 - a22*b13*b31 - a13*b22*b31 + 41 | 3*b13*b22*b31 + a12*b23*b31 - 3*b12*b23*b31 - 42 | a23*b11*b32 + a21*b13*b32 + a13*b21*b32 - 43 | 3*b13*b21*b32 - a11*b23*b32 + 3*b11*b23*b32 + 44 | (a22*b11 - a21*b12 - a12*b21 + 3*b12*b21 + a11*b22 - 45 | 3*b11*b22)*b33; 46 | 47 | *(p+2) = -(a21*a33*b12) + a21*a32*b13 + 48 | a13*a32*b21 - a12*a33*b21 + 2*a33*b12*b21 - 49 | 2*a32*b13*b21 - a13*a31*b22 + a11*a33*b22 - 50 | 2*a33*b11*b22 + 2*a31*b13*b22 + a12*a31*b23 - 51 | a11*a32*b23 + 2*a32*b11*b23 - 2*a31*b12*b23 + 52 | 2*a13*b22*b31 - 3*b13*b22*b31 - 2*a12*b23*b31 + 53 | 3*b12*b23*b31 + a13*a21*b32 - 2*a21*b13*b32 - 54 | 2*a13*b21*b32 + 3*b13*b21*b32 + 2*a11*b23*b32 - 55 | 3*b11*b23*b32 + a23* 56 | (-(a32*b11) + a31*b12 + a12*b31 - 2*b12*b31 - 57 | a11*b32 + 2*b11*b32) + 58 | (-(a12*a21) + 2*a21*b12 + 2*a12*b21 - 3*b12*b21 - 59 | 2*a11*b22 + 3*b11*b22)*b33 + 60 | a22*(a33*b11 - a31*b13 - a13*b31 + 2*b13*b31 + 61 | a11*b33 - 2*b11*b33); 62 | 63 | for (i=0; i < 9; i++) 64 | B[i] = A[i] - B[i]; 65 | 66 | *(p+3) =-(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - 67 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; 68 | } 69 | -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/findinl.m: -------------------------------------------------------------------------------- 1 | % FindInl find inliers in joint image matrix 2 | % by pairwise epipolar geometry 3 | % 4 | % function IdMatIn = findinl(Ws,IdMat,tol) 5 | % Ws ... 3MxN joint image matrix 6 | % IdMat ... MxN ... 0 -> no point detected 7 | % 1 -> point detected 8 | % tol ... [pixels] tolerance for the epipolar geometry 9 | % the point are accpted as outliers only if they 10 | % are closer to the epipolar line than tol 11 | 12 | % $Author: svoboda $ 13 | % $Revision: 2.0 $ 14 | % $Id: findinl.m,v 2.0 2003/06/19 12:07:09 svoboda Exp $ 15 | % $State: Exp $ 16 | 17 | function IdMatIn = findinl(Ws,IdMat,tol) 18 | 19 | NoCams = size(IdMat,1); 20 | 21 | % fill the array of structures not_used denoted as 0 22 | % allocate the array of structures for used 23 | for i=1:NoCams, 24 | not_used(i).pts = sum(IdMat(i,:)); 25 | used(i).pts = -1; 26 | end 27 | 28 | % allocate IdMat for outliers 29 | IdMatIn = zeros(size(IdMat)); 30 | 31 | while (sum([not_used.pts])>1-NoCams), 32 | [buff, id.cam_max] = max([not_used.pts]); 33 | used = add(used, id.cam_max, not_used(id.cam_max).pts); 34 | not_used = remove(not_used, id.cam_max); 35 | Mask = repmat(IdMat(id.cam_max,:),NoCams,1); 36 | Corresp = Mask & IdMat; 37 | Corresp(id.cam_max,:) = 0; 38 | [buff, id.cam_to_pair] = max(sum(Corresp')); % find the camera with most correspondences 39 | idx.corr_to_pair = find(sum(IdMat([id.cam_max,id.cam_to_pair],:))==2); 40 | % used = add(used, id.cam_to_pair, not_used(id.cam_to_pair).pts); 41 | % not_used = remove(not_used, id.cam_to_pair); 42 | if size(idx.corr_to_pair,2)<8, 43 | error('Not enough points to compute epipolar geometry in RANSAC validation') 44 | end 45 | Wspair = []; 46 | Wspair = Ws(id.cam_max*3-2:id.cam_max*3, idx.corr_to_pair); 47 | Wspair = [Wspair; Ws(id.cam_to_pair*3-2:id.cam_to_pair*3, idx.corr_to_pair)]; 48 | % id 49 | [F, inls] = rEG(Wspair,tol,tol,0.99); 50 | IdMatIn(id.cam_max, idx.corr_to_pair(inls)) = 1; 51 | IdMat(id.cam_max, :) = 0; 52 | IdMat(id.cam_max, idx.corr_to_pair(inls)) = 1; 53 | end 54 | 55 | function list = add(list, id, value) 56 | list(id).pts = value; 57 | return 58 | 59 | function list = remove(list, id) 60 | list(id).pts = -1; 61 | return -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/findinl.m: -------------------------------------------------------------------------------- 1 | % FindInl find inliers in joint image matrix 2 | % by pairwise epipolar geometry 3 | % 4 | % function IdMatIn = findinl(Ws,IdMat,tol) 5 | % Ws ... 3MxN joint image matrix 6 | % IdMat ... MxN ... 0 -> no point detected 7 | % 1 -> point detected 8 | % tol ... [pixels] tolerance for the epipolar geometry 9 | % the point are accpted as outliers only if they 10 | % are closer to the epipolar line than tol 11 | 12 | % $Author: svoboda $ 13 | % $Revision: 2.1 $ 14 | % $Id: findinl.m,v 2.1 2003/07/30 10:28:29 svoboda Exp $ 15 | % $State: Exp $ 16 | 17 | function IdMatIn = findinl(Ws,IdMat,tol) 18 | 19 | NoCams = size(IdMat,1); 20 | 21 | % fill the array of structures not_used denoted as 0 22 | % allocate the array of structures for used 23 | for i=1:NoCams, 24 | not_used(i).pts = sum(IdMat(i,:)); 25 | used(i).pts = -1; 26 | end 27 | 28 | % allocate IdMat for outliers 29 | IdMatIn = zeros(size(IdMat)); 30 | 31 | while (sum([not_used.pts])>1-NoCams), 32 | [buff, id.cam_max] = max([not_used.pts]); 33 | used = add(used, id.cam_max, not_used(id.cam_max).pts); 34 | not_used = remove(not_used, id.cam_max); 35 | Mask = repmat(IdMat(id.cam_max,:),NoCams,1); 36 | Corresp = Mask & IdMat; 37 | Corresp(id.cam_max,:) = 0; 38 | [buff, id.cam_to_pair] = max(sum(Corresp')); % find the camera with most correspondences 39 | idx.corr_to_pair = find(sum(IdMat([id.cam_max,id.cam_to_pair],:))==2); 40 | % used = add(used, id.cam_to_pair, not_used(id.cam_to_pair).pts); 41 | % not_used = remove(not_used, id.cam_to_pair); 42 | if size(idx.corr_to_pair,2)<8, 43 | error('Not enough points to compute epipolar geometry in RANSAC validation') 44 | end 45 | Wspair = []; 46 | Wspair = Ws(id.cam_max*3-2:id.cam_max*3, idx.corr_to_pair); 47 | Wspair = [Wspair; Ws(id.cam_to_pair*3-2:id.cam_to_pair*3, idx.corr_to_pair)]; 48 | % id 49 | [F, inls] = rEG(Wspair,tol,tol,0.99); 50 | IdMatIn(id.cam_max, idx.corr_to_pair(inls)) = 1; 51 | IdMat(id.cam_max, :) = 0; 52 | IdMat(id.cam_max, idx.corr_to_pair(inls)) = 1; 53 | end 54 | 55 | function list = add(list, id, value) 56 | list(id).pts = value; 57 | return 58 | 59 | function list = remove(list, id) 60 | list(id).pts = -1; 61 | return 62 | -------------------------------------------------------------------------------- /MultiCamSelfCal/FindingPoints/atlanticall.pm: -------------------------------------------------------------------------------- 1 | $process={"compname"=>"atlantic3","camIds"=>[3],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 2 | push(@processes,$process); 3 | $process={"compname"=>"atlantic4","camIds"=>[4],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 4 | push(@processes,$process); 5 | $process={"compname"=>"atlantic5","camIds"=>[5],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 6 | push(@processes,$process); 7 | $process={"compname"=>"atlantic6","camIds"=>[6],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 8 | push(@processes,$process); 9 | $process={"compname"=>"atlantic7","camIds"=>[7],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 10 | push(@processes,$process); 11 | $process={"compname"=>"atlantic8","camIds"=>[8],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 12 | push(@processes,$process); 13 | $process={"compname"=>"atlantic9","camIds"=>[9],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 14 | push(@processes,$process); 15 | $process={"compname"=>"atlantic10","camIds"=>[10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 16 | push(@processes,$process); 17 | $process={"compname"=>"atlantic11","camIds"=>[11],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 18 | push(@processes,$process); 19 | $process={"compname"=>"atlantic12","camIds"=>[12],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 20 | push(@processes,$process); 21 | $process={"compname"=>"atlantic13","camIds"=>[13],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 22 | push(@processes,$process); 23 | $process={"compname"=>"atlantic14","camIds"=>[14],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 24 | push(@processes,$process); 25 | $process={"compname"=>"atlantic15","camIds"=>[15],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 26 | push(@processes,$process); 27 | $process={"compname"=>"atlantic16","camIds"=>[16],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 28 | push(@processes,$process); 29 | $process={"compname"=>"atlantic17","camIds"=>[17],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 30 | push(@processes,$process); 31 | $process={"compname"=>"atlantic18","camIds"=>[18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; 32 | push(@processes,$process); 33 | -------------------------------------------------------------------------------- /pymulticamselfcal/align.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def estsimt(X1,X2): 4 | # from estsimt.m in MultiCameSelfCal 5 | 6 | # ESTimate SIMilarity Transformation 7 | # 8 | # [s,R,T] = estsimt(X1,X2) 9 | # 10 | # X1,X2 ... 3xN matrices with corresponding 3D points 11 | # 12 | # X2 = s*R*X1 + T 13 | # s ... scalar scale 14 | # R ... 3x3 rotation matrix 15 | # T ... 3x1 translation vector 16 | # 17 | # This is done according to the paper: 18 | # "Least-Squares Fitting of Two 3-D Point Sets" 19 | # by K.S. Arun, T. S. Huang and S. D. Blostein 20 | 21 | N = X1.shape[1] 22 | if N != X2.shape[1]: 23 | raise ValueError('both X1 and X2 must have same number of points') 24 | 25 | X1cent = np.mean(X1,axis=1) 26 | X2cent = np.mean(X2,axis=1) 27 | # normalize coordinate systems for both set of points 28 | x1 = X1 - X1cent[:,np.newaxis] 29 | x2 = X2 - X2cent[:,np.newaxis] 30 | 31 | # mutual distances 32 | d1 = x1[:,1:]-x1[:,:-1] 33 | d2 = x2[:,1:]-x2[:,:-1] 34 | ds1 = np.sqrt( np.sum( d1**2, axis=0) ) 35 | ds2 = np.sqrt( np.sum( d2**2, axis=0) ) 36 | 37 | scales = ds2/ds1 38 | s = np.median( scales ) 39 | 40 | # undo scale 41 | x1s = s*x1 42 | 43 | # finding rotation 44 | H = np.zeros((3,3)) 45 | for i in range(N): 46 | tmp1 = x1s[:,i,np.newaxis] 47 | tmp2 = x2[np.newaxis,:,i] 48 | tmp = np.dot(tmp1,tmp2) 49 | H += tmp 50 | 51 | U,S,Vt = np.linalg.svd(H) 52 | V = Vt.T 53 | X = np.dot(V,U.T) 54 | R=X 55 | 56 | T = X2cent - s*np.dot(R,X1cent) 57 | return s,R,T 58 | 59 | def build_xform(s,R,t): 60 | T = np.zeros((4,4),dtype=float) 61 | T[:3,:3] = R 62 | T = s*T 63 | T[:3,3] = t 64 | T[3,3]=1.0 65 | return T 66 | 67 | def align_points( s,R,T, X ): 68 | T = build_xform(s,R,T) 69 | if X.shape[0]==3: 70 | # make homogeneous 71 | Xnew = np.ndarray((4,X.shape[1]),dtype=X.dtype) 72 | Xnew[3,:].fill(1) 73 | Xnew[:3,:] = X 74 | X = Xnew 75 | X = np.dot(T,X) 76 | return X 77 | 78 | def align_pmat( s,R,T, P ): 79 | T = build_xform(s,R,T) 80 | P = np.dot(P,np.linalg.inv(T)) 81 | return P 82 | 83 | def align_pmat2( M, P ): 84 | P = np.dot(P,np.linalg.inv(M)) 85 | return P 86 | -------------------------------------------------------------------------------- /MultiCamSelfCal/LocalAlignments/joinoscars.m: -------------------------------------------------------------------------------- 1 | % auxiliary script for joining particular oscar settings 2 | % 3 | % The current version is still not optimal. It would be good to have 4 | % some nicer version. It is not very automatic and it does not use the configdata 5 | % 6 | % $Id: joinoscars.m,v 1.5 2004/04/06 12:57:57 svoboda Exp $ 7 | % 8 | 9 | clear all; 10 | 11 | datapath = '/home/svoboda/viroomData/oscar/demo_3p3c_p%d/'; 12 | setups = [1:3]; % related to the data path 13 | path2save = '/home/svoboda/viroomData/oscar/oscardemo3/'; 14 | 15 | % Some dirty scale factors to make ETH Oscar setup compatible 16 | % with Leuven libraries 17 | % the coordinates and resolutions will be divided by theses values 18 | scalefactor.cameras = 1; 19 | scalefactor.projectors = 1; 20 | 21 | % the following indexes are related to the setups 22 | idxcams = [1:4]; 23 | idxproj = [5]; 24 | 25 | % load the partial data sets and join them in a consitent way 26 | % tested with three setups, three cameras and one projector each. 27 | % the three cameras are the same for all the particular datasets 28 | Ws = []; IdMat = []; Res = []; 29 | for i=1:size(setups,2), 30 | pts = load([sprintf(datapath,setups(i)),'points.dat']); 31 | proj(1:3*size(setups,2),1:size(pts,2)) = NaN; 32 | proj(i*3-2:i*3,:) = pts(idxproj*3-2:idxproj*3,:); 33 | proj(i*3-2:i*3-1,:) = proj(i*3-2:i*3-1,:)/scalefactor.projectors; 34 | pts(1:3:3*size(idxcams,2),:) = pts(1:3:3*size(idxcams,2),:)/scalefactor.cameras; 35 | pts(2:3:3*size(idxcams,2),:) = pts(2:3:3*size(idxcams,2),:)/scalefactor.cameras; 36 | Ws = [Ws, [pts(1:3*size(idxcams,2),:); proj]]; 37 | id = load([sprintf(datapath,setups(i)),'IdMat.dat']); 38 | projid(1:size(setups,2),1:size(pts,2)) = 0; 39 | projid(i,:)=id(idxproj,:); 40 | IdMat = [IdMat,[id(idxcams,:); projid]]; 41 | Res = [Res; load([sprintf(datapath,setups(i)),'Res.dat'])]; 42 | proj =[]; projid=[]; 43 | end 44 | % The Res matrix needs a special care when putting 45 | % camera and projector resolutions together 46 | Res2 = Res([idxcams,idxproj:size(idxcams,2)+1:end],:); 47 | Res2(idxcams,:) = Res2(idxcams,:)/scalefactor.cameras; 48 | Res2(end-2:end,:) = Res2(end-2:end,:)/scalefactor.projectors; 49 | 50 | % savings 51 | save([path2save,'points.dat'],'Ws','-ASCII'); 52 | save([path2save,'IdMat.dat'],'IdMat','-ASCII'); 53 | save([path2save,'Res.dat'],'Res2','-ASCII'); 54 | 55 | -------------------------------------------------------------------------------- /CalTechCal/gorad.m: -------------------------------------------------------------------------------- 1 | % main script to launch the estimation 2 | % of the non-linear parameters by using the CalTech 3 | % calibration toolbox and the output from the Svoboda's 4 | % Multicamera self-calibration 5 | % 6 | % How to create the input data: 7 | % 1) Run the MultiCamSelfCam 8 | % 2) Run the MultiCamValidation 9 | % 10 | % $Id: gorad.m,v 2.0 2003/06/19 12:06:00 svoboda Exp $ 11 | 12 | clear all; 13 | 14 | % TODO: isn't this unnecessary now? 15 | addpath ../MultiCamSelfCalib/Cfg 16 | 17 | % Read configuration from whatever is specified on command-line (via --config=FILENAME) 18 | config = read_configuration(); 19 | 20 | % if problem with desactivated images -> some problems with the estimation in general 21 | desactivated_images = []; 22 | 23 | idxcams = config.cal.cams2use; 24 | selfcalib.goradproblem = 0; 25 | 26 | for i = idxcams, 27 | [X_1,x_1] = preparedata(sprintf(config.files.points4cal,i)); 28 | go_calib_optim_iter 29 | if any(isnan(param)) 30 | % when the iteration fails insert null distortion 31 | % it is better than nonsense 32 | kc(1:4) = [0,0,0,0]; 33 | selfcalib.goradproblem=1; 34 | else 35 | visualize_distortions 36 | end 37 | 38 | disp(sprintf('***** camera %d **********************************',i)) 39 | % 40 | outputfile = sprintf(config.files.rad,i); 41 | 42 | fprintf(1,'\nExport of intrinsic calibration data to blue-c configuration file\n'); 43 | % outputfile = input('File basename: ', 's'); 44 | configfile = outputfile; 45 | disp(['Writing ' configfile]); 46 | 47 | fid = fopen(configfile, 'w'); 48 | 49 | fprintf(fid, 'K11 = %.16f\n', KK(1,1)); 50 | fprintf(fid, 'K12 = %.16f\n', KK(1,2)); 51 | fprintf(fid, 'K13 = %.16f\n', KK(1,3)); 52 | fprintf(fid, 'K21 = %.16f\n', KK(2,1)); 53 | fprintf(fid, 'K22 = %.16f\n', KK(2,2)); 54 | fprintf(fid, 'K23 = %.16f\n', KK(2,3)); 55 | fprintf(fid, 'K31 = %.16f\n', KK(3,1)); 56 | fprintf(fid, 'K32 = %.16f\n', KK(3,2)); 57 | fprintf(fid, 'K33 = %.16f\n\n', KK(3,3)); 58 | 59 | fprintf(fid, 'kc1 = %.16f\n', kc(1)); 60 | fprintf(fid, 'kc2 = %.16f\n', kc(2)); 61 | fprintf(fid, 'kc3 = %.16f\n', kc(3)); 62 | fprintf(fid, 'kc4 = %.16f\n', kc(4)); 63 | 64 | status = fclose(fid); 65 | 66 | disp('Press any key to continue'), pause 67 | 68 | %%% 69 | % clear already estimated parameters 70 | clear fc kc alpha_c cc 71 | end 72 | 73 | 74 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/estimatePX.m: -------------------------------------------------------------------------------- 1 | % estimatePX ... estimate the projective shape and motion 2 | % 3 | % [P,X,Lambda] = estimatePX(Ws, Lambda) 4 | % 5 | % Ws ....... 3*nxm measurement matrix 6 | % Lambda ... nxm matrix containg some initial projective depths 7 | % (see also: ESTIMATELAMBDA) 8 | % 9 | % P ........ 3*nx4 matrix containing the projective motion 10 | % X ........ 4xm matrix containing the projective shape 11 | % Lambda ... the new estimation of the projective depths 12 | 13 | function [P,X,Lambda] = estimatePX(Ws, Lambda) 14 | n = size(Ws,1)/3; 15 | m = size(Ws,2); 16 | 17 | % compute 1st updated Ws 18 | for i = 1:n 19 | for j = 1:m 20 | Ws_updated(3*i-2, j) = Ws(3*i-2, j) * Lambda(i, j); 21 | Ws_updated(3*i-1, j) = Ws(3*i-1, j) * Lambda(i, j); 22 | Ws_updated(3*i, j) = Ws(3*i, j) * Lambda(i, j); 23 | end 24 | end 25 | 26 | Lambda_new = Lambda; 27 | iterations = 0; 28 | errs = 1e10*[99.9,99]; 29 | tol = 1e-3; 30 | while (errs(iterations+1)-errs(iterations+2)>tol), 31 | [U,D,V] = svd(Ws_updated); 32 | % the following loop is not needed since these elements of D 33 | % are not considered in further computations 34 | for i = 5:size(D,2) 35 | D(i, i) = 0; 36 | end 37 | 38 | % projective shape X and motion P 39 | P = U*D(1:size(U,2),1:4); 40 | X = V(:,1:4)'; 41 | % U*D*V' == P*X 42 | 43 | % correct projective depths 44 | normfact = sum(P(3:3:(3*n),:)'.^2); 45 | Lambda_old = Lambda_new; 46 | Lambda_new = P(3:3:(3*n),:)./repmat(sqrt(normfact'),1,4)*X; 47 | 48 | % normalize lambdas 49 | lambnfr = sum(Lambda_new.^2); 50 | Lambda_new = sqrt(n)*Lambda_new./repmat(sqrt(lambnfr),n,1); 51 | lambnfc = sum(Lambda_new'.^2); 52 | Lambda_new = sqrt(m)*Lambda_new./repmat(sqrt(lambnfc'),1,m); 53 | 54 | for i = 1:n 55 | for j = 1:m 56 | Ws_updated(3*i-2, j) = Ws(3*i-2, j) * Lambda_new(i, j); 57 | Ws_updated(3*i-1, j) = Ws(3*i-1, j) * Lambda_new(i, j); 58 | Ws_updated(3*i, j) = Ws(3*i, j) * Lambda_new(i, j); 59 | end 60 | end 61 | iterations = iterations + 1; 62 | errs = [errs,sum(sum(abs(Lambda_old-Lambda_new)))]; 63 | % errs(iterations+2) 64 | end 65 | %iterations 66 | 67 | [U,D,V] = svd(Ws_updated); 68 | % compute new projective shape and motion 69 | P = U*D(1:size(U,2),1:4); 70 | X = V(:,1:4)'; 71 | X = X./repmat(X(4,:),4,1); 72 | Lambda = Lambda_new; 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /CalTechCal/comp_ext_calib.m: -------------------------------------------------------------------------------- 1 | %%% Computes the extrinsic parameters for all the active calibration images 2 | 3 | check_active_images; 4 | 5 | N_points_views = zeros(1,n_ima); 6 | 7 | for kk = 1:n_ima, 8 | 9 | if exist(['x_' num2str(kk)]), 10 | 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | eval(['X_kk = X_' num2str(kk) ';']); 13 | 14 | if (isnan(x_kk(1,1))), 15 | if active_images(kk), 16 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 17 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 18 | end; 19 | end; 20 | if active_images(kk), 21 | N_points_views(kk) = size(x_kk,2); 22 | [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); 23 | [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); 24 | if check_cond, 25 | if (cond(JJ_kk)> thresh_cond), 26 | active_images(kk) = 0; 27 | omckk = NaN*ones(3,1); 28 | Tckk = NaN*ones(3,1); 29 | fprintf(1,'\nWarning: View #%d ill-conditioned (C). This image is now set inactive.\n',kk) 30 | desactivated_images = [desactivated_images kk]; 31 | end; 32 | end; 33 | if isnan(omckk(1,1)), 34 | %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) 35 | active_images(kk) = 0; 36 | end; 37 | else 38 | omckk = NaN*ones(3,1); 39 | Tckk = NaN*ones(3,1); 40 | end; 41 | 42 | else 43 | 44 | omckk = NaN*ones(3,1); 45 | Tckk = NaN*ones(3,1); 46 | 47 | if active_images(kk), 48 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 49 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 50 | end; 51 | 52 | active_images(kk) = 0; 53 | 54 | end; 55 | 56 | eval(['omc_' num2str(kk) ' = omckk;']); 57 | eval(['Tc_' num2str(kk) ' = Tckk;']); 58 | 59 | end; 60 | 61 | 62 | check_active_images; 63 | -------------------------------------------------------------------------------- /MultiCamSelfCal/CoreFunctions/selfcalib.m: -------------------------------------------------------------------------------- 1 | function [Xe,Pe,C,R,T,foc] = selfcalib(Ws,IdMat) 2 | 3 | % selfcalib .... performs the self-calibration algorithm on 4 | % a measurement matrix 5 | % 6 | % [Xe,Pe,C,R,T,foc] = selfcalib(Ws) 7 | % 8 | % Ws ........ the 3*nxm measurement matrix 9 | % 10 | % Xe ........ 4xm matrix containg reconstructed calibration points 11 | % Pe ........ 3*CAMSx4 matrix containing estimated camera matrices 12 | % C ......... 4xn matrix containg the camera centers 13 | % R ......... 3*CAMSx3 matrix containing estimated camera rotation matrices 14 | % T ......... 3*n matrix containing the camera translation vectors 15 | % foc ....... CAMSx1 vector containing the focal lengths of the cameras 16 | 17 | 18 | POINTS = size(Ws,2); 19 | CAMS = size(Ws,1)/3; 20 | 21 | if 1 22 | % normalize image data 23 | % (see Hartley, p.91) 24 | T = []; % the CAMS*3x3 normalization transformations 25 | for i=1:CAMS 26 | [X_i, T_i] = isptnorm(Ws(i*3-2:i*3-1,IdMat(i,:)>0)'); 27 | Ws(i*3-2:i*3-1,IdMat(i,:)>0) = [X_i'; ones(1,sum(IdMat(i,:)>0))]; 28 | T = [T; T_i]; 29 | end 30 | else 31 | T = repmat(eye(3),CAMS,1); 32 | end 33 | 34 | % estimate projective depths 35 | Lambda_est = estimateLambda(Ws,IdMat); 36 | % Lambda_est = ones(CAMS,POINTS); 37 | 38 | if 1 39 | % normalize estimated lambdas. 40 | % it is more balancing than normalization 41 | % Check it again. Probably not correct? 42 | lambnfr = sum(Lambda_est.^2); 43 | Lambda_est = sqrt(CAMS)*Lambda_est./repmat(sqrt(lambnfr),CAMS,1); 44 | lambnfc = sum(Lambda_est'.^2); 45 | Lambda_est = sqrt(POINTS)*Lambda_est./repmat(sqrt(lambnfc'),1,POINTS); 46 | end 47 | 48 | % no need for negative lambdas 49 | Lambda_est = abs(Lambda_est); 50 | 51 | % Lambda check 52 | % employing lambdas, the Ws should have rank 4 ! 53 | if 0 54 | lambdaMat = []; 55 | for i=1:CAMS 56 | lambdaMat = [lambdaMat; repmat(Lambda_est(i,:),3,1)]; 57 | end 58 | Ws_rankcheck = lambdaMat.*Ws; 59 | [svd(Ws_rankcheck),svd(Ws)] 60 | end 61 | 62 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 63 | % compute projective shape and motion 64 | [P,X,Lambda] = estimatePX(Ws,Lambda_est); 65 | 66 | % undo normalization 67 | for i=1:CAMS 68 | P(3*i-2:3*i,:) = inv(T(3*i-2:3*i,:))*P(3*i-2:3*i,:); 69 | end 70 | 71 | % Euclidian reconstruction 72 | warn = 0; 73 | [Pe,Xe,C,R,T,foc,warn] = euclidize(Ws,Lambda,P,X); 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /MultiCamSelfCal/changes.txt: -------------------------------------------------------------------------------- 1 | $Id: changes.txt,v 2.2 2005/05/24 09:15:31 svoboda Exp $ 2 | 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | List of the most significant changes since the last public release 6 | from 30th July 2003 7 | 8 | - The RANSAC validation was rewritten as m-functions. Hence, no 9 | re-compilation of the c-codes is necessary. The user can switch 10 | between the mex- and m- version of the RANSAC by specifying the path 11 | in gocal.m addpath ../RansacM; % ./Ransac for mex functions The 12 | m-version can be slow for highly noisy data. 13 | 14 | The m-version of the RANSAC uses the 8-point algorithm not the 15 | 7-point algorithm as the c-version. It is then less optimal. I was 16 | just too lazy to rewrite the more complicated 7-point algorithm. I 17 | did many experiments, the 8-point algorithm did well in all cases. 18 | 19 | - gocal.m ... code cleaned, output better formatted (new function 20 | dispcamstats) 21 | 22 | - local alignments ... code cleaned, redundant I/O parameters 23 | (possible source of errors) removed 24 | 25 | - getpoint.m ... code cleaned, made a bit faster. Made compatible with 26 | the newest image processing toolbox, ver>5 27 | 28 | - showpoints.m ... it possible to switch between single images and 29 | image mosaics. The image mosaics may sometimes speed-up the 30 | debugging process when something goes wrong with the point 31 | detection. 32 | 33 | - The standard Matlab function quiver was copied to the BlueCCal 34 | distribution since it has problems in the newest Matlab ver>7. 35 | 36 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 37 | 38 | List of changes against to the last public release from 18th July 2003: 39 | 40 | Fixed Bugs in Code: 41 | 42 | - radial distorion ... proper handling of images with resolution 43 | different than 640x480 44 | 45 | 46 | Functionality: 47 | 48 | - support for non-color images added. You may either specify 49 | config.imgs.LEDcolor='intensity' and the color images will be 50 | converted to a grayscale or the GETPOINT detects that the image is 51 | only grayscale and it will be handled as such 52 | 53 | - main cycle in GOCAL. Some upgrade to make it more robust. I am 54 | testing the upgrades on all datasets I have. It should be backward 55 | compatible. Typically, new bad data discovers a small 56 | functionality-bug in the main iterative cycle. 57 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/create_nullspace__run_one_trial.oct: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | DEFUN_DLD(create_nullspace__run_one_trial, args, nargsout, 4 | "Run a single trial of the create_nullspace route") 5 | { 6 | int nargin = args.length(); 7 | if (nargin != 6) 8 | { 9 | error("create_nullspace__run_one_trial required 6 parameters"); 10 | } 11 | NDArray I = args(0).array_value(); 12 | NDArray cols_scaled = args(1).array_value(); 13 | NDArray M = args(2).array_value(); 14 | NDArray depths = args(3).array_value(); 15 | NDArray central = args(4).array_value(); 16 | Octave_map opt = args(5).map_value(); 17 | 18 | 19 | // function [nulltemp, result_code] = create_nullspace__run_one_trial(I, cols_scaled, M, depths, central, opt) 20 | 21 | //% choose a 4/max-tuple 22 | // 23 | //[m n] = size(I); 24 | //cols = 1:n; 25 | //rows = 1:m; 26 | //use_maxtuples = 0; % only for debugging 27 | // 28 | //cols_chosen = []; t=1; failed = 0; 29 | //if central, 30 | // scaled_ensured = 0; 31 | //else 32 | // scaled_ensured = 1; % trial version: no scale controling when cutting 33 | //end 34 | //for t = 1:4 35 | // % choose one column, cut useless parts etc. 36 | // [c, cols] = random_element(cols); 37 | // cols_chosen = [cols_chosen c]; 38 | 39 | // % check just added column 40 | // rows = intersect(rows, find(I(:,c) > 0)); 41 | 42 | // if t < 4, 43 | // [rows, cols, scaled_ensured] = cut_useless(I, cols_scaled, ... 44 | // cols_chosen, rows, cols, 4-t, scaled_ensured); 45 | // end 46 | // 47 | // if isempty(rows), failed = 1; break; end 48 | //end 49 | 50 | //if ~failed 51 | // % use the 4/max-tuple 52 | // d = depths(rows,cols_chosen); 53 | // % see ``debug code'' in the comment lower 54 | // 55 | // rowsbig = k2i(rows); 56 | // submatrix=[]; for j=1:length(cols_chosen) % 4, 57 | // submatrix=[ submatrix ... 58 | // spread_depths_col(M(rowsbig,cols_chosen(j)), d(:,j)) ]; end 59 | // debug=1; if debug, if size(submatrix, 1)<=size(submatrix,2) & opt.verbose 60 | // fprintf(1,'-'); end;end 61 | // subnull = nulleps(submatrix,opt.threshold); %svd(submatrix) 62 | // if size(subnull,2)>0 & ( use_maxtuples | ... 63 | // size(submatrix,1) == size(submatrix,2) + size(subnull,2)) 64 | // nulltemp = zeros(size(M,1),size(subnull,2)); 65 | // nulltemp(rowsbig,:) = subnull; % * (length(rows)/m); % weighting 66 | // result_code = 2; 67 | // else 68 | // nulltemp = []; 69 | // result_code = 1; 70 | // end 71 | //else 72 | // nulltemp = []; 73 | // result_code = 0; 74 | //end 75 | } 76 | -------------------------------------------------------------------------------- /CalTechCal/goradf.m: -------------------------------------------------------------------------------- 1 | % main function to launch the estimation 2 | % of the non-linear parameters by using the CalTech 3 | % calibration toolbox and the output from the Svoboda's 4 | % Multicamera self-calibration 5 | % 6 | % How to create the input data: 7 | % 1) Run the MultiCamSelfCam 8 | % 2) Run the MultiCamValidation 9 | % 10 | % $Id: goradf.m,v 2.2 2003/07/30 10:32:22 svoboda Exp $ 11 | 12 | function selfcalib = goradf(config,par2estimate,INL_TOL) 13 | 14 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 15 | 16 | % assignment of the parameters to estimate 17 | initFOV = par2estimate(1); 18 | center_optim = par2estimate(2); 19 | est_dist = par2estimate(3:6)'; 20 | 21 | % if problem with desactivated images -> some problems with the estimation in general 22 | desactivated_images = []; 23 | 24 | idxcams = config.cal.cams2use; 25 | selfcalib.goradproblem = 0; 26 | 27 | count = 0; 28 | 29 | for i = idxcams, 30 | count = count+1; 31 | [X_1,x_1] = preparedata(sprintf(config.files.points4cal,i)); 32 | % handle image resolutions correctly 33 | nx = config.cal.Res(count,1); 34 | ny = config.cal.Res(count,2); 35 | go_calib_optim_iter 36 | if any(isnan(param)) | any(err_std > 2*INL_TOL) 37 | % when the iteration fails insert null distortion 38 | % it is better than nonsense 39 | KK = [700 0 320; 0 700 240; 0 0 1]; % void calibration matrix 40 | kc(1:4) = [0,0,0,0]; 41 | selfcalib.goradproblem=1; 42 | else 43 | 44 | if ~Octave, 45 | visualize_distortions 46 | figure(2), 47 | eval(['print -depsc ', config.paths.data, sprintf('NonLinModel.cam%d.eps',i)]) 48 | end 49 | end 50 | % 51 | disp(sprintf('***** camera %d **********************************',i)) 52 | % 53 | outputfile = sprintf(config.files.rad,i); 54 | fprintf(1,'\nExport of intrinsic calibration data to blue-c configuration file\n'); 55 | % outputfile = input('File basename: ', 's'); 56 | configfile = outputfile; 57 | disp(['Writing ' configfile]); 58 | 59 | fid = fopen(configfile, 'w'); 60 | 61 | fprintf(fid, 'K11 = %.16f\n', KK(1,1)); 62 | fprintf(fid, 'K12 = %.16f\n', KK(1,2)); 63 | fprintf(fid, 'K13 = %.16f\n', KK(1,3)); 64 | fprintf(fid, 'K21 = %.16f\n', KK(2,1)); 65 | fprintf(fid, 'K22 = %.16f\n', KK(2,2)); 66 | fprintf(fid, 'K23 = %.16f\n', KK(2,3)); 67 | fprintf(fid, 'K31 = %.16f\n', KK(3,1)); 68 | fprintf(fid, 'K32 = %.16f\n', KK(3,2)); 69 | fprintf(fid, 'K33 = %.16f\n\n', KK(3,3)); 70 | 71 | fprintf(fid, 'kc1 = %.16f\n', kc(1)); 72 | fprintf(fid, 'kc2 = %.16f\n', kc(2)); 73 | fprintf(fid, 'kc3 = %.16f\n', kc(3)); 74 | fprintf(fid, 'kc4 = %.16f\n', kc(4)); 75 | 76 | status = fclose(fid); 77 | 78 | % disp('Press any key to continue'), pause 79 | 80 | %%% 81 | % clear already estimated parameters 82 | clear fc kc alpha_c cc nx ny 83 | 84 | end 85 | 86 | return 87 | 88 | -------------------------------------------------------------------------------- /MultiCamValidation/FindingPoints/im2pmultiproc.pl: -------------------------------------------------------------------------------- 1 | #! /usr/bin/perl -w 2 | 3 | # find projections of LEDs in images 4 | # it spreads the job to all specified processors/machines 5 | # It requires: 6 | # - matlab 7 | # 8 | # script is useful if more machines or more processors 9 | # are available. 10 | # automatic ssh authentication is necessary, see 11 | # http://www.cs.umd.edu/~arun/misc/ssh.html 12 | # http://www.cvm.uiuc.edu/~ms-drake/linux/ssh_nopw.html 13 | 14 | # $Author: svoboda $ 15 | # $Revision: 2.0 $ 16 | # $Id: im2pmultiproc.pl,v 2.0 2003/06/19 12:07:11 svoboda Exp $ 17 | # $State: Exp $ 18 | 19 | use Env; 20 | # use XML::Simple; 21 | # use Data::Dumper; 22 | 23 | # name of the m-file template 24 | $mfile="im2imstat"; 25 | $mvariable="CamsIds"; 26 | $donefile=".done"; 27 | 28 | # not yet ready 29 | # $config = XMLin('multiprocAtlantic.xml'); 30 | # print Dumper($config); 31 | # @processes = @$config-> 32 | 33 | # load the info about local machines and parallel processes 34 | use virooms; 35 | 36 | # just a debug cycle 37 | foreach $process (@processes) { 38 | print "$process->{'compname'} \n"; 39 | foreach $camId (@{$process->{'camIds'}}) { 40 | print "$camId \n"; 41 | } 42 | } 43 | 44 | # create temporary m-files with using the template m-file 45 | # store their names for delete at the very end 46 | # each process must have its own temporary m-file 47 | 48 | # compose the names and commands 49 | foreach $process (@processes) { 50 | $idfile=""; 51 | $str4cmd=""; 52 | foreach $camId (@{$process->{'camIds'}}) { 53 | $idfile = "$idfile$camId"; 54 | $str4cmd = "$str4cmd $camId"; 55 | } 56 | $process->{'scriptName'} = "$ENV{'PWD'}/$mfile${idfile}.m"; 57 | $process->{'donefile'} = "$ENV{'PWD'}/${donefile}${idfile}"; 58 | $process->{'catcmd'} = "echo \"${mvariable} = [${str4cmd}]; donefile='$process->{'donefile'}'; addpath $ENV{'PWD'}; addpath $ENV{'PWD'}/../Cfg; \" | cat - ${mfile}.m > $process->{'scriptName'}"; 59 | $process->{'mcmd'} = "ssh $process->{'compname'} /pub/bin/matlab -nosplash -nojvm < $process->{'scriptName'} > $process->{'scriptName'}.out &"; 60 | } 61 | 62 | print "*************************** \n"; 63 | 64 | # create the auxiliary scripts 65 | foreach $process (@processes) { 66 | system($process->{'catcmd'}); 67 | } 68 | 69 | # run the parallel matlabs 70 | foreach $process (@processes) { 71 | system($process->{'mcmd'}); 72 | } 73 | 74 | # wait until all processing is done 75 | print "Image processing in progress. Plase be patient \n"; 76 | do { 77 | sleep 10; 78 | @donefiles = glob("${donefile}*"); 79 | } while (@donefiles < @processes); 80 | 81 | # copy the data files to a coomon disc space yet to be implemented 82 | 83 | # final cleaning of the auxiliary files 84 | foreach $process (@processes) { 85 | system("rm -f $process->{'donefile'}"); 86 | system("rm -f $process->{'scriptName'}"); 87 | system("rm -f $process->{'scriptName'}.out"); 88 | } 89 | # system("rm -f *.out"); 90 | 91 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm_test/eval_y_and_dy.m: -------------------------------------------------------------------------------- 1 | %% 2 | % function eval_y_and_dy: (Np,1) --> (Ny,1)x(Ny,Np), [y,J]=F(p), value y and jacobian J=dF(p)/dp of the function. 3 | % 4 | % p ... size ((11+3)*K+3*N,1), p = [iP(1); u0(1); kappa(1); .. iP(K); u0(K); kappa(K); iX(1); .. iX(N)], 5 | % where Np = (11+2+raddeg)*K + 3*N 6 | % 7 | % Optional parameters: 8 | % TP{1:K} ... size (12,11), Pk(:) = P0k + TP{k}*iPk 9 | % TX{1:N} ... size (4,3), Xn = X0n + TX{n}*iXn 10 | % qivis ... size (K,N), has 1 in entries corresponding to visible image points 11 | % y ... size (2*nnz(qivis),1), visible image observations, stacked as [q(1,1); q(2,1); .. q(2*K,N)], unobserved q are omitted 12 | %% 13 | % We consider the imaging model as 14 | % u = hom(x) 15 | % x = P*X 16 | % P = P0 + TP*iP 17 | % X = X0 + TX*iX 18 | %% 19 | function [y,J] = eval_y_and_dy(p,P0,TP,X0,TX,y,qivis,RADIAL) 20 | % 21 | [K,N] = size(qivis); 22 | NR = RADIAL*3; % number of radial distortion paramters 23 | % 24 | % rearrange p to P, X [,radial] 25 | for k = 1:K 26 | P(k2i(k),:) = P0(k2i(k),:) + reshape(TP{k}*p([1:11]+(k-1)*(11+NR)),[3 4]); 27 | if RADIAL 28 | radial(k).u0 = p([12:13]+(k-1)*(11+NR)); 29 | radial(k).kappa = p([14]+(k-1)*(11+NR)); 30 | end 31 | end 32 | X = zeros(4,N); 33 | for n = 1:N 34 | X(:,n) = TX{n}*p((11+NR)*K+[1:3]+(n-1)*3); 35 | end 36 | X = X0 + X; 37 | % 38 | % compute retina points q 39 | for k = 1:K 40 | x(k2i(k),:) = P(k2i(k),:)*X; 41 | q(k2i(k,2),:) = nhom(x(k2i(k),:)); 42 | if RADIAL 43 | q(k2i(k,2),:) = raddist(q(k2i(k,2),:),radial(k).u0,radial(k).kappa); 44 | end 45 | end 46 | qq = reshape(q,[2 K*N]); 47 | qq = qq(:,qivis); 48 | y = qq(:)-y; % function value 49 | % 50 | % compute Jacobian J = dF(p)/dp 51 | if nargout<2 52 | return 53 | end 54 | [kvis,nvis] = find(qivis); 55 | Ji = zeros(2*length(kvis)*(11+NR+3),1); Jj = zeros(size(Ji)); Jv = zeros(size(Ji)); 56 | cnt = 0; 57 | for l = 1:length(kvis) % loop for all VISIBLE points in all cameras 58 | k = kvis(l); 59 | n = nvis(l); 60 | xl = x(k2i(k),n); 61 | ul = nhom(xl); 62 | 63 | % Compute derivatives (Jacobians). Notation: E.g., dudx = du(x)/dx, etc. 64 | dxdP = kron(X(:,n)',eye(3))*TP{k}; % dx(iP,iX)/diP 65 | dxdX = P(k2i(k),:)*TX{n}; % dx(iP,iX)/diX 66 | dudx = [eye(2) -ul]/xl(3); 67 | if RADIAL 68 | [dqdu,dqdu0,dqdkappa] = raddist(ul,radial(k).u0,radial(k).kappa); 69 | else 70 | dqdu = eye(2); dqdu0 = []; dqdkappa = []; 71 | end 72 | dqdP = dqdu*dudx*dxdP; % dq(iP,iX)/diP 73 | dqdX = dqdu*dudx*dxdX; % dq(iP,iX)/dX 74 | c = cnt+[1:2*(11+NR+3)]; 75 | [Ji(c),Jj(c),Jv(c)] = spidx([1:2]+(l-1)*2,[[1:(11+NR)]+(k-1)*(11+NR) (11+NR)*K+[1:3]+(n-1)*3],[dqdP dqdu0 dqdkappa dqdX]); 76 | cnt = cnt + 2*(11+NR+3); 77 | end 78 | J = sparse(Ji,Jj,Jv,length(y),length(p)); 79 | return 80 | 81 | function [i,j,v] = spidx(I,J,V) 82 | % 83 | i = I'*ones(1,length(J)); 84 | j = ones(length(I),1)*J; 85 | i = i(:); 86 | j = j(:); 87 | v = V(:); 88 | return 89 | 90 | -------------------------------------------------------------------------------- /MultiCamSelfCal/FindingPoints/im2pmultiproc.pl: -------------------------------------------------------------------------------- 1 | #! /usr/bin/perl -w 2 | 3 | # find projections of LEDs in images 4 | # it spreads the job to all specified processors/machines 5 | # It requires: 6 | # - matlab 7 | # - read_configuration 8 | # - expname 9 | # 10 | # script is useful if more machines or more processors 11 | # are available. 12 | # automatic ssh authentication is necessary, see 13 | # http://www.cs.umd.edu/~arun/misc/ssh.html 14 | # http://www.cvm.uiuc.edu/~ms-drake/linux/ssh_nopw.html 15 | 16 | # $Author: svoboda $ 17 | # $Revision: 2.0 $ 18 | # $Id: im2pmultiproc.pl,v 2.0 2003/06/19 12:06:52 svoboda Exp $ 19 | # $State: Exp $ 20 | 21 | use Env; 22 | # use XML::Simple; 23 | # use Data::Dumper; 24 | 25 | # name of the m-file template 26 | $mfile="im2imstat"; 27 | $mvariable="CamsIds"; 28 | $donefile=".done"; 29 | 30 | # not yet ready 31 | # $config = XMLin('multiprocAtlantic.xml'); 32 | # print Dumper($config); 33 | # @processes = @$config-> 34 | 35 | # load the info about local machines and parallel processes 36 | use atlantic; 37 | 38 | # just a debug cycle 39 | foreach $process (@processes) { 40 | print "$process->{'compname'} \n"; 41 | foreach $camId (@{$process->{'camIds'}}) { 42 | print "$camId \n"; 43 | } 44 | } 45 | 46 | # create temporary m-files with using the template m-file 47 | # store their names for delete at the very end 48 | # each process must have its own temporary m-file 49 | 50 | # compose the names and commands 51 | foreach $process (@processes) { 52 | $idfile=""; 53 | $str4cmd=""; 54 | foreach $camId (@{$process->{'camIds'}}) { 55 | $idfile = "$idfile$camId"; 56 | $str4cmd = "$str4cmd $camId"; 57 | } 58 | $process->{'scriptName'} = "$ENV{'PWD'}/$mfile${idfile}.m"; 59 | $process->{'donefile'} = "$ENV{'PWD'}/${donefile}${idfile}"; 60 | $process->{'catcmd'} = "echo \"${mvariable} = [${str4cmd}]; donefile='$process->{'donefile'}'; addpath $ENV{'PWD'}; addpath $ENV{'PWD'}/../Cfg; \" | cat - ${mfile}.m > $process->{'scriptName'}"; 61 | $process->{'mcmd'} = "ssh $process->{'compname'} /usr/sepp/bin/matlab -nosplash -nojvm < $process->{'scriptName'} > $process->{'scriptName'}.out &"; 62 | } 63 | 64 | print "*************************** \n"; 65 | 66 | # create the auxiliary scripts 67 | foreach $process (@processes) { 68 | system($process->{'catcmd'}); 69 | } 70 | 71 | # run the parallel matlabs 72 | foreach $process (@processes) { 73 | system($process->{'mcmd'}); 74 | } 75 | 76 | # wait until all processing is done 77 | print "Image processing in progress. Plase be patient \n"; 78 | do { 79 | sleep 10; 80 | @donefiles = glob("${donefile}*"); 81 | } while (@donefiles < @processes); 82 | 83 | # copy the data files to a coomon disc space yet to be implemented 84 | 85 | # final cleaning of the auxiliary files 86 | # foreach $process (@processes) { 87 | # system("rm -f $process->{'donefile'}"); 88 | # system("rm -f $process->{'scriptName'}"); 89 | # system("rm -f $process->{'scriptName'}.out"); 90 | # } 91 | # system("rm -f *.out"); 92 | 93 | -------------------------------------------------------------------------------- /BlueCFindingPoints/localconfig.m: -------------------------------------------------------------------------------- 1 | % LocalConfig configuration file for self-calibration experiments 2 | % 3 | % 4 | % config = configdata(experiment) 5 | % 6 | % experiment ... string with an experiment name 7 | 8 | function config = configdata(experiment) 9 | 10 | HOME_PATH = '/home/svoboda/Work/BlueCCal/'; 11 | 12 | % add paths 13 | addpath([HOME_PATH,'MultiCamSelfCal/FindingPoints']); 14 | addpath([HOME_PATH,'BlueCFindingPoints']); 15 | 16 | 17 | if nargin<1, 18 | display('No name of the experiment specified: >>basic<< used as default') 19 | experiment = 'basic'; 20 | end 21 | 22 | if strcmp(experiment,'basic') 23 | error; 24 | elseif strcmp(experiment,'BlueCHoengg') 25 | config.paths.data = '/local/tomas/'; 26 | config.paths.img = config.paths.data; 27 | config.files.imnames = 'arctic%d.pvi.*.'; 28 | config.files.idxcams = [1:16]; % related to the imnames 29 | config.files.imgext = 'jpg'; 30 | config.imgs.LEDsize = 7; % avg diameter of a LED in pixels 31 | config.imgs.LEDcolor = 'green'; % color of the laser pointer 32 | config.imgs.subpix = 1/5; 33 | elseif strcmp(experiment,'BlueCRZ') 34 | config.paths.data = '/local/tomas/'; 35 | config.paths.img = config.paths.data; 36 | config.files.imnames = 'atlantic%d.pvi.*.'; 37 | config.files.idxcams = [3:12,14:18]; % related to the imnames 38 | config.files.imgext = 'jpg'; 39 | config.imgs.LEDsize = 7; % avg diameter of a LED in pixels 40 | config.imgs.LEDcolor = 'green'; % color of the laser pointer 41 | config.imgs.subpix = 1/5; 42 | else 43 | error('Configdata: wrong identifier of the data set'); 44 | end 45 | 46 | 47 | % image resolution 48 | try config.imgs.res; catch, config.imgs.res = [640,480]; end; 49 | 50 | % scale for the subpixel accuracy 51 | % 1/3 is a good compromise between speed and accuracy 52 | % for high-resolution images or bigger LEDs you may try 1/1 or 1/2 53 | try config.imgs.subpix; catch, config.imgs.subpix = 1/3; end; 54 | 55 | % data names 56 | try config.files.Pmats; catch, config.files.Pmats = [config.paths.data,'Pmatrices.dat']; end; 57 | try config.files.points; catch, config.files.points = [config.paths.data,'points.dat']; end; 58 | try config.files.IdPoints; catch, config.files.IdPoints = [config.paths.data,'IdPoints.dat']; end; 59 | try config.files.Res; catch, config.files.Res = [config.paths.data,'Res.dat']; end; 60 | try config.files.IdMat; catch, config.files.IdMat = [config.paths.data,'IdMat.dat']; end; 61 | try config.files.inidx; catch, config.files.inidx = [config.paths.data,'idxin.dat']; end; 62 | try config.files.avIM; catch, config.files.avIM = [config.paths.data,'camera%d.average.tiff']; end; 63 | try config.files.stdIM; catch, config.files.stdIM = [config.paths.data,'camera%d.std.tiff']; end; 64 | try config.files.CalPar; catch, config.files.CalPar = [config.paths.data,'camera%d.cal']; end; 65 | try config.files.CalPmat; catch, config.files.CalPmat = [config.paths.data,'camera%d.Pmat.cal']; end; 66 | try config.files.StCalPar; catch, config.files.StCalPar = [config.paths.data,'atlantic%d.ethz.ch.cal']; end; 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /MultiCamSelfCal/BlueCLocal/bluecrz.m: -------------------------------------------------------------------------------- 1 | function [align] = bluecrz(in,config) 2 | % bluecrz ... localized output function for the BlueCRZ installation 3 | % 4 | % [align] = bluecrz(in,config) 5 | % in, cam, config ... see the main GOCAL script 6 | % 7 | % align ... structures aligned wit the specified world frame 8 | % 9 | % $Id: bluecrz.m,v 2.1 2005/05/20 15:27:37 svoboda Exp $ 10 | 11 | Cst = in.Cst; 12 | Rot = in.Rot; 13 | 14 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); 15 | 16 | horizplane.idx(1) = find(config.cal.cams2use==9); 17 | horizplane.idx(2) = find(config.cal.cams2use==10); 18 | horizplane.idx(3) = find(config.cal.cams2use==17); 19 | horizplane.idx(4) = find(config.cal.cams2use==18); 20 | horizplane.vec = pinv([Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*Cst(horizplane.idx,3); 21 | horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; 22 | horizplane.n = horizplane.par(1:3)'; 23 | 24 | % set the camera on top 25 | set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); 26 | set(gca,'CameraPosition',mean(Cst(horizplane.idx,:))+3*horizplane.n'); 27 | % figure(41), print -depsc grapheval.eps 28 | 29 | % definition of the absolute world frame 30 | cave.x=2.8; cave.y=2.8; cave.z=2.36; 31 | cam(9).C = [cave.x/2, -cave.y/2, cave.z]'; 32 | cam(10).C = [cave.x/2, cave.y/2, cave.z]'; 33 | cam(17).C = [-cave.x/2, -cave.y/2, cave.z]'; 34 | cam(18).C = [-cave.x/2, cave.y/2, cave.z]'; 35 | cam(4).C = [0,0.05,3.7]'; % relatively ad hoc values to improve the stability 36 | % of the similarity computation 37 | 38 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst(find(config.cal.cams2use==4),:)',Cst(horizplane.idx,1:3)'],[cam(:).C]); 39 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 40 | % save aligned data 41 | if 1 % SAVE_STEPHI | SAVE_PGUHA 42 | [align.Cst,align.Rot] = savecalpar(align.P,config); 43 | end 44 | % plot the 3D results from a better perspective by estimating the plane of the Cams 9,10,17,18 45 | % let call this plane "horizontal". 46 | % this plot makes sense only for the BigBlueC 47 | horizplane.vec = pinv([align.Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*align.Cst(horizplane.idx,3); 48 | horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; 49 | horizplane.n = horizplane.par(1:3)'; 50 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: View from the top camera',config.cal.cams2use); 51 | % set the camera on top 52 | set(gca,'CameraTarget',mean(align.Cst(horizplane.idx,:))); 53 | set(gca,'CameraPosition',mean(align.Cst(horizplane.idx,:))+3*horizplane.n'); 54 | % set(gca,'CameraPosition',align.Cst(find(config.cal.cams2use==4),:)); % view from the perspective of the camera4 55 | 56 | % drawscene(in.Xe,Cst',Rot,42,'cloud','Graphical Output Validation: View from side',config.cal.cams2use); 57 | % set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); 58 | % set(gca,'CameraPosition',mean(Cst(horizplane.idx,:)+Cst(horizplane(4),:)-Cst(horizplane(1),:))'); 59 | 60 | figure(61), 61 | % print -depsc graphevalaligned.eps 62 | eval(['print -depsc ', config.paths.data, 'graphevalaligned.eps']) 63 | 64 | return; 65 | -------------------------------------------------------------------------------- /CalTechCal/compute_extrinsic_refine.m: -------------------------------------------------------------------------------- 1 | function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), 2 | 3 | %compute_extrinsic 4 | % 5 | %[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) 6 | % 7 | %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection 8 | %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. 9 | %Works with planar and non-planar structures. 10 | % 11 | %INPUT: x_kk: Feature locations on the images 12 | % X_kk: Corresponding grid coordinates 13 | % fc: Camera focal length 14 | % cc: Principal point coordinates 15 | % kc: Distortion coefficients 16 | % alpha_c: Skew coefficient 17 | % MaxIter: Maximum number of iterations 18 | % 19 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space 20 | % Tckk: 3D translation vector attached to the grid positions in space 21 | % Rckk: 3D rotation matrices corresponding to the omc vectors 22 | 23 | % 24 | %Method: Computes the normalized point coordinates, then computes the 3D pose 25 | % 26 | %Important functions called within that program: 27 | % 28 | %normalize: Computes the normalize image point coordinates. 29 | % 30 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. 31 | % 32 | %project_points.m: Computes the 2D image projections of a set of 3D points 33 | 34 | 35 | if nargin < 10, 36 | thresh_cond = inf; 37 | end; 38 | 39 | 40 | if nargin < 9, 41 | MaxIter = 20; 42 | end; 43 | 44 | if nargin < 8, 45 | alpha_c = 0; 46 | if nargin < 7, 47 | kc = zeros(5,1); 48 | if nargin < 6, 49 | cc = zeros(2,1); 50 | if nargin < 5, 51 | fc = ones(2,1); 52 | if nargin < 4, 53 | error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); 54 | return; 55 | end; 56 | end; 57 | end; 58 | end; 59 | end; 60 | 61 | 62 | % Initialization: 63 | 64 | omckk = omc_init; 65 | Tckk = Tc_init; 66 | 67 | 68 | % Final optimization (minimize the reprojection error in pixel): 69 | % through Gradient Descent: 70 | 71 | param = [omckk;Tckk]; 72 | 73 | change = 1; 74 | 75 | iter = 0; 76 | 77 | %keyboard; 78 | 79 | %fprintf(1,'Gradient descent iterations: '); 80 | 81 | while (change > 1e-10)&(iter < MaxIter), 82 | 83 | %fprintf(1,'%d...',iter+1); 84 | 85 | [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); 86 | 87 | ex = x_kk - x; 88 | 89 | %keyboard; 90 | 91 | JJ = [dxdom dxdT]; 92 | 93 | if cond(JJ) > thresh_cond, 94 | change = 0; 95 | else 96 | 97 | JJ2 = JJ'*JJ; 98 | 99 | param_innov = inv(JJ2)*(JJ')*ex(:); 100 | param_up = param + param_innov; 101 | change = norm(param_innov)/norm(param_up); 102 | param = param_up; 103 | iter = iter + 1; 104 | 105 | omckk = param(1:3); 106 | Tckk = param(4:6); 107 | 108 | end; 109 | 110 | end; 111 | 112 | %fprintf(1,'\n'); 113 | 114 | Rckk = rodrigues(omckk); 115 | -------------------------------------------------------------------------------- /MultiCamSelfCal/BlueCLocal/bluechoengg.m: -------------------------------------------------------------------------------- 1 | function [align] = bluechoengg(in,config) 2 | % bluechoengg ... local routines for the hoengg installation 3 | % 4 | % [align] = bluechoengg(in,config) 5 | % in, config ... see the main GOCAL script 6 | % 7 | % align ... structures aligned wit the specified world frame 8 | % 9 | % $Id: bluechoengg.m,v 2.1 2005/05/20 15:27:37 svoboda Exp $ 10 | 11 | Cst = in.Cst; 12 | Rot = in.Rot; 13 | 14 | drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); 15 | 16 | horizplane.idx(1) = find(config.cal.cams2use==13); 17 | horizplane.idx(2) = find(config.cal.cams2use==14); 18 | horizplane.idx(3) = find(config.cal.cams2use==15); 19 | horizplane.idx(4) = find(config.cal.cams2use==16); 20 | horizplane.vec = pinv([Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*Cst(horizplane.idx,3); 21 | horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; 22 | horizplane.n = horizplane.par(1:3)'; 23 | 24 | % set the camera on top 25 | set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); 26 | set(gca,'CameraPosition',mean(Cst(horizplane.idx,:))+3*horizplane.n'); 27 | % figure(41), print -depsc grapheval.eps 28 | 29 | % definition of the absolute world frame 30 | % ccam(11).C = [1.40, -2.05, 2.70]; 31 | cam(13).C = [-1.40, -2.55, 2.70]'; 32 | cam(14).C = [-1.40, 2.70, 2.70]'; 33 | cam(15).C = [0, 2.70, 2.70]'; 34 | cam(16).C = [1.40, 2.70, 2.70]'; 35 | 36 | cam(1).C = [-3.70, -2.55, 1.55]'; % relatively ad hoc values to improve the stability 37 | cam(5).C = [-3.70, 4.06, 1.55]'; 38 | cam(6).C = [3.70, 4.06, 1.55]'; 39 | cam(10).C = [3.70, -2.55, 1.55]'; 40 | % of the similarity computation 41 | 42 | [align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst(find(config.cal.cams2use==1),:)', Cst(find(config.cal.cams2use==5),:)', Cst(find(config.cal.cams2use==6),:)', Cst(find(config.cal.cams2use==10),:)',Cst(horizplane.idx,1:3)'],[cam(:).C]); 43 | [align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); 44 | % save aligned data 45 | if 1 % SAVE_STEPHI | SAVE_PGUHA 46 | [align.Cst,align.Rot] = savecalpar(align.P,config); 47 | end 48 | % plot the 3D results from a better perspective by estimating the plane of the Cams 9,10,17,18 49 | % let call this plane "horizontal". 50 | % this plot makes sense only for the BigBlueC 51 | horizplane.vec = pinv([align.Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*align.Cst(horizplane.idx,3); 52 | horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; 53 | horizplane.n = horizplane.par(1:3)'; 54 | drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: View from the top camera',config.cal.cams2use); 55 | % set the camera on top 56 | set(gca,'CameraTarget',mean(align.Cst(horizplane.idx,:))); 57 | set(gca,'CameraPosition',mean(align.Cst(horizplane.idx,:))+3*horizplane.n'); 58 | % set(gca,'CameraPosition',align.Cst(find(config.cal.cams2use==15),:)); % view from the perspective of the camera4 59 | 60 | % drawscene(in.Xe,Cst',Rot,42,'cloud','Graphical Output Validation: View from side',config.cal.cams2use); 61 | % set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); 62 | % set(gca,'CameraPosition',mean(Cst(horizplane.idx,:)+Cst(horizplane(4),:)-Cst(horizplane(1),:))'); 63 | 64 | figure(61), 65 | % print -depsc graphevalaligned.eps 66 | eval(['print -depsc ', config.paths.data, 'graphevalaligned.eps']) 67 | 68 | return 69 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/balance_triplets.m: -------------------------------------------------------------------------------- 1 | %balance_triplets Balance PRMM by column-wise and "triplet-of-rows"-wise 2 | % scalar multiplications. 3 | % 4 | % After balancing, overall weight of M will be m*n where 3m*n is size of M 5 | % (i.e. 3 coordinates of each image point will give together 1 in average). 6 | % 7 | % Parameters: 8 | % opt.info_separately .. 1(default) .. display info on separate row 9 | % 0 .. display info in brackets 10 | % .verbose(1) .. whether display info 11 | 12 | function B = balance_triplets(M, opt); 13 | 14 | if nargin < 2, opt = []; end 15 | if ~isfield(opt,'info_separately') 16 | opt.info_separately = 1; end 17 | if ~isfield(opt,'verbose') 18 | opt.verbose = 1; end 19 | 20 | if opt.verbose 21 | if opt.info_separately, fprintf(1,'Balancing PRMM...'); tic 22 | else, fprintf(1,'(balancing PRMM...'); tic; end; end 23 | 24 | m=size(M,1)/3; %number of cameras 25 | n=size(M,2); %number of points 26 | 27 | B=M; change=inf; diff_rows = inf; diff_cols = inf; iteration = 0; 28 | while (change > 0.01 || diff_rows > 1 || diff_cols > 1) && iteration <= 20 29 | Bold=B; 30 | 31 | % 1. rescale each column l so that sum w_r_l^2=1 i.e. column of unit 32 | % length. However, due to the missing data, the length must be smaller 33 | % in (linear) dependance on amount of missing data. 34 | diff_cols = -inf; 35 | for l=1:n 36 | rows = find(~isnan(M(1:3:end,l))); 37 | if length(rows) > 0 38 | rowsb = k2i(rows); 39 | s = sum(B(rowsb,l) .^ 2); 40 | supposed_weight = length(rows); % the less data, the slighter impact 41 | diff_cols = max(abs(s-supposed_weight), diff_cols); 42 | B(rowsb,l) = B(rowsb,l) ./ sqrt(s/supposed_weight); 43 | end 44 | end 45 | 46 | % 2. rescale each triplet of rows so that it's sum w_i_l^2=1 i.e. unit 47 | % area. However, due to the missing data, the area must be smaller in 48 | % (linear) dependance on amount of missing data. 49 | diff_rows = -inf; 50 | for k=1:m 51 | cols = find(~isnan(M(3*k,:))); 52 | if length(cols) > 0 53 | s = sum(sum( B(3*k-2:3*k,cols) .^ 2 )); 54 | supposed_weight = length(cols); % the less data, the slighter impact 55 | diff_rows = max(abs(s-supposed_weight), diff_rows); 56 | B(3*k-2:3*k,cols) = B(3*k-2:3*k,cols) ./ sqrt(s/supposed_weight); 57 | end 58 | end 59 | 60 | % repeat steps 1 and 2 if W changed significantly 61 | % Note: It is not ensured that sums (s) would not change significantly 62 | % in the (hypothetical) next step. The reason is that rescaling 63 | % No. 1 rescales n columns to overall weight n whereas rescaling 64 | % No. 2 rescales m triplets of rows to overall weight m. 65 | change = 0; 66 | for k=1:m 67 | cols = find(~isnan(M(3*k,:))); 68 | change = change + sum(sum( (B (3*k-2:3*k,cols)-... 69 | Bold(3*k-2:3*k,cols)) .^ 2 )); 70 | end 71 | %disp(sprintf('change=%f, diff_cols=%f, diff_rows=%f', ... 72 | % change, diff_cols, diff_rows)); 73 | iteration = iteration +1; 74 | if opt.verbose, fprintf(1,'.'); end 75 | end 76 | 77 | 78 | if opt.verbose 79 | if opt.info_separately, disp(['(' num2str(toc) ' sec)']); 80 | else, fprintf(1,[num2str(toc) ' sec)']); end; end 81 | 82 | %disp(sprintf('change: %f, diff_rows: %f, diff_cols: %f, iterations: %d', ... 83 | % change, diff_rows, diff_cols, iteration)); 84 | -------------------------------------------------------------------------------- /strawlab/test-data/caldata20130726_122220/IdMat.dat: -------------------------------------------------------------------------------- 1 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 2 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 3 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 1 1 1 1 1 1 1 1 0 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 4 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 5 | -------------------------------------------------------------------------------- /MultiCamSelfCal/OutputFunctions/savecalpar.m: -------------------------------------------------------------------------------- 1 | % SaveCalPar save calibration parameters in different formats 2 | % 3 | % [Cst,Rot] = savecalpar(P,config) 4 | % P ... 3*CAM x 4 matrix containing result of selfcalibration, see EUCLIDIZE 5 | % config ... configuration structure, see CONFIGDATA 6 | % 7 | % 8 | % Cst ... CAMSx3 matrix containing the camera centers (in world coord.) 9 | % Rot ... 3*CAMSx3 matrix containing camera rotation matrices 10 | 11 | % $Author: svoboda $ 12 | % $Revision: 2.0 $ 13 | % $Id: savecalpar.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $ 14 | % $State: Exp $ 15 | 16 | 17 | function [Cst,Rot] = savecalpar(P,config) 18 | 19 | Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0; 20 | 21 | idxused = config.cal.cams2use; 22 | 23 | CAMS = size(P,1)/3; 24 | 25 | Cst = zeros(CAMS,3); 26 | Pst = zeros(3*CAMS,3); 27 | Rot = []; 28 | for i=1:CAMS, 29 | % do not save P matrices in separate files 30 | if 1 31 | Pmat = P(i*3-2:i*3,:); 32 | if Octave 33 | save(sprintf(config.files.CalPmat,idxused(i)),'Pmat'); 34 | else 35 | save(sprintf(config.files.CalPmat,idxused(i)),'Pmat','-ASCII'); 36 | end 37 | end 38 | sc = norm(P(i*3,1:3)); 39 | % first normalize the Projection matrices to get normalized pixel points 40 | P(i*3-2:i*3,:) = P(i*3-2:i*3,:)./sc; 41 | % decompose the matrix by using rq decomposition 42 | [K,R] = rq(P(i*3-2:i*3,1:3)); 43 | tvec= inv(K)*P(i*3-2:i*3,4); % translation vector 44 | C = -R'*tvec; % camera center 45 | % Stephi calib params 46 | Pstephi = R'*inv(K); 47 | Pst(i*3-2:i*3,:) = Pstephi; 48 | Cst(i,:) = C'; 49 | % Stephi requires to save the pars in more "wordy" form 50 | fid = fopen(sprintf(config.files.StCalPar,idxused(i)),'wt'); 51 | if ~fid 52 | error('SaveCalPar: The camera cal file cannot be opened'); 53 | else 54 | fprintf(fid,'C1 = %f \n', C(1)); 55 | fprintf(fid,'C2 = %f \n', C(2)); 56 | fprintf(fid,'C3 = %f \n', C(3)); 57 | fprintf(fid,'\n'); 58 | fprintf(fid,'P11 = %f \n', Pstephi(1,1)); 59 | fprintf(fid,'P12 = %f \n', Pstephi(1,2)); 60 | fprintf(fid,'P13 = %f \n', Pstephi(1,3)); 61 | fprintf(fid,'P21 = %f \n', Pstephi(2,1)); 62 | fprintf(fid,'P22 = %f \n', Pstephi(2,2)); 63 | fprintf(fid,'P23 = %f \n', Pstephi(2,3)); 64 | fprintf(fid,'P31 = %f \n', Pstephi(3,1)); 65 | fprintf(fid,'P32 = %f \n', Pstephi(3,2)); 66 | fprintf(fid,'P33 = %f \n', Pstephi(3,3)); 67 | fclose(fid); 68 | end 69 | Rot = [Rot;R]; 70 | % Prithwijit requires to save the pars in more "wordy" form 71 | if 0 % do not save in the Prithwijit format 72 | fid = fopen(sprintf(config.files.CalPar,idxused(i)),'wt'); 73 | if ~fid 74 | error('SaveCalPar: The camera cal file cannot be opened'); 75 | else 76 | fprintf(fid,'R11 = %f \n',R(1,1)); 77 | fprintf(fid,'R12 = %f \n',R(1,2)); 78 | fprintf(fid,'R13 = %f \n',R(1,3)); 79 | fprintf(fid,'R21 = %f \n',R(2,1)); 80 | fprintf(fid,'R22 = %f \n',R(2,2)); 81 | fprintf(fid,'R23 = %f \n',R(2,3)); 82 | fprintf(fid,'R31 = %f \n',R(3,1)); 83 | fprintf(fid,'R32 = %f \n',R(3,2)); 84 | fprintf(fid,'R33 = %f \n',R(3,3)); 85 | fprintf(fid,'\n'); 86 | fprintf(fid,'t11 = %f \n',tvec(1)); 87 | fprintf(fid,'t21 = %f \n',tvec(2)); 88 | fprintf(fid,'t31 = %f \n',tvec(3)); 89 | fprintf(fid,'\n'); 90 | fprintf(fid,'K11 = %f \n',K(1,1)); 91 | fprintf(fid,'K12 = %f \n',K(1,2)); 92 | fprintf(fid,'K13 = %f \n',K(1,3)); 93 | fprintf(fid,'K21 = %f \n',K(2,1)); 94 | fprintf(fid,'K22 = %f \n',K(2,2)); 95 | fprintf(fid,'K23 = %f \n',K(2,3)); 96 | fprintf(fid,'K31 = %f \n',K(3,1)); 97 | fprintf(fid,'K32 = %f \n',K(3,2)); 98 | fprintf(fid,'K33 = %f \n',K(3,3)); 99 | fprintf(fid,'\n'); 100 | fclose(fid); 101 | end 102 | end 103 | end 104 | 105 | % save Stehpi params 106 | if Octave 107 | save(config.files.Pst,'Pst'); 108 | save(config.files.Cst,'Cst'); 109 | else 110 | save(config.files.Pst,'Pst','-ASCII'); 111 | save(config.files.Cst,'Cst','-ASCII'); 112 | end 113 | -------------------------------------------------------------------------------- /CommonCfgAndIO/loaddata.m: -------------------------------------------------------------------------------- 1 | % loaddata ... load the input data 2 | % 3 | % loaded = loaddata(config) 4 | % 5 | % config ... configuration structure, see the CONFIGDATA 6 | % 7 | % M cameras and N frames 8 | % loaded.Ws ... 3M x N joint image matrix 9 | % .IdMat ... M x N point identification matrix 10 | % .Res ... M x 2 image resolutions 11 | % .Pmat ... {1xM} cell array of the projection matrices 12 | % 13 | % see the FindingPoint and MulticamSelfCalib for more details 14 | % 15 | % $Id: loaddata.m,v 2.2 2005/05/23 16:23:35 svoboda Exp $ 16 | 17 | function loaded = loaddata(config) 18 | 19 | USED_MULTIPROC = 0; % was the multipropcessing used? 20 | % if yes then multiple IdMat.dat and points.dat have to be loaded 21 | % setting to 1 it forces to read the multiprocessor data against the 22 | % monoprocessor see the IM2POINTS, IM2PMULTIPROC.PL 23 | 24 | %%% 25 | % read the data structures 26 | if ~USED_MULTIPROC 27 | try, 28 | Ws = load(config.files.points); % distorted points as found by Im2Points 29 | IdMat = load(config.files.IdMat); % see function im2points for detailed comments 30 | %%% 31 | % try,load the file with Images resolutions which is on of the output files 32 | % from finding LEDs procedure or take the pre-defined resolutions specified in the configdata 33 | try, Res = load(config.files.Res); catch, Res = repmat(config.imgs.res,size(IdMat,1),1); end 34 | 35 | resample_factor = config.cal.USE_NTH_FRAME; 36 | if resample_factor ~= 1 37 | start_i = 1; 38 | Ws = Ws(1:end,start_i:resample_factor:end); 39 | IdMat = IdMat(1:end,start_i:resample_factor:end); 40 | end 41 | catch 42 | warning('Data from mono-processor version not found, trying the multi-proc ones ...') 43 | USED_MULTIPROC=1; 44 | end 45 | end 46 | 47 | if USED_MULTIPROC 48 | pointsfiles = dir([config.files.points,'.*']); 49 | IdMatfiles = dir([config.files.IdMat,'.*']); 50 | Resfiles = dir([config.files.Res,'.*']); 51 | pp = []; 52 | for i=1:size(pointsfiles,1), 53 | W{i} = load([config.paths.data,pointsfiles(i).name],'-ASCII'); 54 | pp = [pp,size(W{i},2)]; 55 | IdM{i} = load([config.paths.data,IdMatfiles(i).name],'-ASCII'); 56 | try, Rs{i} = load([config.paths.data,Resfiles(i).name],'-ASCII'); catch, Rs{i} = repmat(config.imgs.res,size(IdM{i},1),1); end 57 | end 58 | % throw away some point to preserve consistency 59 | [minpp,minidx] = min(pp); 60 | if minpp == 0 61 | error('loaddata: Problem in loading input data. Check CONFIGDATA, EXPNAME settings'); 62 | end 63 | % merge the matrices 64 | Ws = []; 65 | IdMat = []; 66 | Res = []; 67 | for i=1:size(pointsfiles,1), 68 | Ws = [Ws; W{i}(:,1:minpp)]; 69 | IdMat = [IdMat; IdM{i}(:,1:minpp)]; 70 | Res = [Res; Rs{i}]; 71 | end 72 | if isempty(Ws) | isempty(IdMat) 73 | error('Error in loading parallel data. Did you really use multi-processor version of finding points'); 74 | end 75 | end 76 | 77 | % oscar data hack. The projectors are handled as the cameras 78 | try,config.files.idxcams = [config.files.idxcams,config.files.idxproj]; catch, config.files.idxcams; end 79 | 80 | if size(Ws,1)/3 ~= size(config.files.idxcams,2) 81 | error('Problem in loading points data. Less or more data than expected found') 82 | end 83 | 84 | 85 | %%% read the P matrices 86 | count = 1; 87 | for i=config.cal.cams2use, 88 | pmat_fname = sprintf(config.files.CalPmat,i); 89 | if exist(pmat_fname) 90 | P = load(pmat_fname,'-ascii'); 91 | Pmat{count} = P; 92 | count = count+1; 93 | end 94 | end 95 | 96 | idx2use = zeros(size(config.cal.cams2use)); 97 | for i=1:size(config.cal.cams2use,2), 98 | idx2use(i) = find(config.cal.cams2use(i) == config.files.idxcams); 99 | end 100 | 101 | idx2use3 = []; 102 | for i=1:size(idx2use,2), 103 | idx2use3 = [idx2use3, [idx2use(i)*3-2:idx2use(i)*3]]; 104 | end 105 | 106 | %%% 107 | % fill the loaded structure 108 | loaded.Ws = Ws(idx2use3,:); 109 | loaded.IdMat = IdMat(idx2use,:); 110 | loaded.Res = Res(idx2use,:); 111 | 112 | try,loaded.Pmat = Pmat; catch, warning('No Pmat available'); end; 113 | 114 | return; 115 | -------------------------------------------------------------------------------- /MultiCamValidation/CoreFunctions/estimateX.m: -------------------------------------------------------------------------------- 1 | % estimateX ... estimate 3D points robustly 2 | % 3 | % reconstructed = estimateX(loaded,IdMat,cam) 4 | % 5 | % data ... data structure, see LOADDATA 6 | % IdMat ... current point identification matrix 7 | % cam ... array of camera structures, see the main script GO 8 | % 9 | % reconstructed.ptdIdx ... indexes->data of points used for the reconstruction 10 | % .X ... reconstructed points, see u2PX 11 | % .CamIdx ... indexes->data of cameras used for the reconstruction 12 | % 13 | % $Id: estimateX.m,v 2.0 2003/06/19 12:07:09 svoboda Exp $ 14 | 15 | function reconstructed = estimateX(data,IdMat,cam,config) 16 | 17 | SS = config.cal.NTUPLES; % sample size 18 | MIN_POINTS = config.cal.MIN_PTS_VAL; % minimal number of correnspondences in the sample 19 | 20 | Ws = data.Ws; 21 | Pmat = data.Pmat; 22 | 23 | CAMS = size(IdMat,1); 24 | FRAMES = size(IdMat,2); 25 | 26 | % create indexes for all possible SS-tuples 27 | if 1 28 | count = 1; 29 | if SS == 2; 30 | for i=1:CAMS, 31 | for j=(i+1):CAMS, 32 | sample(count).CamIds = [i,j]; 33 | count = count+1; 34 | end 35 | end 36 | end 37 | if SS == 3; 38 | for i=1:CAMS, 39 | for j=(i+1):CAMS, 40 | for k=(j+1):CAMS, 41 | sample(count).CamIds = [i,j,k]; 42 | count = count+1; 43 | end 44 | end 45 | end 46 | end 47 | if SS == 4; 48 | for i=1:CAMS, 49 | for j=(i+1):CAMS, 50 | for k=(j+1):CAMS, 51 | for l=(k+1):CAMS, 52 | sample(count).CamIds = [i,j,k,l]; 53 | count = count+1; 54 | end 55 | end 56 | end 57 | end 58 | end 59 | if SS == 5; 60 | for i=1:CAMS, 61 | for j=(i+1):CAMS, 62 | for k=(j+1):CAMS, 63 | for l=(k+1):CAMS, 64 | for m=(l+1):CAMS, 65 | sample(count).CamIds = [i,j,k,l,m]; 66 | count = count+1; 67 | end 68 | end 69 | end 70 | end 71 | end 72 | end 73 | else 74 | sample(1).CamIds = [1:15]; 75 | SS = size(sample(1).CamIds,2); 76 | end 77 | 78 | disp(sprintf('Computing recontruction from all %d camera %d-tuples',size(sample,2), SS)); 79 | 80 | % create triple indexes 81 | for i=1:CAMS, 82 | tripleIdx{i} = [i*3-2:i*3]; 83 | end 84 | 85 | %%% 86 | % for all possible combination of SS-tuples of cameras 87 | % do the linear 3D reconctruction if enough point avaialable 88 | for i=1:size(sample,2), 89 | ptsIdx = find(sum(IdMat([sample(i).CamIds],:))==SS); 90 | if size(ptsIdx,2) > MIN_POINTS 91 | X = uP2X(Ws([tripleIdx{[sample(i).CamIds]}],ptsIdx), [Pmat{[sample(i).CamIds]}]); 92 | % compute the reprojections 93 | for j=1:CAMS, 94 | xe = Pmat{j}*X; 95 | cam(j).xe = xe./repmat(xe(3,:),3,1); 96 | % these points were the input into Martinec and Pajdla filling 97 | mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far 98 | mask.vis = zeros(1,FRAMES); % maks of visible points 99 | mask.rec(ptsIdx) = 1; 100 | mask.vis(cam(j).ptsLoaded) = 1; 101 | mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera 102 | unmask.rec = cumsum(mask.rec); 103 | unmask.vis = cumsum(mask.vis); 104 | cam(j).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); 105 | cam(j).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); 106 | cam(j).err2d = sum([cam(j).xe(1:2,cam(j).recandvis) - cam(j).xgt(1:2,cam(j).visandrec)].^2); 107 | cam(j).mean2Derr = mean(cam(j).err2d); 108 | cam(j).std2Derr = std(cam(j).err2d); 109 | end 110 | sample(i).mean2Derrs = [cam(:).mean2Derr]; 111 | sample(i).std2Derrs = sum([cam(:).std2Derr]); 112 | sample(i).mean2Derr = sum(sample(i).mean2Derrs); 113 | else 114 | sample(i).mean2Derr = 9e99; 115 | sample(i).std2Derrs = 9e99; 116 | end 117 | end 118 | 119 | % find the best sample 120 | [buff,idxBest] = min([sample(:).mean2Derr]+[sample(:).std2Derrs]); 121 | 122 | % and recompute it the best values 123 | reconstructed.ptsIdx = find(sum(IdMat(sample(idxBest).CamIds,:))==SS); 124 | reconstructed.X = uP2X(Ws([tripleIdx{[sample(idxBest).CamIds]}],reconstructed.ptsIdx), [Pmat{[sample(idxBest).CamIds]}]); 125 | reconstructed.CamIdx = sample(idxBest).CamIds; 126 | 127 | return; 128 | 129 | -------------------------------------------------------------------------------- /MultiCamSelfCal/MartinecPajdla/fill_mm/approximate.m: -------------------------------------------------------------------------------- 1 | %approximate Compute r-rank approximation of MM using null-space. 2 | % 3 | % The approximated matrix is P*X. 4 | 5 | function [P,X, u1,u2] = approximate(M, r, P, opt) 6 | 7 | [m n] = size(M); 8 | rows = find(mean(abs(P')) > opt.tol); %find(sum(P'))'; 9 | nonrows = setdiff(1:m, rows); 10 | 11 | if opt.verbose, fprintf(1,'Immersing columns of MM into the basis...'); tic;end 12 | [Mapp, noncols, X] = approx_matrix(M(rows,:), P(rows,:),r, opt); 13 | if opt.verbose, disp(['(' num2str(toc) ' sec)']); end 14 | 15 | % The rows of NULLSPACE now contain all the nullspaces of the crossproduct 16 | % spaces. Use the nullspace to approximate M, taking r least principal 17 | % components. 18 | cols = setdiffJ(1:n, noncols); 19 | if length(cols) < r; % it will not be able to extend the matrix correctly 20 | u1 = 1:m; u2 = 1:n; 21 | else 22 | if isempty(nonrows), u1 = []; r1 = 1:m; else 23 | if opt.verbose, 24 | disp('Filling rows of MM which have not been computed yet...'); end 25 | [Mapp1, u1, Pnonrows] = extend_matrix(M(:,cols), Mapp(:,cols), ... 26 | X(:,cols), rows, nonrows, r,opt.tol); 27 | Mapp = []; Mapp(:,cols) = Mapp1; 28 | if length(u1) < length(nonrows), P(setdiff(nonrows,u1),:) = Pnonrows; end 29 | r1 = union(rows, setdiff(nonrows,u1)); P = P(r1,:); 30 | end 31 | if isempty(noncols), u2 = []; else 32 | if opt.verbose 33 | disp('Filling columns of MM which have not been computed yet...'); end 34 | [Mapp_tr, u2, Xnoncols_tr] = extend_matrix(M(r1,:)', Mapp(r1,cols)', P',... 35 | cols, noncols, r, opt.tol); 36 | Mapp2 = Mapp_tr'; Xnoncols = Xnoncols_tr'; 37 | if length(u2) < length(noncols), X(:,setdiff(noncols,u2)) = Xnoncols; end 38 | X = X(:, union(cols, setdiff(noncols, u2))); 39 | if sum(~sum(X)), error('nullspace problem 1'); end 40 | end 41 | % These extensions are necessary because the nullspace might not allow us to 42 | % compute every row of the approximating rank r linear space, and then this 43 | % linear space might not allow us to fill in some columns, if they are 44 | % missing too much data. 45 | end 46 | if sum(~sum(X)), error('nullspace problem 2'); end 47 | 48 | 49 | %approx_matrix Immerse columns of MM into the basis. 50 | 51 | function [Mapp, misscols, X] = approx_matrix(M, P, r, opt) 52 | 53 | [m n] = size(M); misscols = []; 54 | Mapp(m, n) = NaN; X(r,n) = NaN; % memory allocation 55 | 56 | if ~isempty(P) 57 | % P has r columns, which span the r-D space that gives the best linear 58 | % surface to approximate M. 59 | for j = 1:n 60 | rows = find(~isnan(M(:,j))); 61 | if rank(P(rows,:), opt.tol) == r 62 | X(:,j) = P(rows,:)\M(rows,j); Mapp(:,j) = P*X(:,j); 63 | else 64 | misscols = [misscols, j]; 65 | end 66 | end 67 | end 68 | 69 | 70 | %extend_matrix Fill rows of MM which have not been computed yet. 71 | % 72 | % [E, unrecovered] = extend_matrix(M,INC,subM, rows, nonrows, r) 73 | % 74 | % Parameters: 75 | % rows ... rows of M which have been used to find the solution 76 | % subM ... is a fit to just these rows 77 | % nonrows ... indicate rows of that still need to be fit 78 | % 79 | % Output parameters: 80 | % Pnonrows ... in terms of unrecovered rows 81 | 82 | function [E, unrecovered, Pnonrows] = extend_matrix(M, subM, X, rows, ... 83 | nonrows, r, tol) 84 | 85 | E(size(M,1),size(M,2)) = NaN; 86 | E(rows,:) = subM; unrecovered = []; row = 0; Pnonrows = []; 87 | for i = nonrows 88 | cols = find(~isnan(M(i,:))); 89 | if rank(X(:,cols)) == r 90 | if max(abs(M(i,cols)/X(:,cols) * X)) > 1/tol 91 | unrecovered = [unrecovered i]; 92 | else, row = row + 1; 93 | Pnonrows(row,:) = M(i,cols)/X(:,cols); E(i,:) = Pnonrows(row,:)*X; 94 | end 95 | else 96 | unrecovered = [unrecovered i]; 97 | end 98 | end 99 | 100 | 101 | function c = setdiffJ(a,b) %J as Jacobs so that matlab's setdiff isn't damaged 102 | c = []; 103 | for i = a 104 | if ~member(i,b) 105 | c = [c,i]; 106 | end 107 | end 108 | 109 | 110 | %member(e,s) Return 1 if element e is a member of set s. Otherwise return 0. 111 | 112 | function c = member(e,s) 113 | c = 0; 114 | for i = s 115 | if i == e 116 | c = 1; 117 | break 118 | end 119 | end 120 | --------------------------------------------------------------------------------